Segoway for EV3

This is where you talk about the EV3 software itself, installation issues, and programming talk.

Moderators: roger, gloomyandy, skoehler

krchilders
New User
Posts: 11
Joined: Tue Mar 18, 2014 10:49 pm

Segoway for EV3

Postby krchilders » Tue Apr 08, 2014 4:12 pm

I have tried for days to get the Segoway class to work on the EV3. I copied it over from the NXT classes and modified it to use GyroscopeAdaptor with a HiTehnicGyro. I also looked at the BallBot code in ev3classes and it looks the same as what I have except that BallBot uses Gyroscope although I have no idea how that works as it is an empty interface in the ev3classes. What I am finding is that the execution time of the balance loop on the EV3 looks to be much slower than on the NXT so the robot just falls over. I also took the GyroBoy code in the Lego education software to Lejos Java. No luck there either.

By the way, does the BallBot class actually work?

Does running off the SD card slow things down? if so is there a plan to at some point actually replace the EV3 firmware similar to the NXT.

For the GyroBoy I used all of the EV3 sensors and motors as per the build instructions.
For the EV3Segoway I used NXT motors and the HiTechnicGyro on a robot very similar to the one in the third addition of Maximum Lego NXT. Only difference is the mounting of the brick.

Am I just trying to get to far ahead of the development of lejos for the EV3?

Sorry there are a lot of questions buried in here. I'm just trying to see if I should stop banging my head against the wall for now and move on to other less tricky EV3 projects. :?

User avatar
gloomyandy
leJOS Team Member
Posts: 5507
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: Segoway for EV3

Postby gloomyandy » Tue Apr 08, 2014 8:04 pm

The Segoway code worked fine for me when I tried it some time ago (using the Lego gyro).
https://www.youtube.com/watch?v=jgc-9GFOKgM
Ignore all of the extra motors and the 3 NXTs. This is basically an EV3 running the Segoway code. All of the rest of the stuff is there to add extra weight to the robot. I was working with some folks at Oracle for a demo at JavaOne and they had a very heavy robot that I was basically simulating by adding every bit of Lego I could find. Without all of that extra weight the robot was very stable.

The control loop was considerably faster than on the NXT. All of this was with an older version of leJOS not with the current version. I have no idea if recent changes have caused issues, but if you are unable to track down what is going wrong then perhaps you should try some simpler projects first?

krchilders
New User
Posts: 11
Joined: Tue Mar 18, 2014 10:49 pm

Re: Segoway for EV3

Postby krchilders » Tue Apr 08, 2014 10:02 pm

Just so I'm clear you used 2 EV3 large motors and the EV3 gyro?

if I may ask how did you implement the gyro interface. The NXT code uses GyroSensor in the call to Segoway and Gyroscope in the Segoway constructor and was based on the HiTechnicGyro? Doesn't seem to me that this is needed for the EV3GyroSensor. According to the Wiki, lejos 0.5.0 took away a lot of the sensor backward compatibility so maybe you used an earlier version.

Also, in order to use the EV3 motors as unregulated I am using "EncoderMotor left = new NXTMotor(MotorPort.B);" per the Wiki.

I have begun to work on simpler projects while this gets sorted out.

Thx!

User avatar
gloomyandy
leJOS Team Member
Posts: 5507
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: Segoway for EV3

Postby gloomyandy » Tue Apr 08, 2014 10:35 pm

Yes two large motors and the EV3 gyro. As I said I used an older version of leJOS. But it really is not that hard to convert the code to use the new sensor interfaces (after all it is only a single value being read). Yes you need to use an unregulated motor, but again it is pretty simple to convert the code.

krchilders
New User
Posts: 11
Joined: Tue Mar 18, 2014 10:49 pm

Re: Segoway for EV3

Postby krchilders » Sat Apr 12, 2014 1:02 am

I finally got it to work on the EV3. I had not implemented the calls to the sample provider for the gyro very efficiently. I found that the K factors in the original Segoway code were not very stable. When I switched to the ones that are used in the Lego Education software Gyro Boy it was very stable. Thanks for the help. :)

hitvq
New User
Posts: 1
Joined: Sat Aug 29, 2015 2:14 am

Re: Segoway for EV3

Postby hitvq » Sun Aug 30, 2015 11:15 pm

