MPU6050 and Lejos

Post your NXJ projects, project ideas, etc here!

Moderators: 99jonathan, roger, imaqine

MPU6050 and Lejos

Postby dantjh » Tue Mar 11, 2014 10:37 pm

I tried very hard to build a Segway balancing bot with the MPU6050 sensor board attached to the NXT.

The software I used is based on http://www.i2cdevlib.com/devices/mpu6050#source that I ported to Java: RawI2CSensor.java and MPU6050.java. Using the complementarity filter I succeeded to get more or less stable values. Not moving the robot gives readings between 0 and 3° deg.

For the balancing part I used parts of an older lejos code: https://code.google.com/p/gelway/. I had to rewrite the getAngleVelocity(), since the getspeed() method cannot be used for NXTMotors any more. I used a method from the Segway lejos class instead.

Can you check the code if something is odd? Do you have any tips how to tune the PID loop in BalanceController.java?

Code: Select all
package thomas.nxt.gelway;

import lejos.nxt.I2CSensor;
import lejos.nxt.SensorPort;

public class RawI2CSensor extends I2CSensor {

    RawI2CSensor(SensorPort p)
    {
        super(p);
    }

        @Override
    public void setAddress(int addr)
    {
        super.address = addr;
    }

        public int getBit(int register, int bitNum, int buf) {
          byte [] buf1 = new byte[1];
          int count = super.getData(register, buf1,1);
          buf =  buf1[0] & (1 << bitNum);
          return count;
        }
       
        public int getBits(int register, int bitStart, int len, byte [] buf) {
            int count;
            byte [] buf1 = new byte[1];
            if ((count = super.getData(register, buf1, 1)) == 0) {             
                int mask = ((1 << len) - 1) << (bitStart - len + 1);
                buf1[0] &= mask;
                buf1[0] >>= (bitStart - len + 1);
                buf[0] = buf1[0];
            }
            return count;
        }
       
        public int sendBit(int register, int bitNum, int data){
           byte [] buf = new byte[1];
           super.getData(register, buf, 1);
           int buf1 = (data != 0) ? (buf[0] | (1 << bitNum)) : (buf[0] & ~(1 << bitNum));
           return super.sendData(register,(byte) buf1);
        }
       
        public int sendBits(int regAddr, int bitStart, int length, byte data) {

            byte [] b = new byte[1];
            if (super.getData(regAddr, b,1) == 0) {
                int mask = ((1 << length) - 1) << (bitStart - length + 1);
                data <<= (bitStart - length + 1); // shift data into correct position
                data &= mask; // zero all non-important bits in data
                b[0] &= ~(mask); // zero all important bits in existing byte
                b[0] |= data; // combine data with existing byte
                return super.sendData(regAddr, b[0]);
            } else {
                return -1;
            }
        }

}

Code: Select all
package thomas.nxt.gelway;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.util.Delay;

/**
 * Display the type and address of any I2C sensors attached to the NXT
 * @author andy
 */


public class MPU6050 {
   
   // https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
   private static final int MPU6050_RA_PWR_MGMT_1 = 0x6B;
   private static final int MPU6050_RA_CONFIG = 0x1A;
   private static final int MPU6050_RA_GYRO_CONFIG = 0x1B;
   private static final int MPU6050_RA_GYRO_XOUT_H = 0x43;
   private static final int MPU6050_RA_GYRO_YOUT_H = 0x45;
   private static final int MPU6050_RA_GYRO_ZOUT_H = 0x47;
   private static final int MPU6050_RA_ACCEL_XOUT_H = 0x3B;
   private static final int MPU6050_RA_ACCEL_YOUT_H = 0x3D;
   private static final int MPU6050_RA_ACCEL_ZOUT_H = 0x3F;
   
   private static final int MPU6050_RA_SMPLRT_DIV = 0x19;
   
   private static final int MPU6050_RA_WHO_AM_I = 0x75;
   private static final int MPU6050_RA_ACCEL_CONFIG = 0x1C;
   
   private static final int MPU6050_WHO_AM_I_LENGTH = 6;
   private static final int MPU6050_WHO_AM_I_BIT = 6;
   
   private static final int MPU6050_PWR1_DEVICE_RESET_BIT = 7;
   private static final int MPU6050_PWR1_SLEEP_BIT = 6;
   private static final int MPU6050_PWR1_CLKSEL_BIT =  2;
   private static final int MPU6050_PWR1_CLKSEL_LENGTH = 3;
   private static final int MPU6050_GCONFIG_FS_SEL_BIT  =  4;
   private static final int MPU6050_GCONFIG_FS_SEL_LENGTH =  2;
   private static final int MPU6050_ACONFIG_AFS_SEL_BIT = 4;
   private static final int MPU6050_ACONFIG_AFS_SEL_LENGTH = 2;
   private static final byte MPU6050_CLOCK_PLL_XGYRO = 0x01;
   
   private static final int MPU6050_CFG_DLPF_CFG_BIT = 2;
   private static final int MPU6050_CFG_DLPF_CFG_LENGTH =3;
   
