[NAV] Add DifferentialPilot support

[jabrena]

Feature: Develop the library EV3Dev-lang-java
In the third generation of Lego Mindstorms, 
it is possible to install a complete Linux distribution and a complete Oracle JRE. 
It is necessary to develop a library to use the hardware compatible with EV3Dev.

    Scenario: Add the support for differential pilot
        Given The library EV3Dev-lang-java
        Then It is necessary to be the core library to be used by the lejos-navigation library

Classes: http://www.lejos.org/ev3/docs/lejos/robotics/navigation/package-frame.html

Interfaces:
ArcMoveController
ArcRotateMoveController
LineFollowingMoveController
MoveController
MoveListener
MoveProvider
NavigationListener
RotateMoveController
WaypointListener

Classes:
ArcAlgorithms
Ballbot
CompassPilot
DifferentialPilot
Move
MovePilot
Navigator
OmniPilot
Pose
SteeringPilot
Waypoint

Enums:
Move.MoveType

Exceptions:
DestinationUnreachableException

Examples: https://github.com/eekkelund/LeJos/tree/HEAD/LeJos/src/navirobo https://github.com/rho2/leJOS-Workshop/blob/master/3.%20Pilot/src/Pilot.java https://github.com/tenyoung795/cs424-rlabs/tree/master/rlab2 https://github.com/A-Malone/rob301_labs https://github.com/A-Malone/rob301_labs/tree/master/final_project/src/localization https://github.com/A-Malone/rob301_labs/blob/master/final_project/src/launchers/NavigateToRoad.java https://github.com/NicolasVerbeek/Rapport-jury/blob/master/Programation%20NXT/Algo%20Beta/Main.java

Search: https://github.com/search?p=2&q=import+lejos.robotics.localization.OdometryPoseProvider&type=Code&utf8=%E2%9C%93

For v0.6.0, it is necessary to run the following example without any problem:

package ev3dev.examples;

import ev3dev.actuators.ev3.motors.EV3LargeRegulatedMotor;
import ev3dev.sensors.Battery;
import lejos.hardware.port.MotorPort;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lombok.extern.slf4j.Slf4j;

public @Slf4j class DifferentialPilotTest1 {

    public static void main(final String[] args){

        final double wheelDiameter = 8.2;
        final double trackWidth = 12.6;
        final RegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.A);
        final RegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.B);

        //Special Stop modes from EV3Dev
        leftMotor.brake();
        rightMotor.brake();

        final DifferentialPilot pilot = new DifferentialPilot(
                wheelDiameter,
                trackWidth,
                leftMotor,
                rightMotor);

        //pilot.setAngularAcceleration();
        pilot.setAngularSpeed(100);
        //pilot.setLinearAcceleration();
        pilot.setLinearSpeed(100);
        pilot.travel(20);
        pilot.rotate(90);

        log.info("Voltage: {}", Battery.getInstance().getVoltage());
    }
}

[jabrena]

It is necessary to add the class Motor. http://www.lejos.org/ev3/docs/lejos/hardware/motor/Motor.html

[jabrena]

http://www.legolab.daimi.au.dk/DigitalControl.dir/NXT/Lesson10.dir/Lesson.html

[jabrena]

robot@ev3dev:~$ ps aux
USER       PID %CPU %MEM    VSZ   RSS TTY      STAT START   TIME COMMAND
root     15244  0.0  0.0   1848     0 ?        Ss   18:00   0:00 /bin/sh /usr/lib/apt/apt.systemd.daily update
root     15268 43.6 12.4  55288  7292 ?        R    18:00   6:16 apt-get check -qq

robot@ev3dev:~$ free
             total       used       free     shared    buffers     cached
Mem:         58648      24652      33996          0        984       8800
-/+ buffers/cache:      14868      43780
Swap:        98300      12172      86128

[jabrena]

It is Amazing!!!

the LeJOS classes are running nice with EV3Dev classes.

Example1:

package ev3dev.examples;

import ev3dev.actuators.ev3.motors.EV3LargeRegulatedMotor;
import ev3dev.sensors.Battery;
import lejos.hardware.port.MotorPort;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lombok.extern.slf4j.Slf4j;

