Losing pose on EV3

[wheelebigger]

Hey, i have this code which should make the rover go to a waypoint, but if it encounters an obstacle on the way, it should "dodge" said object.
Currently it can only do that by turning left, and then measuring on a second sensor, to determine when it's free of the object, so it can go to its waypoint again.
My problem is, after using the second sensor the rover loses it's pose, so if i call the waypoint(fx,fy) after using more than 1 sensor, the pose has been reset, or something along those lines. Bascially it looks like the rover decides that the original 0,0 of the waypoint is whereever it is when it doesn't measure on the second sensor.
It is really super frustrating, and i feel like i have tried any and everything. I have multiple versions of my code, but this is the most recent one;

Code: Select all

package testies;

import java.io.BufferedWriter;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileWriter;
import java.io.IOException;
import java.io.PrintWriter;
import java.math.*;
import java.nio.file.Files;
import java.nio.file.Paths;
import java.nio.file.StandardOpenOption;
import java.util.Date;
import java.sql.Timestamp;
import lejos.hardware.ev3.LocalEV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.motor.EV3MediumRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.hardware.sensor.HiTechnicCompass;
import lejos.hardware.sensor.SensorMode;
import lejos.hardware.sensor.SensorModes;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.chassis.*;
import lejos.robotics.geometry.Point;
import lejos.robotics.navigation.*;
import lejos.robotics.localization.*;
import lejos.robotics.localization.CompassPoseProvider.*;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;
import lejos.hardware.Button;
import com.sun.corba.se.impl.oa.poa.ActiveObjectMap.Key;

import jdk.jfr.events.FileWriteEvent;

public class stateofthemachine
{
   //Init
   static RegulatedMotor leftm = new EV3LargeRegulatedMotor(MotorPort.A); //motor 1
   static RegulatedMotor rightm = new EV3LargeRegulatedMotor(MotorPort.B); //motor 2
   
   static Wheel wheelLeft = WheeledChassis.modelWheel(leftm,4.32).offset(-6.35);
   static Wheel wheelRight = WheeledChassis.modelWheel(rightm,4.32).offset(6.35);
   static Chassis chassis = new WheeledChassis(new Wheel[]{wheelLeft, wheelRight},WheeledChassis.TYPE_DIFFERENTIAL);
   static MovePilot Bronchio = new MovePilot(chassis);
   static OdometryPoseProvider Poseo = new OdometryPoseProvider(Bronchio);
   static Navigator naviBot = new Navigator(Bronchio, Poseo);
   
   
   static Port port2 = LocalEV3.get().getPort("S2"); //ultrasonic
   static SensorModes sens1UltraFront = new EV3UltrasonicSensor(port2);
   
   static Port port3 = LocalEV3.get().getPort("S3"); //ultrasonic
   static SensorModes sens2UltraSide = new EV3UltrasonicSensor(port3);
   
   static SampleProvider distancefront = sens1UltraFront.getMode("Distance");
   static float[] sampledistfront = new float[distancefront.sampleSize()];
   
   static SampleProvider distanceside = sens2UltraSide.getMode("Distance");
   static float[] sampledistside = new float[distanceside.sampleSize()];
   
   static Pose ThePoseLol;
   
   
   static int fx = 200;
   static int fy = 0;

   
   static int state = 0;
      
   static Point polarHeading(int heading, Pose thePose)
   {
      float fucklort = Float.parseFloat(String.valueOf(Math.toRadians(thePose.getHeading()+heading)));
      float x = Float.parseFloat(String.valueOf(Math.cos(fucklort)));
      float y = Float.parseFloat(String.valueOf(Math.sin(fucklort)));
      Point polarPoint = new Point(x,y);
      return polarPoint;
   }
      
   static PrintWriter logWrite()
   {
      Date date = new Date();
      try(   FileWriter fileWriter = new FileWriter("File.txt", true);
            BufferedWriter bufferedWriter = new BufferedWriter(fileWriter);
            PrintWriter printWriter = new PrintWriter(bufferedWriter);   )
      {
         printWriter.println(new Timestamp(date.getTime()));
         printWriter.println("State: " + state);
         printWriter.println("Pose: " + ThePoseLol.getX() + " " + ThePoseLol.getY() + " " + ThePoseLol.getHeading());

         
      } catch (IOException e)
      {
         e.printStackTrace();
      }
         
         return null;
      
      //The example below creates a new textfile every time
      /*try
      {
         File file = new File ("/home/lejos/programs/text.txt");
          PrintWriter printWriter = new PrintWriter (file);
          printWriter.println (state);
          printWriter.close ();
      }
      catch (IOException e)
      {
         e.printStackTrace();
         // TODO: handle exception
      }
         return null;*/
          
   }
   