Hi all, i'm trying to adapt the Segoway code into a ballbot with 3 motors.
There's one thing i don't get though in the segoway code: the method updateMotorData.

As I can see that you guys have worked on this, would you mind explaining me the way this method works?
And how would you adapt it to consider a 2D movement?

Thanks in advance!

p4co86
New User
Posts: 1
Joined: Fri Oct 23, 2015 11:52 am

Re: Segoway for EV3

Postby p4co86 » Fri Oct 23, 2015 11:55 am

Hi All ! I Try to get the gyroboy working for me, i use the education version.

in eclipse i have this programm, but it is not balancing itself. could someone please help ?

This is my Code!

Code: Select all

import java.io.File;
import java.io.FileNotFoundException;
import java.io.PrintWriter;

import lejos.hardware.Button;
import lejos.hardware.Sound;
import lejos.hardware.motor.UnregulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3GyroSensor;
import lejos.robotics.EncoderMotor;
import lejos.utility.Stopwatch;

public class GyroBoy
{
//   private static final boolean DEBUG = true;
   private static final boolean DEBUG = false;

   EV3GyroSensor gyro = new EV3GyroSensor(SensorPort.S2);
   EncoderMotor leftMotor = new UnregulatedMotor(MotorPort.A);
   EncoderMotor rightMotor = new UnregulatedMotor(MotorPort.D);
   File logfile = new File("/home/root/gyroboy.log");

   private static final float Kp = 0.5f;  // default 0.5f
   private static final float Ki = 11;   // default 11
   private static final float Kd = 0.005f; // default 0.005f

   private static final float gain_angular_velocity = 1.3f; // for theta_hat
   private static final float gain_angle = 25;      // for theta
   private static final float gain_motor_speed = 75;   // for y_hat, default 75
   private static final float gain_motor_position = 350;   // for y, default 350

   private static final int max_iter = 50000; // 50000 for sleep 7 seconds
   private static final int drive_sleep = 7000; // milliseconds, default = 7000
   private static final int STEERING = 20;  // 30 bad, default 20

   //boolean sound = false;
   boolean sound = true;

   float refpos = 0;   // reference position
   private static final int sample_time = 20;   // 15/20/25/30 good, 40 bad, sample time in milliseconds (ms), default 20
   private static final float dt = sample_time/1000f;   // verified, default 0.02
   float speed = 0;
   private static final int wheel_diameter = 55; // in millimeters (mm), default 55
   private static final float radius = wheel_diameter / 2000f; // verified, default 0.0275

   private static final int max_index = 7;
   float[] enc_val = new float[max_index];
   int enc_index = 0;

   boolean nowOutOfBound = false;
   boolean prevOutOfBound = false;
   int outOfBoundCount = 0;
   //      int outOfBound = 0;

   float ang = 0, mean_ang = 0;
   float mean = 0;
   int steering = 0;
   float old_steering = 0;
//   int max_acceleration = 0;

   private static final float radius_const = 57.3f;

   float acc_err = 0, prev_err = 0;

   boolean complete = false;

   Stopwatch stopwatch = new Stopwatch();
   Thread balancer = new Thread (new Balancer());
   Thread driver = new Thread (new Driver());
   PrintWriter writer;
   
   public GyroBoy() {
      try {
         writer = new PrintWriter(logfile);
      } catch (FileNotFoundException e) {
         System.out.println("Can't find " + logfile);
      }
   }
   
   public static void main(String[] args) {
      GyroBoy gboy = new GyroBoy();
      gboy.start();
   }
   
   // verified
   public void start() {
      // verified
      initialize ();

      // verified
      balancer.start ();
      // verified
      driver.start ();

      try {
         balancer.join ();
         driver.join ();
      } catch (InterruptedException e) {
         e.printStackTrace();
      }
      
      Button.ESCAPE.waitForPressAndRelease();
   }

   // verified
   // controls speed and steering
   class Driver implements Runnable {
      public void run() {
         System.out.println ("driving ...");
         writer.println ("driving ...");

         //         while (true) {
         while (!complete) {
            speed = 0; steering = 0;
            sleep (drive_sleep);
            speed = 0; steering = STEERING;
            sleep (drive_sleep);
            speed = 0; steering = -1*STEERING;
            sleep (drive_sleep);
         }
      }
   }