   private static final int MPU6050_DLPF_BW_256 = 0x00;
   private static final int MPU6050_DLPF_BW_188 = 0x01;
   private static final int MPU6050_DLPF_BW_98 = 0x02;
   private static final int MPU6050_DLPF_BW_42 = 0x03;
   private static final int MPU6050_DLPF_BW_20 = 0x04;
   private static final int MPU6050_DLPF_BW_10 = 0x05;
   private static final int MPU6050_DLPF_BW_5 = 0x06;
   
   
   
   private static final byte MPU6050_GYRO_FS_250 = 0x00;
   private static final byte MPU6050_GYRO_FS_500 = 0x01;
   private static final byte MPU6050_GYRO_FS_1000 = 0x02;
   private static final byte MPU6050_GYRO_FS_2000 = 0x03;
   
   private static final byte MPU6050_ACCEL_FS_2 = 0x00;
   private static final byte MPU6050_ACCEL_FS_4 = 0x01;
   private static final byte MPU6050_ACCEL_FS_8 = 0x02;
   private static final byte MPU6050_ACCEL_FS_16 = 0x03;
   
   
    private static double ACCELEROMETER_SENSITIVITY = 8192.0;
    private static double GYROSCOPE_SENSITIVITY = 65.536;
    //private static double dt =  0.01;                     // 10 ms sample rate!   
   
   
    private double angle = 0.0;
    private int lastGetAngleTime = 0;
    private double lastOffset = 0;
    private final double a = 0.999999;// Weight of older offset values.
   
    private RawI2CSensor I2C;
   
    MPU6050(SensorPort sens) {
       I2C = new RawI2CSensor(sens);
      
      int waittime = 100;
      LCD.drawString("setting address",0,0);
      Delay.msDelay(waittime);
      I2C.setAddress(0x68<<1);

      
      LCD.drawString(testConnection()?"success":"fail",5,1);
      Delay.msDelay(waittime);
         
       setClockSource(MPU6050_CLOCK_PLL_XGYRO);
       setFullScaleGyroRange(MPU6050_GYRO_FS_250); // most sensitive
       setFullScaleAccelRange(MPU6050_ACCEL_FS_2);// most sensitive
      
       setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
      
       //setRate((byte) 7); //7 Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
       setRate((byte)4); // 200 Hz
       //setDLPFMode((byte) 0); // Gyroscope Output Rate = 8kHz when the DLPF is disabled
       setDLPFMode((byte) MPU6050_DLPF_BW_42);
       LCD.drawString("Rate= " + getRate() + "       ", 0, 1);
       LCD.drawString("DLPF= " + getDLPFMode() + " ", 0, 2);
      
       resetGyro();
      
       Delay.msDelay(100); // Wait for sensor to stabilize
    }
   
   
    /**
     * Reset the gyro angle
     */
    public void resetGyro()
    {
       angle = 0.0;
       lastGetAngleTime = 0;
       calcOffset();
    }
    public void calcOffset()
    {
       lastOffset = 0;
       double offsetTotal = 0;
       LCD.drawString("Calibrating Gyro", 0, 2);
       int npts=500;
       for (int i = 0; i < npts; i++) {
          offsetTotal += (double) (getRotationX() / GYROSCOPE_SENSITIVITY);
          try {
             Thread.sleep(4);
          } catch (InterruptedException e) {
          }
       }
//       while (!Button.ENTER.isDown()) {
          lastOffset = Math.ceil(offsetTotal / npts) + 1;
//          LCD.drawString("Calibration Done", 0, 4);
//          LCD.drawString("offset: " + lastOffset, 2, 5);
//          LCD.drawString("Press Enter", 1, 6);
//       }
       try {
          Thread.sleep(500);
       } catch (InterruptedException e) {
       }
    }
   
    private double getAngleOffset()
    {
       double offset = lastOffset * a + (1.0 - a) * (getRotationX() / GYROSCOPE_SENSITIVITY);
       lastOffset = offset;
       return offset;
    }
   
    public double getAngleVelocity()
    {
       double offset = getAngleOffset();
       
       return (double) (getRotationX() / GYROSCOPE_SENSITIVITY) - offset;
       
       //return (double) port.readValue() - offset;
    }
   
    // calculate the angle by direct integration
//    public double getAngle() {
//         int now = (int) System.currentTimeMillis();
//          int delta_t = now - lastGetAngleTime;
//
//          // Make sure we only add to the sum when there has actually
//          // been a previous call (delta_t == now if its the first call).
//          if (delta_t != now) {
//             angle += getAngleVelocity() * ((double) delta_t / 1000.0);
//          }
//          lastGetAngleTime = now;
//
//          return angle;
//    }
   
    // calculate the angle using complementarity filter
    public double getAngle() {
      int [] accData = new int[3];
      int [] gyrData = new int[3];
      float [] pitch = new float[1];
      float [] roll = new float[1];
      
      pitch[0]=(float) angle;
      //Delay.msDelay((long) (10 - (System.nanoTime()-timer)/1e6));
        getMotion6(accData, gyrData);
        ComplementaryFilter(accData, gyrData, pitch, roll);
        angle = pitch[0];
        return angle;
    }
   