   public static void main (String[] args) throws InterruptedException
   {
      
      
      while(true)
      {   
         
         switch (state)
         {
            case 0: LCD.clear(); LCD.drawString("case 0", 1, 1);
         
               naviBot.addWaypoint(fx,fy);
               naviBot.followPath();
            
               distancefront.fetchSample(sampledistfront,0);
               distanceside.fetchSample(sampledistside,0);
               
               if(sampledistfront[0]<0.2)
               {
                  naviBot.clearPath();
                  state = 1;
               }
               
               if(sampledistside[0]<0.3)
               {
                  naviBot.clearPath();
                  state = 2;
               }
               
               break;
               
            case 1: LCD.clear(); LCD.drawString("case 1", 1, 1);
               Thread.sleep(100);
               Point Left = polarHeading(-90,Poseo.getPose());
               naviBot.addWaypoint(Poseo.getPose().getX()+Left.x,Poseo.getPose().getY()+Left.y);
               naviBot.followPath();
               if(naviBot.waitForStop())
               state = 3;
               
               break;
               
               
            case 2: LCD.clear(); LCD.drawString("case 2", 1, 1);
                  naviBot.clearPath();
                  Thread.sleep(100);
                  naviBot.addWaypoint(Poseo.getPose().getX()+5f,Poseo.getPose().getY(),Poseo.getPose().getHeading());
                  naviBot.followPath();
                  if(naviBot.waitForStop())
                  {
                     distanceside.fetchSample(sampledistside,0);
                     if(sampledistside[0]<0.3)
                     {
                        state = 3;
                     }else if(sampledistside[0]>0.3)
                     {
                        state = 0;
                     }
                     
                  }
            
               break;
               
            case 3: LCD.clear(); LCD.drawString("case 3", 1, 1);
               Thread.sleep(100);
               distancefront.fetchSample(sampledistfront,0);
               distanceside.fetchSample(sampledistside,0);
               
               if(sampledistside[0]<0.3)
               {
                  naviBot.clearPath();
                  state = 2;
               }               
               break;
               
            default:
               
               break;
               
         }
      }
   }
}


Currently we are not using the filewriter thingy.
I really hope anyone can help me solve this, i'm literally ripping my hair out.

[gloomyandy]

It is hard to be sure what may be going on here. It would help if you had logging that tracked your robot state and the current pose so that you can see what is going on and if the current pose is ever being lost. To avoid the log being very large you could only log information if the state changes or if the pose changes. It would also help if you posted a video showing how your robot is moving and what you are expecting to see.

[wheelebigger]

When driving ”correctly” going to the wall, turning, and then following it we get this from the Pose;
State: 0 Pose: 52.477165 0.0 0.0
State: 1 Pose: 61.97734 0.0 0.0
State: 3 Pose: 62.95523 -0.924599 0.0
State: 2 Pose: 62.95523 -0.924599 0.0
State: 3 Pose: 67.95036 -0.924599 0.17007874
State: 2 Pose: 67.95036 -0.924599 0.17007874
State: 3 Pose: 72.907776 -0.9098832 0.0
State: 2 Pose: 72.907776 -0.9098832 0.0
State: 3 Pose: 77.90291 -0.9098832 0.17007874
State: 2 Pose: 77.90291 -0.9098832 0.17007874
State: 3 Pose: 82.86032 -0.8951674 0.0
State: 2 Pose: 82.86032 -0.8951674 0.0
State: 3 Pose: 87.85545 -0.8951674 0.17007874
State: 2 Pose: 87.85545 -0.8951674 0.17007874
State: 3 Pose: 92.83171 -0.88039565 0.17007874
State: 2 Pose: 92.83171 -0.88039565 0.17007874
State: 3 Pose: 97.80797 -0.8656239 0.17007874
State: 2 Pose: 97.80797 -0.8656239 0.17007874
State: 3 Pose: 102.80308 -0.85079616 0.34015748
State: 2 Pose: 102.80308 -0.85079616 0.34015748
State: 3 Pose: 102.80308 -0.85079616 0.34015748
State: 2 Pose: 102.80308 -0.85079616 0.34015748
State: 3 Pose: 107.779274 -0.8212528 0.34015748
State: 2 Pose: 107.779274 -0.8212528 0.34015748
State: 3 Pose: 112.75547 -0.7917095 0.34015748
State: 2 Pose: 112.75547 -0.7917095 0.34015748
As we can see from this, it keeps going out of the X axis, even though when it has turned, it should be going out of the Y-axis (+ or -) so it somehow kind of swaps X and Y, or something along those lines.
As in; after State 1, when it swaps between state 2 and 3, only the Y-axis should really be changing.


Does that sound about right?

[gloomyandy]

Without seeing what the robot is actually doing it is hard to say, you should also be logging the details of what you are telling the robot to do (so logging the details of the waypoints you are adding.

Also why on earth are you converting things to strings and then back to floats in the PolarHeading code? That does not seem like a good idea!

Oh and it is probably not a good idea to use the 3 parameter version of addWaypoint, unless you really care about the orientation of the robot at the end of the move, if you do care about the orientation than using the current heading (before the waypoint move, may not be a good idea).