   // verified
   class Balancer implements Runnable{
      public void run() {
         writer.println("refpos = " + refpos + ", dt = " + dt + ", Kp = " + Kp + ", Ki = " + Ki + ", Kd = " + Kd);
         String header = ("iter\tspeed\tang_vel\tang"
               + "\tsensor\tavg_pwr\toffset\trefpos"
               + "\tmspeed"
               + "\tpowerA\tpowerD\textra\tpwr_b\tpwr_c"
               //            + "\tcurr_err\tacc_err\tdif_err\tprev_err"
               );
         writer.println (header);

         Stopwatch totalwatch = new Stopwatch();
         Stopwatch functionwatch = new Stopwatch();
         int iter = 0;
         float motor_speed = 0, motor_position = 0, ang_vel = 0, sensor_values = 0, avg_pwr = 0;
         stopwatch.reset();

//         while (++iter < max_iter && !complete) {
         while (!Button.ESCAPE.isDown() || (++iter < max_iter && !complete)) {
            // Position: verified
            refpos = refpos + (dt * speed * 0.002f);

            // ReadEncoders: verified
            //functionwatch.Restart();
            motor_speed = (radius * getMotorSpeed ()) / radius_const;
            motor_position = (radius * (leftMotor.getTachoCount() + rightMotor.getTachoCount())/ 2.0f) / radius_const;
            //Console.Write(functionwatch.ElapsedMilliseconds + "ms ");

            // ReadGyro: verified
            //functionwatch.Restart();
            ang_vel = readGyro();
            //Console.Write(functionwatch.ElapsedMilliseconds + "ms ");

            // CombineSensorValues: verified
            //functionwatch.Restart();
            sensor_values = combineSensorValues (ang_vel, motor_position, motor_speed);
            //Console.Write(functionwatch.ElapsedMilliseconds + "ms ");

            // PID: verified
            // input: sensor values
            // output: average power
            //functionwatch.Restart();
            avg_pwr = pid (sensor_values);
            //            float avg_pwr = pid (sensor_values, curr_err, acc_err, dif_err, prev_err);
            //Console.Write(functionwatch.ElapsedMilliseconds + "ms ");

            // Errors: verified
            // input: PID output
            //functionwatch.Restart();
            errors (avg_pwr);
            //Console.Write(functionwatch.ElapsedMilliseconds + "ms ");

            if(DEBUG) {
               writer.printf ("%d\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n",
                     iter, speed, ang_vel, ang
                     , sensor_values, avg_pwr, (motor_position - refpos)
                     , refpos, motor_speed
                     );
            }
            
            // SetMotorPower: verified
            // input: pid output
            //functionwatch.Restart();
            setMotorPower(avg_pwr);
            //System.out.println(functionwatch.ElapsedMilliseconds + "ms");

            // Wait: verified
            // Timer >= dt, elapsedTime
//            int elapsedTime = stopwatch.elapsed(); //
//            System.out.println(elapsedTime + " " + totalwatch.elapsed());
//            if(elapsedTime < (int)(dt * 1000f))
//               sleep((int)(dt * 1000f) - elapsedTime);
            while(stopwatch.elapsed() < dt*1000) {
               sleep(1);
            }
            stopwatch.reset();
         }
         int totaltime = totalwatch.elapsed();
         System.out.println("Iteration:" + iter);
         writer.println("Iteration:" + iter);
         System.out.println("TotalTime:" + totaltime + "ms");
         writer.println("TotalTime:" + totaltime + "ms");
         System.out.printf("AvgTime:%.2fms\n", 1f*totaltime/iter);
         writer.printf("AvgTime:%.2fms\n", 1f*totaltime/iter);
         // total time with prints = 4546/100 = 45.46ms
         // total time with some prints = 1916/100 = 19.16ms
         // total time without prints = 432/100 = 4.32ms
         // total time without prints = 559/100 = 5.59ms
         // total time without prints = 898/100 = 8.98ms
         // total time without prints = 532/100 = 5.32ms
         // total time without prints = 1678/500 = 3.36ms
         // total time without prints = 8009/2485 = 3.22ms

         writer.println (header);
         writer.printf ("%d\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n",
               iter, speed, ang_vel, ang
               , sensor_values, avg_pwr, (motor_position - refpos)
               , refpos, motor_speed
            );
         writer.flush();
         
         complete = true;
      }
   }