    private void ComplementaryFilter(int [] accData, int [] gyrData, float [] pitch, float [] roll) {          
        double pitchAcc, rollAcc;               
       int now = (int) System.currentTimeMillis();
      int dt = (now - lastGetAngleTime);
     
      if (dt != now) {
        // Integrate the gyroscope data -> int(angularSpeed) = angle
        //double dt = (double) (System.nanoTime()-timer) * 1e-9;
        pitch[0] += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt/1000.; // Angle around the X-axis
        roll[0] -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt/1000.;    // Angle around the Y-axis
     
        // Compensate for drift with accelerometer data if !bullshit
        // Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
        int forceMagnitudeApprox = Math.abs(accData[0]) + Math.abs(accData[1]) + Math.abs(accData[2]);
        if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
        {
       // Turning around the X axis results in a vector on the Y-axis
            pitchAcc = Math.atan2((double)accData[1], (double)accData[2]) * 180 / Math.PI;
            pitch[0] = (float) (pitch[0] * 0.98 + pitchAcc * 0.02);
     
       // Turning around the Y axis results in a vector on the X-axis
            rollAcc = Math.atan2((double)accData[0], (double)accData[2]) * 180 / Math.PI;
            roll[0] = (float) (roll[0] * 0.98 + rollAcc * 0.02);
        }
      }
        lastGetAngleTime = now;
    }
                   
        private void setClockSource(byte source) {
           I2C.sendBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
        }
       
        private void setFullScaleGyroRange(byte range) {
           I2C.sendBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
        }
       
        private void setFullScaleAccelRange(byte range) {
           I2C.sendBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
        }
        private void setSleepEnabled(boolean enabled) {
           I2C.sendBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled?1:0);
        }
       
        private boolean testConnection() {
           return getDeviceID() == 0x34;
        }
       
        private byte getDeviceID() {
          byte[] buffer=new byte[1];
            //super.getData(MPU6050_RA_WHO_AM_I, buffer, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH);
          I2C.getBits(MPU6050_RA_WHO_AM_I,MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH,buffer);
          //LCD.drawInt(buffer[0],5,5); Delay.msDelay(2000);
            return buffer[0];
        }
       
       
       
       
        private int getDLPFMode() {
           byte [] buffer = new byte[1];
            int count = I2C.getBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer);
            if(count == 0) {
               return buffer[0];
            } else {
               return -1;
            }
   
        }
       
        private void setDLPFMode(byte mode) {
           I2C.sendBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
        }
       
        private int getRate() {
           byte [] buffer = new byte[1];
            int count = I2C.getData(MPU6050_RA_SMPLRT_DIV, buffer,1);
            if(count == 0) {
            return buffer[0];
            } else {
               return -1;
            }
        }
        private void setRate(byte rate) {
           I2C.sendData(MPU6050_RA_SMPLRT_DIV, rate);
        }
         
 
       
   
       
        // get rotation and acceleration in one read
        private void getMotion6(int [] accData, int [] gyrData) {
           byte [] buffer = new byte[14];
           I2C.getData(MPU6050_RA_ACCEL_XOUT_H, buffer,14);
            accData[0] = (((int)buffer[0]) << 8) | buffer[1];
            accData[1] = (((int)buffer[2]) << 8) | buffer[3];
            accData[2] = (((int)buffer[4]) << 8) | buffer[5];
            gyrData[0] = (((int)buffer[8]) << 8) | buffer[9];
            gyrData[1] = (((int)buffer[10]) << 8) | buffer[11];
            gyrData[2] = (((int)buffer[12]) << 8) | buffer[13];
        }
       
       
        // get rotation
        private void getRotation(int[] x, int[] y, int[] z) {
           byte[] buffer = new byte[6];
           I2C.getData(MPU6050_RA_GYRO_XOUT_H, buffer,6);
            x[0] = (((int) buffer[0] ) << 8) | buffer[1];
            y[0] = (((int) buffer[2] ) << 8) | buffer[3];
            z[0] = (((int) buffer[4] ) << 8) | buffer[5];
        }

        private int getRotationX() {
           byte[] buffer = new byte[2];
           I2C.getData( MPU6050_RA_GYRO_XOUT_H, buffer,2);
            //return (((int)buffer[0]) << 8) | buffer[1];
            return (short) ((buffer[0]) << 8) | (buffer[1] & (byte) 0xFF);
            //return (short) ((buffer[1]) << 8) | (buffer[0] & 0xFF);
        }
        private int getRotationY() {
           byte[] buffer = new byte[2];
           I2C.getData( MPU6050_RA_GYRO_YOUT_H, buffer,2);
            return (short) ((buffer[0]) << 8) | (buffer[1] & (byte) 0xFF);
            //return (short) ((buffer[1]) << 8) | (buffer[0] & 0xFF);
        } 
       
       
        //return (short)((b[off] & 0xFF) | (b[off + 1] << 8));
       
        private int getRotationZ() {
           byte[] buffer = new byte[2];
           I2C.getData( MPU6050_RA_GYRO_ZOUT_H, buffer,2);
            return (short) ((buffer[0]) << 8) | (buffer[1] & (byte) 0xFF);
        } 
       
        // get acceleration
        private void getAcceleration(int[] x, int[] y, int[] z) {
           byte[] buffer = new byte[6];
           I2C.getData(MPU6050_RA_ACCEL_XOUT_H, buffer,6);
            x[0] = (((int) buffer[0] ) << 8) | buffer[1];
            y[0] = (((int) buffer[2] ) << 8) | buffer[3];
            z[0] = (((int) buffer[4] ) << 8) | buffer[5];
        }

        private int getAccelerationX() {
           byte[] buffer = new byte[2];
           I2C.getData( MPU6050_RA_ACCEL_XOUT_H, buffer,2);
            return (short) ((buffer[0]) << 8) | (buffer[1] & (byte) 0xFF);
        }
        private int getAccelerationY() {
           byte[] buffer = new byte[2];
           I2C.getData( MPU6050_RA_ACCEL_YOUT_H, buffer,2);
            return (short) ((buffer[0]) << 8) | (buffer[1] & (byte) 0xFF);
        } 
        private int getAccelerationZ() {
           byte[] buffer = new byte[2];
           I2C.getData( MPU6050_RA_ACCEL_ZOUT_H, buffer,2);
            return (short) ((buffer[0]) << 8) | (buffer[1] & (byte) 0xFF);
        }         
       
       
}