public @Slf4j class DifferentialPilotTest1 {

    public static void main(final String[] args){

        final double wheelDiameter = 8.2;
        final double trackWidth = 12.6;
        final RegulatedMotor leftMotor = new EV3LargeRegulatedMotor(MotorPort.A);
        final RegulatedMotor rightMotor = new EV3LargeRegulatedMotor(MotorPort.B);

        //Special Stop modes from EV3Dev
        leftMotor.brake();
        rightMotor.brake();

        final DifferentialPilot pilot = new DifferentialPilot(
                wheelDiameter,
                trackWidth,
                leftMotor,
                rightMotor);

        //pilot.setAngularAcceleration();
        pilot.setAngularSpeed(100);
        //pilot.setLinearAcceleration();
        pilot.setLinearSpeed(100);
        pilot.travel(20);
        pilot.rotate(90);

        log.info("Voltage: {}", Battery.getInstance().getVoltage());
    }
}

[jabrena]

Magic Square:

package ev3dev.examples;

import ev3dev.actuators.ev3.motors.Motor;
import ev3dev.sensors.Battery;
import ev3dev.sensors.Button;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Pose;
import lejos.utility.Delay;
import lombok.extern.slf4j.Slf4j;

public @Slf4j class PilotSquare {

   public static void main(String [] args) throws Exception {

       final double wheelDiameter = 8.2;
       final double trackWidth = 12.6;
       RegulatedMotor leftMotor = Motor.A;
       RegulatedMotor rightMotor = Motor.B;

       //Special Stop modes from EV3Dev
       leftMotor.brake();
       rightMotor.brake();

       DifferentialPilot pilot = new DifferentialPilot(
               wheelDiameter,
               trackWidth,
               leftMotor,
               rightMotor);

       //pilot.setAngularAcceleration();
       pilot.setAngularSpeed(100);
       //pilot.setLinearAcceleration();
       pilot.setLinearSpeed(100);

       OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);

       Pose initialPose = new Pose(0,0,0);
       poseProvider.setPose(initialPose);

       log.info("Pilot square");

       for(int i = 0; i < 4; i++) {
           pilot.travel(20);           
           show(poseProvider.getPose());
           Delay.msDelay(1000);

           pilot.rotate(90);
           show(poseProvider.getPose());
           Delay.msDelay(1000);
       }

       pilot.stop();

       log.info("Program stopped");
       log.info("Voltage: {}", Battery.getInstance().getVoltage());
   }

   private static void show(Pose p) {
        log.info("Pose X " + p.getX());
        log.info("Pose Y " + p.getY());
        log.info("Pose V " + p.getHeading());
   }

}

[jabrena]

Add some example stopping motors: https://stackoverflow.com/questions/264825/get-notification-on-a-java-process-termination

pkill java

[jabrena]

@Waypoints:

package ev3dev.examples;

import ev3dev.actuators.ev3.motors.Motor;
import ev3dev.sensors.Battery;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Waypoint;
import lombok.extern.slf4j.Slf4j;

public @Slf4j class WaypointTest {

    public static void main(String[] args) {

        final double wheelDiameter = 8.2;
        final double trackWidth = 12.6;
        RegulatedMotor leftMotor = Motor.A;
        RegulatedMotor rightMotor = Motor.B;

        //Special Stop modes from EV3Dev
        leftMotor.brake();
        rightMotor.brake();

        DifferentialPilot pilot = new DifferentialPilot(
                wheelDiameter,
                trackWidth,
                leftMotor,
                rightMotor);

        //pilot.setAngularAcceleration();
        pilot.setAngularSpeed(100);
        //pilot.setLinearAcceleration();
        pilot.setLinearSpeed(100);

        Navigator nav = new Navigator(pilot);

        OdometryPoseProvider opp = new OdometryPoseProvider(pilot);

        //some waypoints
        Waypoint finish = new Waypoint(50, 50, 0);

        //TODO: Mutable objects. Speak with Aswin
        float[] points = new float[opp.sampleSize()];
        int count = 0;

/*
        // establish a fail-safe: pressing Escape quits
        Brick brick = BrickFinder.getDefault(); // get specifics about this
        // robot
        brick.getKey("Escape").addKeyListener(new KeyListener() {
            @Override
            public void keyPressed(Key k) {
                pilot.stop();
                us.close();
                eopd.close();
                Motor.A.close();    
                Motor.B.close();
                System.exit(1);
            }

            @Override
            public void keyReleased(Key k) {
                System.exit(1);
            }
        });
        */

        pilot.travel(20);
        //pilot.rotate(45);

        pilot.stop();

        pilot.setLinearSpeed(10);  //sets straight ahead speed
        pilot.setAngularSpeed(50); //sets turning speed
        pilot.addMoveListener(opp);  //adds the listerner to capture x,y,heading values

        nav.goTo(finish);
        //pilot.steer(0);
        while ( !nav.pathCompleted() ){
            opp.fetchSample(points, 0);
            log.info("Count: {}, x: {}, y: {}, t: {}",  count, points[0], points[1], points[2]);
            count++;
        }

        pilot.stop();
        opp.fetchSample(points, 0);
        log.info("Count: {}, x: {}, y: {}, t: {}",  count, points[0], points[1], points[2]);

        log.info("Voltage: {}", Battery.getInstance().getVoltage());

    }

}