   // verified
   void initialize() {
      System.out.println ("initializing ...");
      writer.println ("initializing ...");
      //         dt = (sample_time - 2)/1000;

      // erase array
      for (int i = 0; i < max_index; i++) {
         enc_val [i] = 0;
      }

      stopwatch.reset();
      leftMotor.resetTachoCount();
      rightMotor.resetTachoCount();
//      System.out.println("Reset tacho count: " + (stopwatch.elapsed()) + "ms");

      sleep (100);

      // verified
      stopwatch.reset();
      mean = calibrate ();
//      System.out.println(stopwatch.elapsed() + "ms");

      speed = 0;
      steering = 0;

      // reset timer
      stopwatch.reset();
   }

   /**
    * verified
    * average of 20 gyroRate values
    */
   float calibrate() {
      System.out.println ("calibrating ...");
      writer.println ("calibrating ...");

      // Play tone: frequency 440Hz, volume 10
      // duration 0.1sec, play type 0
      if(sound)
         Sound.playTone(440, 100, 10);

      sleep (100);
      mean = 0;

      int count = 20;

      // gyro rate
      for (int i = 0; i < count; i++) {
         mean += gyroRate ();
         //System.out.println ("gyroRate mean: " + mean);
         sleep (5);
      }
      mean = mean / count;

      System.out.printf ("gyroRate:%.2f\n", mean);
      writer.printf ("gyroRate:%.2f\n", mean);

      sleep (1000);
      // Play tone: frequency 440Hz, volume 10
      if(sound)
         Sound.playTone(440, 100, 10);

      sleep (1000);
      // Play tone
      if(sound)
         Sound.playTone(440, 100, 10);

      return mean;
   }

   /**
    * verified
    * average of 5 samples of angular velocity
    */
   float gyroRate() {
      float filter = 0;

      // get 5 samples
      float[] sample = new float[1];
      for(int i = 0; i < 5; i ++) {
         gyro.getRateMode().fetchSample(sample, 0);
         filter += sample[0];
      }
      
      return filter / 5f;
   }

   // verified
   // change ang and return ang_vel
   float readGyro() {
      Stopwatch gyrowatch = new Stopwatch();
      float curr_val = gyroRate ();
      System.out.print("gyro Rate: " + (gyrowatch.elapsed()) + "ms "); // 2ms

      // EMA
      mean = mean * (1f - 0.2f * dt) + (curr_val * 0.2f * dt);
      float ang_vel = curr_val - mean;
      ang = ang + dt * ang_vel;

      // what is this part? in lighter color
      mean_ang = mean_ang * 0.999f + ang * 0.001f;
      ang = ang - mean_ang;

      return ang_vel;
   }

   // verified
   float getMotorSpeed() {
      enc_index++;

      if (max_index <= enc_index)
         enc_index = 0;

      int compare_index = enc_index + 1;
      if (max_index <= compare_index)
         compare_index = 0;

      enc_val[enc_index] = (leftMotor.getTachoCount() + rightMotor.getTachoCount())/2.0f;
      //         System.out.println (enc_val [enc_index] + " " + enc_val[compare_index] + " " + max_index + " " + dt);

      return ((enc_val [enc_index] - enc_val [compare_index]) / (max_index * dt));
   }

   // verified
   public float combineSensorValues(float ang_vel, float motor_position, float motor_speed) {
      return gain_angle * ang
            + gain_angular_velocity * ang_vel
            + gain_motor_position * (motor_position - refpos)
            + gain_motor_speed * motor_speed;
   }

   // verified, but missing prev_err = curr_err
   //      public float pid(float sensor_values, float curr_err, float acc_err, float dif_err, float prev_err) {
   public float pid(float sensor_values) {
      final float ref_val = 0;

      float curr_err = sensor_values - ref_val;
      acc_err += curr_err * dt;
      float dif_err = (curr_err - prev_err) / dt;
      prev_err = curr_err;

      return curr_err * Kp
            + acc_err * Ki
            + dif_err * Kd;
   }