Code: Select all
package thomas.nxt.gelway;
/**
 * A class which handles controlling the GELway directional movements. Works by altering
 * parameters in the CtrlParam class which are called by the BalanceController thread.
 *
 * @author Steven Jan Witzand
 * @version August 2009
 */
public class MotorDirection
{
   private CtrlParam ctrl;
   private final double turnPower = 200; // Speed at which the GELway rotates
   private final double tiltPower = 3; // Speed of moving forwards and backwards

   /**
    * MotorDirection constructor.
    *
    * @param ctrl
    *           The motor control parameters.
    */
   public MotorDirection(CtrlParam ctrl)
   {
      this.ctrl = ctrl;
   }

   /**
    * Moves the GELway forward for a specified period of time. Works by incrementally
    * increasing the motor tilt angle offset.
    *
    * @param period
    *           An integer number specifying the length to move the GELway forward.
    */
   public void forward(int period)
   {
      ctrl.setLeftMotorOffset(0);
      ctrl.setRightMotorOffset(0);
      for (int time = 0; time < period; time += 10) {
         try {
            Thread.sleep(10);
         } catch (InterruptedException e) {
         }
         ctrl.setTiltAngle(tiltPower);
      }
   }

   /**
    * Moves the GELway backward for a specified period of time. Works by incrementally
    * decreasing the motor tilt angle offset.
    *
    * @param period
    *           An integer number specifying the length to move the GELway backward.
    */
   public void backward(int period)
   {
      ctrl.setLeftMotorOffset(0);
      ctrl.setRightMotorOffset(0);

      for (int time = 0; time < period; time += 10) {
         try {
            Thread.sleep(10);
         } catch (InterruptedException e) {
         }
         ctrl.setTiltAngle(-1.5 * tiltPower);
      }
   }

   /**
    * Moves the GELway in a direction for a specified period of time and power. Works by
    * incrementally increasing the motor tilt angle offset.
    *
    * @param period
    *           An integer number specifying the length to move the GELway forward.
    */
   public void move(int period, double tiltDir)
   {
      ctrl.setLeftMotorOffset(0);
      ctrl.setRightMotorOffset(0);
      for (int time = 0; time < period; time += 10) {
         try {
            Thread.sleep(10);
         } catch (InterruptedException e) {
         }
         ctrl.setTiltAngle(tiltDir);
      }
   }

   /**
    * Rotate the GELway left indefinitely until a forward or backward movement is given.
    *
    * @param period
    *           An integer to delay other commands being sent to the GELway
    */
   public void left(int period)
   {
      ctrl.setLeftMotorOffset(-turnPower);
      ctrl.setRightMotorOffset(turnPower);
      delay(period);
   }

   /**
    * Rotate the GELway right indefinitely until a forward or backward movement is given.
    *
    * @param period
    *           An integer to delay other commands being sent to the GELway
    */
   public void right(int period)
   {
      ctrl.setLeftMotorOffset(turnPower);
      ctrl.setRightMotorOffset(-turnPower);
      delay(period);
   }

   /**
    * Instruct GELway to stop, and stay stopped for a given period of time.
    *
    * @param period
    *           An integer containing the number of millisecond to pause before returning.
    */
   public void stop(int period)
   {
      ctrl.setLeftMotorOffset(0);
      ctrl.setRightMotorOffset(0);
      delay(period);
   }

   /**
    * Make the programming sleep for a specified period of time.
    *
    * @param time
    *           An integer containing the number of milliseconds to sleep.
    */
   public void delay(int time)
   {
      try {
         Thread.sleep(time);
      } catch (Exception e) {}
   }
}