[jabrena]

In the next release, it is possible to debug some class/method that now I don´t know in detail but the basic stuff to navigate in a local environment is running nice in combination with EV3Dev and lejos-navigation

Tomorrow, I will continue testing some features about routes.

[jabrena]

https://github.com/Devski/Robole/blob/master/Pozrobot_OCR_Robot/src/pl/robole/projects/pozrobot/ocr/nxj/test/OdometryMoveTest.java https://github.com/davidportilla/robot-lejos/blob/master/src/NavigatorRobot.java https://github.com/los-lejos/sdp-group3/blob/master/Milestone/src/NavigatorTest.java https://github.com/ArtZhu/EV3-demos https://github.com/npiscitello/leJOS-Projects/blob/master/src/testing/NavPractice.java https://github.com/ohadcn/robots/blob/master/rabat/ex2/Back.java https://github.com/ArtZhu/EV3-demos/blob/master/demo4_v1/src/ProfTests/testOfNavAroundWall.java https://github.com/ArtZhu/EV3-demos/blob/master/demo4_v1/src/ProfTests/testAlongWall.java https://github.com/LightMind/embeddedSystems/blob/master/EmbeddedSystems/src/project/GraphBot.java

[jabrena]

pilot.setLinearSpeed(10); //sets straight ahead speed pilot.setAngularSpeed(50); //sets turning speed

[jabrena]

pilot.steer(0); //start moving

[jabrena]

https://github.com/ArtZhu/EV3-demos/blob/master/demo4_v1/src/ProfTests/testAlongWall.java

[jabrena]

http://www.lejos.org/ev3/docs/lejos/robotics/navigation/DifferentialPilot.html

[jabrena]

https://lejosnews.wordpress.com/2015/01/17/lejos-navigation-pilots/

[jabrena]

http://thetechnicgear.com/2014/05/tutorial-using-motors-draw-square-mindstorms-ev3-lejos/

[jabrena]

https://www.google.co.uk/url?sa=t&source=web&rct=j&url=http://www.cs.ox.ac.uk/people/michael.wooldridge/teaching/robotics/lect03.pdf&ved=0ahUKEwixoImH0JTUAhXDJMAKHbD6Dqs4ChAWCFMwDw&usg=AFQjCNHWtZuiyOltv10vOIHKcrRsMhenYA&sig2=YTiES1bZyH_oRCqmi6JtvA

[jabrena]

Good idea, but code obsoleted. http://legolab2012.blogspot.co.uk/2012/05/lesson-9-navigation-and-map-building_08.html?m=1

[jabrena]

http://www.lejos.org/nxt/nxj/tutorial/WheeledVehicles/WheeledVehicles.htm

[jabrena]

https://github.com/search?l=Java&q=DifferentialPilot+Waypoint&type=Code&utf8=%E2%9C%93

[jabrena]

https://lejosnews.wordpress.com/2015/01/17/lejos-navigation/ https://lejosnews.wordpress.com/2015/01/17/lejos-navigation-pilots/ https://lejosnews.wordpress.com/2015/01/17/lejos-navigation-pose-providers/ https://lejosnews.wordpress.com/2015/01/17/lejos-navigation-object-detection/ https://lejosnews.wordpress.com/2015/01/18/lejos-navigation-waypoint-navigator/ https://lejosnews.wordpress.com/2015/01/18/lejos-navigation-mapping/ https://lejosnews.wordpress.com/2015/01/18/lejos-navigation-path-finding/ https://lejosnews.wordpress.com/2015/01/19/lejos-navigation-navigation-model/ https://lejosnews.wordpress.com/2014/04/27/httyr-map-command/

[jabrena]

Problem:

Solution:

package ev3dev.examples.pilot;

import ev3dev.utils.PilotProps;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;

import java.io.IOException;

public class PilotConfig {

    private DifferentialPilot pilot;

    public PilotConfig() throws IOException {

        PilotProps pp = new PilotProps();
        pp.loadPersistentValues();
        float wheelDiameter = Float.parseFloat(pp.getProperty(PilotProps.KEY_WHEELDIAMETER));
        float trackWidth = Float.parseFloat(pp.getProperty(PilotProps.KEY_TRACKWIDTH));
        RegulatedMotor leftMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_LEFTMOTOR));
        RegulatedMotor rightMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_RIGHTMOTOR));
        boolean reverse = Boolean.parseBoolean(pp.getProperty(PilotProps.KEY_REVERSE));

        //Special Stop modes from EV3Dev
        leftMotor.brake();
        rightMotor.brake();

        System.out.println("Any button to start");

        pilot = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor, rightMotor, reverse);
        //pilot.setAngularAcceleration();
        pilot.setAngularSpeed(100);
        //pilot.setLinearAcceleration();
        pilot.setLinearSpeed(100);
    }

    public DifferentialPilot getPilot(){
        return pilot;
    }
}

package ev3dev.examples.pilot;

import ev3dev.actuators.ev3.motors.Motor;
import ev3dev.sensors.Battery;
import ev3dev.sensors.ev3.EV3GyroSensor;
import lejos.hardware.port.SensorPort;
import lejos.robotics.DirectionFinderAdapter;
import lejos.robotics.localization.CompassPoseProvider;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Waypoint;
import lejos.robotics.pathfinding.Path;
import lombok.extern.slf4j.Slf4j;

import java.io.IOException;

public class DifferentialPilotTest8 {

    public static void main(final String[] args) throws IOException {

        PilotConfig pilotConf = new PilotConfig();
        final DifferentialPilot pilot = pilotConf.getPilot();

        OdometryPoseProvider pp = new OdometryPoseProvider(pilot);
        Navigator navigator = new Navigator(pilot, pp);

        navigator.goTo(0,0);
        navigator.goTo(100,0);
        navigator.goTo(50,50);
        navigator.goTo(100,-50);
        navigator.followPath();
        navigator.waitForStop();

        System.out.println(pp.getPose().getHeading());
        System.out.println(pp.getPose().getX());
        System.out.println(pp.getPose().getY());

        System.out.println("Voltage: " + Battery.getInstance().getVoltage());

    }

}

[jabrena]

Amazing: https://lejosnews.wordpress.com/2015/01/17/lejos-navigation-pilots/

package ev3dev.examples.pilot;

import ev3dev.sensors.ev3.EV3UltrasonicSensor;
import lejos.hardware.port.SensorPort;
import lejos.robotics.SampleProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.MoveController;

import java.io.IOException;

public class DifferentialPilotTest10 {

  MoveController pilot;
  EV3UltrasonicSensor us = new EV3UltrasonicSensor(SensorPort.S1);
  SampleProvider bump = us.getDistanceMode();
  float[] sample = new float[1];

  public void go() {
    pilot.forward();
    while (pilot.isMoving()) {
      bump.fetchSample(sample, 0);
      if (sample[0] < 20) pilot.stop();
    }
    float dist = pilot.getMovement().getDistanceTraveled();
    System.out.println("Distance = " + dist);
    pilot.travel(-dist);
  }

  public static void main(String[] args) throws IOException {

    final DifferentialPilotTest10 traveler = new DifferentialPilotTest10();
    final PilotConfig pilotConf = new PilotConfig();
    final DifferentialPilot pilot = pilotConf.getPilot();

    traveler.pilot = pilot;
    traveler.go();
  }
}

[jabrena]

After the testing process, the Following classes was tested:

Interfaces:
ArcMoveController PENDING
ArcRotateMoveController PENDING
LineFollowingMoveController PENDING
MoveController
MoveListener
MoveProvider
NavigationListener
RotateMoveController
WaypointListener

Classes:
ArcAlgorithms PENDING
CompassPilot PENDING
DifferentialPilot
Move
Navigator
Pose
Waypoint

Enums:
Move.MoveType PENDING

Exceptions:
DestinationUnreachableException

I removed from the package MovePilot because the class depends of Chassis and this package was not tested in this iteration. OnmiPilot was tested too.

[jabrena]

Library read to be released in this iteration.