   // verified
   // read the shared variable steering
   public void setMotorPower(float avg_pwr) {
      // limit steering: [-50, 50]
      int new_steering = steering; // always 0 or +/-STEERING
      if (steering > 50)
         new_steering = 50;
      if (steering < -50)
         new_steering = -50;

      float extra_pwr = 0;
      if (new_steering == 0) {
         int sync_0 = 0;

         if (old_steering != 0) { // +/-STEERING
            sync_0 = rightMotor.getTachoCount() - leftMotor.getTachoCount();
         }
         extra_pwr = (rightMotor.getTachoCount () - leftMotor.getTachoCount() - sync_0) * 0.05f;
      } else {
         extra_pwr = new_steering * (-0.5f);
      }

      float pwr_b = avg_pwr + extra_pwr;
      float pwr_c = avg_pwr - extra_pwr;
      old_steering = new_steering;

      float powerA = (pwr_b * 0.021f / radius);
      float powerD = (pwr_c * 0.021f / radius);
      leftMotor.setPower((int)powerA);
      rightMotor.setPower((int)powerD);
      
      //System.out.println (speedA.ToString(FORMAT) + "\t" + speedD.ToString(FORMAT)
      //   + "\t" + extra_pwr.ToString(FORMAT) + "\t" + pwr_b.ToString(FORMAT) + "\t" + pwr_c.ToString(FORMAT));
   }

   // verified except the interrupting balance loop
   public void errors(float avg_pwr) {
      nowOutOfBound = (Math.abs (avg_pwr) > 5000f);
//      nowOutOfBound = (Math.abs (avg_pwr) > 100f);

      // read cur_err

      if (nowOutOfBound && prevOutOfBound) {
         outOfBoundCount++;
      } else {
         outOfBoundCount = 0;
      }

      if (outOfBoundCount > 20) {
         System.out.printf("avg_pwr:%.2f\n", avg_pwr);
         writer.printf("avg_pwr:%.2f\n", avg_pwr);
         /*writer.printf (iter + "\t" + speed + "\t" + ang_vel + "\t" + ang
               + "\t" + sensor_values + "\t" + avg_pwr
               + "\t" + (motor_position - refpos) + "\t" + refpos
               + "\t" + motor_speed + "\t"
            );*/
         
         sleep (100);
         leftMotor.stop();
         rightMotor.stop();
         
         // diplay ERROR
         System.out.println("ERROR");
         writer.println("ERROR");
         
         Sound.playTone(800, 100, 50);
         Sound.playTone(600, 100, 50);
         Sound.playTone(300, 100, 50);

         // B+C
         //            ev3.offMotorB ();
         //            ev3.offMotorC ();

         // interrupt balance loop
         complete = true;
         balancer.interrupt();

         sleep (4000);
         // stop
      } else {
         prevOutOfBound = nowOutOfBound;
      }
   }
   
   private void sleep(int milliseconds) {
      try {
         Thread.sleep(milliseconds);
      } catch (InterruptedException e) {
         e.printStackTrace(writer);
         writer.flush();
      }
   }
}


User avatar
gloomyandy
leJOS Team Member
Posts: 5507
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: Segoway for EV3

Postby gloomyandy » Fri Oct 23, 2015 1:08 pm

I doubt if anyone is going to read through all of the code to work out what is going on, so if you need help you will need to provide some more info. You haven't told us much about what is going wrong, or where this code has come from. Is this a direct translation of the original LEGO code? You have also not told us what version of leJOS you are using. You have not told us what is going wrong (a video clip often helps), does the robot attempt to balance at all? What have you done to try and fix the problem? Have you tried the obvious things like reversing the mounting of the gyro (leJOS may not use the same rotation rule as the LEGO software).

There are some rather odd things in there, for instance you seem to be reading the gyro sensor five times and calculating an average reading from that. But the thing is that depending upon the sensor update rate you may well be simply reading the same value five times. When you read the sensor like this there is no guarantee that you will be getting a new reading you simply get the most recent value from the sensor, it may or may not have been updated. According to the LEGO doc the update rate is KHz so if you are reading it more often than that (which you almost certainly are), it is likely that you are getting the same sample multiple times.
leJOS news https://lejosnews.wordpress.com/


Return to “EV3 Software”

Who is online

Users browsing this forum: Google [Bot] and 1 guest