Code: Select all
package thomas.nxt.gelway;
/* -*- tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
import lejos.nxt.*;
import lejos.robotics.EncoderMotor;

/**
 * A class used to handle controlling the motors. Has methods to set the motors speed and
 * get the motors angle and velocity. Based off original programmers of Marvin Bent Bisballe
 * Nyeng, Kasper Sohn and Johnny Rieper
 *
 * @author Steven Jan Witzand
 * @version August 2009
 */
class MotorController
{

   private NXTMotor leftMotor;
   private NXTMotor rightMotor;
   // Sinusoidal parameters used to smooth motors
   private double sin_x = 0.0;
   private final double sin_speed = 0.1;
   private final double sin_amp = 20.0;
   // Motor globals
   private double motorPos = 0;
   private long mrcSum = 0, mrcSumPrev;
   private long motorDiff;
   private long mrcDeltaP3 = 0;
   private long mrcDeltaP2 = 0;
   private long mrcDeltaP1 = 0;
   private double tInterval;
   private long tCalcStart;
   
   /**
    * MotorController constructor.
    *
    * @param leftMotor
    *           The GELways left motor.
    * @param rightMotor
    *           The GELways right motor.
    */
   public MotorController(NXTMotor leftMotor, NXTMotor rightMotor)
   {
      this.leftMotor = leftMotor;
     
      this.leftMotor.resetTachoCount();

      this.rightMotor = rightMotor;
      this.rightMotor.resetTachoCount();
   }

   /**
    * Method is used to set the power level to the motors required to keep it upright. A
    * dampened sinusoidal curve is applied to the motors to reduce the rotation of the
    * motors over time from moving forwards and backwards constantly.
    *
    * @param leftPower
    *           A double used to set the power of the left motor. Maximum value depends on
    *           battery level but is approximately 815. A negative value results in motors
    *           reversing.
    * @param rightPower
    *           A double used to set the power of the right motor. Maximum value depends on
    *           battery level but is approximately 815. A negative value results in motors
    *           reversing.
    */
   public void setPower(double leftPower, double rightPower)
   {
//      sin_x += sin_speed;
//      int pwl = (int) (leftPower + Math.sin(sin_x) * sin_amp);
//      int pwr = (int) (rightPower - Math.sin(sin_x) * sin_amp);
      int pwl = (int) leftPower;
      int pwr = (int) rightPower;
      
      leftMotor.setPower(Math.abs(pwl));
      if (pwl > 0) {
         leftMotor.backward();
      } else if (pwl < 0) {
         leftMotor.forward();
      } else {
         leftMotor.stop();
      }

      rightMotor.setPower(Math.abs(pwr));
      if (pwr > 0) {
         rightMotor.backward();
      } else if (pwr < 0) {
         rightMotor.forward();
      } else {
         rightMotor.stop();
      }
   }

   /**
    * getAngle returns the average motor angle of the left and right motors
    *
    * @return A double of the average motor angle of the left and right motors in degrees.
    */
   public double getAngle()
   {
      return ((double) leftMotor.getTachoCount() +
            (double) rightMotor.getTachoCount()) / 2.0;
   }

   /**
    * getAngle returns the average motor velocity of the left and right motors
    *
    * @return a double of the average motor velocity of the left and right motors in
    *         degrees.
    */
   public void calcInterval(long cLoop) {
      if (cLoop == 0) {
         // First time through, set an initial tInterval time and
         // record start time
         tInterval = 0.0055;
         tCalcStart = System.currentTimeMillis();
      } else {
         // Take average of number of times through the loop and
         // use for interval time.
         tInterval = (System.currentTimeMillis() - tCalcStart)/(cLoop*1000.0);
      }
   }
   
   public double getAngleVelocity()
   {
//      return ((double) leftMotor.getActualSpeed() +
//            (double) rightMotor.getActualSpeed()) / 2.0;
     long mrcLeft, mrcRight, mrcDelta;

   // Keep track of motor position and speed
   mrcLeft = leftMotor.getTachoCount();
   mrcRight = rightMotor.getTachoCount();

   // Maintain previous mrcSum so that delta can be calculated and get
   // new mrcSum and Diff values
   mrcSumPrev = mrcSum;
   mrcSum = mrcLeft + mrcRight;
   motorDiff = mrcLeft - mrcRight;

   // mrcDetla is the change int sum of the motor encoders, update
   // motorPos based on this detla
   mrcDelta = mrcSum - mrcSumPrev;
   motorPos += mrcDelta;

   // motorSpeed is based on the average of the last four delta's.
   double motorSpeed = (mrcDelta+mrcDeltaP1+mrcDeltaP2+mrcDeltaP3)/(4*tInterval);

   // Shift the latest mrcDelta into the previous three saved delta values
   mrcDeltaP3 = mrcDeltaP2;
   mrcDeltaP2 = mrcDeltaP1;
   mrcDeltaP1 = mrcDelta;
   return motorSpeed;
   
   }

   /**
    * reset the motors tacho count
    */
   public void resetMotors()
   {
      leftMotor.resetTachoCount();
      rightMotor.resetTachoCount();
   }

   /**
    * stop both motors from rotating
    */
   public void stop()
   {
      leftMotor.stop();
      rightMotor.stop();
   }
}


Code: Select all
package thomas.nxt.gelway;

/**
 * This class is utilised to set and get common parameters between the Balance and
 * Behavioural threads. It is used to offset the motors power levels and the tilt offset of
 * the robot.
 *
 * @author Steven Witzand
 * @version August 2009
 */
class CtrlParam
{
   private double offsetLeft = 0.0;
   private double offsetRight = 0.0;
   private double tiltAngle = 0.0;
   private int driveState = 0;
   private double damp = 0.0;
   private boolean upright = false;

   /**
    * Set the GELways LEFT motor offset.
    *
    * @param offset
    *           a double used to set the left motor offset in degrees per second.
    */
   public synchronized void setLeftMotorOffset(double offset)
   {
      this.offsetLeft = offset;
   }

   /**
    * Set the GELways RIGHT motor offset.
    *
    * @param offset
    *           a double used to set the right motor offset in degrees per second.
    */
   public synchronized void setRightMotorOffset(double offset)
   {
      this.offsetRight = offset;
   }

   /**
    * Set the GELways LEFT motor offset.
    *
    * @return A double of the left motor offset in degrees per second.
    */
   public synchronized double leftMotorOffset()
   {
      return this.offsetLeft;
   }

   /**
    * Set the GELways RIGHT motor offset.
    *
    * @return A double of the right motor offset in degrees per second.
    */
   public synchronized double rightMotorOffset()
   {
      return this.offsetRight;
   }

   /**
    * Set the tilt motor angle offset. The method works incrementally since it is the nature
    * off the PID controller to remove the offset.
    *
    * @param angle
    *           A double of the tilt angle offset in degrees.
    */
   public synchronized void setTiltAngle(double angle)
   {
      this.tiltAngle += angle;
   }

   /**
    * Get the tilt motor angle offset.
    *
    * @return A double of the tilt angle offset in degrees.
    */
   public synchronized double tiltAngle()
   {
      return this.tiltAngle;
   }

   /**
    * Resets the tilt motor angle back to zero.
    */
   public void resetTiltAngle()
   {
      this.tiltAngle = 0.0;
   }

   /**
    * Set the current drive state of the GELway. 0 = Forwards, 1 = Backwards, 2 = Stationary
    *
    * @param state
    *           current drive state of the GELway
    */
   public void setDriveState(int state)
   {
      this.driveState = state;
   }
   /**
    * Returns the current drive state of the GELway
    *
    * @return current drive state of the GELway
    */
   public synchronized int getDriveState()
   {
      return this.driveState;
   }
   /**
    * Returns the current upright state of the GELway
    *
    * @return current upright state of the GELway
    */
   public void setUpright(boolean state)
   {
      this.upright = state;
   }
   public synchronized boolean getUpright()
   {
      return this.upright;
   }
   /**
    * Testing methods used to slow the initial accelleration of the GELway
    */
   public void setDamp(double weight)
   {
      this.damp += weight;
   }
   public void resetDamp()
   {
      this.damp = 0.0;
   }
   public synchronized double getDamp()
   {
      return this.damp;
   }
}


Code: Select all
package thomas.nxt.gelway;

import lejos.nxt.*;
import lejos.nxt.addon.EOPD;
import lejos.util.Datalogger;
/**
 * This class contains the parameters needed to keep the GELway balanced. It contains a PID
 * controller which reads in the angles and the angle velocities from the gyro sensor and
 * the left and right motor, and uses these (weighted) values to calculate the motor output
 * required to keep the GELway balanced.
 *
 * @author Steven Witzand
 * @version March 2009
 */
public class BalanceController extends Thread
{
   // The PID control parameters
   private final double Kp = 1.2;
   private final double Ki = 0.25;
   private final double Kd = 0.1;
   //private final int eopdThresh = 1022;
   double num = 0.0;
   int startLog = 0;
   static double damp = 0.1;
   private static final int stationary = 0; // left
   private static final int forwards = 1; // up
   private static final int backwards = 2; // down
   static boolean upright = true;
   // Teseing error contributions.
//   private final double K_psi = 44.257035; // Gyro angle weight
//   private final double K_phi = 0.806876; // Motor angle weight
//   private final double K_psidot = 0.620882; // Gyro angle velocity weight
//   private final double K_phidot = 0.039711;// Motor angle velocity weight
   // Original Balance numbers
    private final double K_psi = 34.189581; // Gyro angle weight
    private final double K_phi = 0.835082; // Motor angle weight
    private final double K_psidot = 0.646772; // Gyro angle velocity weight
    private final double K_phidot = 0.028141; // Motor angle velocity weight
   private static CtrlParam ctrl;
   public boolean offsetDone = false;
   /**
    * BalanceController constructor.
    *
    * @param ctrl The motor control parameters.
    */
   public BalanceController(CtrlParam ctrl)
   {
      this.ctrl = ctrl;
      setDaemon(true);
   }

   /**
    * The BalanceController thread which constantly runs to keep the GELway upright
    */
   public void run()
   {
      
      MotorController motors = new MotorController(new NXTMotor(MotorPort.A),new NXTMotor(MotorPort.B));
      MPU6050 gyro = new MPU6050(SensorPort.S2);
      TouchSensor touch1 = new TouchSensor(SensorPort.S3);      
     TouchSensor touch2 = new TouchSensor(SensorPort.S4);
      
     // EOPD eopd = new EOPD(SensorPort.S2);
      //eopd.setModeLong();
      double int_error = 0.0;
      double prev_error = 0.0;
      int cLoop = 0;
      while (true) {
         motors.calcInterval(cLoop++);
         // Start balancing provided GELway is upright and the EOPD sensor can sense the
         // ground
        // while (eopd.readRawValue() < eopdThresh && upright) {
         while(!touch1.isPressed() && !touch2.isPressed() && upright) {
            ctrl.setUpright(true);
            runDriveState();
            double Psi = gyro.getAngle();
            double PsiDot = gyro.getAngleVelocity();
            // ctrl.tiltAngle() is used to drive the robot forwards and backwards
            double Phi = motors.getAngle() - ctrl.tiltAngle();
            double PhiDot = motors.getAngleVelocity();
            // Proportional Error
            double error = Psi * K_psi + Phi * K_phi + PsiDot * K_psidot + PhiDot
                  * K_phidot;
            // Integral Error
            int_error += error;
            // Derivative Error
            double deriv_error = error - prev_error;
            prev_error = error;
            // Power sent to the motors
            double pw = (error * Kp + deriv_error * Kd + int_error * Ki) * 1;
           
           LCD.drawString("G angle= " + Psi + "       ", 0, 2);
           LCD.drawString("G dotang= " + PsiDot + "       ", 0, 3);
           LCD.drawString("M angle= " + Phi + "       ", 0, 4);
           LCD.drawString("M dotang= " + PhiDot + "       ", 0, 5);          
           LCD.drawString("power= " + pw + "       ", 0, 6);

//           motors.setPower(10, 10);
//            motors.setPower(pw + ctrl.leftMotorOffset(), pw + ctrl.rightMotorOffset());
            // Delay used to stop Gyro being read to quickly. May need to be increase or
            // decreased depending on leJOS version.
            //delay(6);
         }
         startLog = 0;
         motors.stop();
         upright = false;
         ctrl.setUpright(false);
         //while (eopd.readRawValue() > eopdThresh) {}
         while(touch1.isPressed() || touch2.isPressed()) {}
         // Restart the robot after the third beep. Reset balance parameters
         //if (eopd.readRawValue() < eopdThresh) {
         if(!touch1.isPressed() && !touch2.isPressed()) {
            for (int i = 0; i < 3; i++) {
               //if (eopd.readRawValue() > eopdThresh) break;
               if(touch1.isPressed() || touch2.isPressed()) break;
               Sound.setVolume(50 + i * 25);
               Sound.beep();
               delay(700);
            }
            gyro.resetGyro();
            motors.resetMotors();
            ctrl.resetTiltAngle();
            int_error = 0.0;
            prev_error = 0.0;
            ctrl.setDriveState(stationary);
            upright = true;
         } else { motors.stop(); }
      }
   }

   public static void delay(int time)
   {
      try {Thread.sleep(time);} catch (Exception e) {}
   }

   /**
    * Returns the current upright state of the GELway. TRUE = Upright
    *
    * @return upright current upright state of the GELway
    */
   public static boolean getUpright() { return upright; }

   /**
    * Run drive state is used to keep the GELway in a constant state of forwards or
    * backwards motion. It can be set to keep the GELway stationary as well.
    */
   public static void runDriveState()
   {
      if (ctrl.getDriveState() == forwards)
         ctrl.setTiltAngle(3 - 3 * Math.exp(-ctrl.getDamp()));
      else if (ctrl.getDriveState() == backwards)
         ctrl.setTiltAngle(-5 + 3 * Math.exp(-ctrl.getDamp()));
      else
         ctrl.setTiltAngle(0);
      ctrl.setDamp(0.1);
   }
}


Code: Select all
package thomas.nxt.gelway;

import lejos.nxt.Button;

public class GELway {

   public static void main(String[] args) {
      // TODO Auto-generated method stub
      CtrlParam ctrl1 = new CtrlParam();
      BalanceController bcthread = new BalanceController(ctrl1);
      bcthread.start();
      
      while(Button.ESCAPE.isUp()) {Thread.yield();};
   }

}
dantjh
New User
 
Posts: 3
Joined: Tue Mar 11, 2014 8:41 pm

Re: LEJOS ROCKS PART 2

Postby dantjh » Sat Mar 15, 2014 3:54 pm

Hello all.

I had to step back a bit and keep it simple. Now the robot is balancing using a < 5 USD 6-axis board MPU-6050 using of course Java LEJOS. The program is mostly a 1:1 translation of the NXC program from here: http://robin-grotkasten.jimdo.com/elektronik-projekte/lego-nxt-standalone-mpu6050-nxc/ and here: http://forum.arduino.cc/index.php/topic,161727.0.html.
Herewith I show that LEJOS is able to balance the robot and that there is no reason to use NXC as suggested by the second URL above. The balancing loop takes 1 millisecond in LEJOS ! What an incredible software LEJOS is - thanks to all developers :wink:

I still might have a sign error on the route_DTerm. But the balancing seems to work very well. Please see the youtube video http://youtu.be/RXCjLufuLCw


Code: Select all
package thomas.nxt.simplebalance;

import lejos.nxt.Button;
import lejos.nxt.I2CPort;
import lejos.nxt.I2CSensor;
import lejos.nxt.LCD;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTMotor;
import lejos.nxt.SensorConstants;
import lejos.nxt.SensorPort;
import lejos.util.Delay;
import lejos.util.EndianTools;

public class SimpleBalance {
      private static NXTMotor leftMotor;
      private static NXTMotor rightMotor;
      
      public static int TachoCount()
      {
         return ( leftMotor.getTachoCount() +  rightMotor.getTachoCount()) / 2;
      }
      
   private static double tInterval;
   private static long tCalcStart;
   
   public static void calcInterval(long cLoop) {
      if (cLoop == 0) {
         // First time through, set an initial tInterval time and
         // record start time
         tInterval = 0.0055;
         tCalcStart = System.currentTimeMillis();
      } else {
         // Take average of number of times through the loop and
         // use for interval time.
         tInterval = (System.currentTimeMillis() - tCalcStart)/(cLoop*1000.0);
      }
   }
   
   private static byte [] buf = new byte[6];
   private static int angle_speed0 = 200;
   
   public static void main(String[] args) {
      
      I2CSensor gyro = new I2CSensor(SensorPort.S2,0x68<<1,SensorConstants.TYPE_LOWSPEED,I2CPort.HIGH_SPEED);
      
      
      gyro.getData(0x75, buf,0, 1);
      LCD.drawString("who am I: " + buf[0],0,1);
      
      gyro.sendData(0x6b, (byte) 0x80); // reset all register to default
      
      Delay.msDelay(5);
      gyro.sendData(0x6b, (byte) 0x03); // set z axis as clock source
      gyro.sendData(0x1a, (byte) 0x00); // disables FSYNC, configures DPLF
      gyro.sendData(0x1b, (byte) 0x18);   //0b11000 set range to 2000*2
      
      Delay.msDelay(100);
      
      
      
      int angle_absolute=0;
      long cloop = 0;
      int angle_P_factor = 12;
      int angle_D_factor = 100;
      int last_angle_error = 0;
      int target_angle = 0;
      
      
      int route = 0;
      int route_P_factor = -10;
      int route_D_factor = -250;
      int last_route_error = 0;
      
      
      leftMotor = new NXTMotor(MotorPort.A);
      rightMotor = new NXTMotor(MotorPort.B);
      
      leftMotor.resetTachoCount();
      rightMotor.resetTachoCount();
      
      
      while(Button.ESCAPE.isUp()) {
         calcInterval(cloop);
         gyro.getData(0x43, buf,0, 6); // read only gyro data
         int angle_speed = EndianTools.decodeShortBE(buf,0)*10 + angle_speed0;
         int true_angle_speed = (int) (angle_speed / 655.536);
         //int angle_speed =  (((int)buf[0]) << 8) | buf[1];
         angle_absolute +=  true_angle_speed;
         
         int error_angle = target_angle - angle_absolute;
         int angle_PTerm = (int) (error_angle*angle_P_factor);
         int angle_DTerm = (int) (true_angle_speed*angle_D_factor);
         
         route = TachoCount();
         int error_route = 0 - route;
         int route_PTerm = (int) (error_route*route_P_factor);
         int route_DTerm = (int) ((error_route - last_route_error)*route_D_factor);
         last_route_error = error_route;
         
         int PD = angle_PTerm-angle_DTerm-route_PTerm-route_DTerm;
         PD/=10;
         
         if(PD>100) PD = 100;
         if(PD<-100) PD = -100;
         
         int PD_abs = Math.abs(PD);
         leftMotor.setPower((int)(PD_abs));
         rightMotor.setPower((int)(PD_abs));
            if (PD > 0) {
                leftMotor.backward();
                rightMotor.backward();
             } else if (PD < 0) {
                leftMotor.forward();
                rightMotor.forward();
             } else {
                leftMotor.stop();
                rightMotor.stop();
             }
         
         if(cloop%200==0) {
            LCD.drawString("gyr sp x: " + angle_speed + "     ", 0, 2);
            LCD.drawString("angle x: " + angle_absolute+"   ", 0, 3);
            LCD.drawString("loop_time ms: " + (int) (tInterval*1000)+"   ", 0, 4);
            LCD.drawString("power: "+PD+"   ",0,5);
            LCD.drawString("err angle: " + error_angle + "     ", 0, 5);
            LCD.drawString("err route: " + error_route +"   ", 0, 6);
         }
         if(Button.LEFT.isDown()) target_angle-=1;
         if(Button.RIGHT.isDown()) target_angle+=1;
         
         //Delay.msDelay(10);
         cloop++;
      }
      

   }

}
dantjh
New User
 
Posts: 3
Joined: Tue Mar 11, 2014 8:41 pm


Return to NXJ Projects

Who is online

Users browsing this forum: No registered users and 5 guests

more stuff