View unanswered posts | View active topics It is currently Mon Sep 21, 2020 11:49 am






Reply to topic  [ 11 posts ] 
EV3 Balanc3r code 
Author Message
Rookie

Joined: Tue Oct 28, 2014 9:17 am
Posts: 7
Post EV3 Balanc3r code
Hi all

I have ported Laurens Balanc3r http://robotsquare.com/2014/06/23/tutorial-building-balanc3r/ /SegWay-wannabee LabView (?) program to RobotC. Just the basic balancing stuff, although it is easy to modify it to allow motion by simply updating the requestedSpeed or referencePosition variables in the code below.

I have made a number of changes, mainly regarding naming of variables and especially regarding gyro rate readings and calibration, but on top-level it is still close to the original code. The control loop parameters are not exactly equal due to changing the sample time.

There are still a number of TODOs as well as some debug statements, but it seems to work fine as it is now, using RobotC 4.26.

Enjoy... :D

EDIT: I use a few Natural Language commands; please enable Natural Language under Robot->Platform Type->Natural Language before trying to compile the program. It is easy to get rid of these commands (see Xanders post below), but I find the use of them convenient.

Code:
#pragma config(Sensor, S2,     gyroSensor,     sensorEV3_Gyro, modeEV3Gyro_Rate)
#pragma config(Sensor, S4,     remoteSensor,   sensorEV3_IRSensor, modeEV3IR_Remote)
#pragma config(Motor,  motorA,          rightMotor,    tmotorEV3_Large, openLoop, driveRight, encoder)
#pragma config(Motor,  motorB,           ,             tmotorEV3_Large, openLoop)
#pragma config(Motor,  motorC,          leftMotor,     tmotorEV3_Large, openLoop, driveLeft, encoder)
#pragma config(Motor,  motorD,           ,             tmotorEV3_Large, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Pragmas for showing different debug windows
//#pragma DebuggerWindows("Locals")
//#pragma DebuggerWindows("Globals")
#pragma DebuggerWindows("DebugStream")
// #pragma DebuggerWindows("EV3LCDScreen")


// Maximum number of entries stored in encoder values array, used for computing speed over some dt's
#define MAX_ENCODER_VALUES ( 7 )
// Enable history?
//#define ENABLE_HISTORY
// History size for debugging
#define HISTORY_SIZE ( 300 )
// Power limit (+/- POWER_LIMIT) for motors
// For debugging this can be set to a low value (< 100)
#define POWER_LIMIT ( 100 )


// We use a lot of global variables
// Not very nice...
// TODO clean up at some point in time...

// Requested sample time in milliseconds
int sampleTime = 22;
// With a sampleTime = 22 and basic balancing, the T1 timer samples
// shows an average of 23.5 ms actual sample time, +/- around 2 ms
// This needs to be taken into account when calculating dt below
// Sample time difference is dt * 1000 - sampleTime, as calculated based on T1 samples
// In milliseconds
// The original Balanc3r code had -2 implicitly here
#define SAMPLE_TIME_DIFFERENCE ( 1.5 )
// Sample time, converted to seconds and corrected for delays in loop
float dt = ( 1.0 * sampleTime + SAMPLE_TIME_DIFFERENCE ) / 1000.0;

// Wheel diameter in mm
short wheelDiameterInMm = 42.0;
// Convert wheel diameter to radius in meters
float wheelRadius = wheelDiameterInMm / 2000.0;

// Encoder index for indexing encoder values array for motor speed computation
int encoderValueIndex = 0;
// Running encoder value for the last MAX_ENCODER_VALUES epochs
// Used for computing the average speed over MAX_ENCODER_VALUES epochs
long encoderValues[ MAX_ENCODER_VALUES ];

// Angle (integrated from angular rate) in degrees
float robotAngle = 0.0;
// Current of gyro rate bias in deg/s
float gyroRateBias;
// Currently measured gyro rate in deg/s
float gyroRate;

//
// PID constants
//
// P constant in TODO unit, original Balanc3r value 0.6
float kp = 0.6;
// I constant in TODO unit, original Balanc3r value 14, changed due to another dt
float ki = 12.2;
// D constant in TODO unit, original Balanc3r value 0.005, changed due to another dt
float kd = 0.0058;

//
// Sensor "fusion" constants
//
// Gain for sensor fusion for angular velocity in TODO unit, original Balanc3r value 1.3
float gainAngularVelocity = 1.3;
// Gain for sensor fusion for angle TODO unit, original Balanc3r value 25, changed due to another dt
float gainAngle = 21.7;
// Gain for sensor fusion for motor speed TODO units, , original Balanc3r value 75, changed due to another dt
float gainRobotSpeed = 86.3;
// Gain for sensor fusion for motor position TODO units, baseline 350
float gainRobotPosition = 350.0;

//
// Current speed, position and power
//
// Robot speed in m/s
float robotSpeed = 0.0;
// Robot position in m
float robotPosition = 0.0;

// Current power
int rightPower, leftPower;

//
// Error stuff
//
// Out-of-bounds variables and counter
bool NowOutOfBound = false;
bool PrevOutOfBound = false;
int OutOfBoundCount = 0;

//
// Debugging
//

#ifdef ENABLE_HISTORY

// Time history
int timeHist[ HISTORY_SIZE ];
// Angle history in deg
float angleHist[ HISTORY_SIZE ];
// Mean angular rate history in deg/s
float meanAngularRateHist[ HISTORY_SIZE ];
// Current angular rate history in deg/s
float angularRateHist[ HISTORY_SIZE ];
// Robot speed history in TODO units
float robotSpeedHist[ HISTORY_SIZE ];
// Robot position history in meters
float robotPositionHist[ HISTORY_SIZE ];
// Combined sensor history in TODO units
float combinedSensorsHist[ HISTORY_SIZE ];
// PID output history in TODO units
float pidOutputHist[ HISTORY_SIZE ];
// Storing of past motor powers
int pwrHist[ HISTORY_SIZE ];

#endif



/**
 * Get gyro rate based on a single reading
 */
float getGyroRate()
{
  // Originally an average was used in Balanc3r, but a single reading actually seems to be more stable
   return getGyroRate( gyroSensor );
}


/**
 * Calibration of gyro
 */
float calibrate()
{
   int numberOfReadings = 100;
   playSound( soundBeepBeep );
   writeDebugStreamLine( "Resetting gyro, keep robot still" );
  resetGyro( gyroSensor );
  // This sleep is very important!
  // If not used, the gyro readings will be way off afterwards
   sleep( 3000 );

   gyroRateBias = 0.0;
   // Make a few test readings prior to actual calibration
   for( int i = 0; i < numberOfReadings; i++ ) {
      gyroRateBias += getGyroRate();
   }
   gyroRateBias /= numberOfReadings;
   writeDebugStreamLine("Initial gyroRateBias (not used): %f", gyroRateBias );
   sleep( 500 );
   gyroRateBias = 0.0;
   sleep(100);
   for( int i = 0; i < numberOfReadings; i++ ) {
      gyroRateBias += getGyroRate();
   }
   gyroRateBias /= numberOfReadings;
   writeDebugStreamLine("Second gyroRateBias (not used): %f", gyroRateBias );
   sleep(500);
   gyroRateBias = 0.0;
   // And now do the actual calibration
   for( int i = 0; i < numberOfReadings; i++ ) {
      gyroRateBias += getGyroRate();
   }
   gyroRateBias /= numberOfReadings;
   writeDebugStreamLine("Third gyroRateBias (used): %f", gyroRateBias );
   playSound( soundDownwardTones );
   sleep( 100 );
   return gyroRateBias;
}


/**
 * Initialize
 */
void initialize()
{
   writeDebugStreamLine("dt %f", dt );
   for ( int i = 0; i < MAX_ENCODER_VALUES; i++ ) encoderValues[ i ] = 0;
   resetMotorEncoder( rightMotor );
   resetMotorEncoder( leftMotor );
   sleep( 100 );
   gyroRateBias = calibrate();
}


/**
 * Propagates reference position based on wanted speed and time step.
 * Returns updated referencePosition
 */
float position( float lastReferencePosition, float requestedSpeed )
{
   // TODO why 0.002?
//   return lastReferencePosition + requestedSpeed * dt * 0.002;
   return lastReferencePosition + requestedSpeed * dt;
}


/**
 * Returns the motor speed in deg/s
 * based on the last MAX_ENCODER_VALUES averages of encodervalues for left/right encoders
 */
float myGetMotorSpeed()
{
   encoderValueIndex++;
   if ( encoderValueIndex == MAX_ENCODER_VALUES ) encoderValueIndex = 0;
   int compare_index = encoderValueIndex + 1;
   if ( compare_index == MAX_ENCODER_VALUES ) compare_index = 0;
   long rightEncoderValue = getMotorEncoder( rightMotor );
   long leftEncoderValue = getMotorEncoder( leftMotor );
   long avgEncoderValue = ( rightEncoderValue + leftEncoderValue ) / 2;
   encoderValues[ encoderValueIndex ] = avgEncoderValue;
   float tmp = ( encoderValues[ encoderValueIndex ] - encoderValues[ compare_index ] ) / ( dt * MAX_ENCODER_VALUES );
   return tmp;
}


/**
 * Read encoders and determine current robot speed and robot position in m/s and m.
 */
void readEncoders()
{
   robotSpeed = wheelRadius * myGetMotorSpeed() / 57.3; // 57.3 = 180/pi, needed for converting deg->rad
   // TODO Here we read the encoders again; this can be optimized away as we have just read them above...
   long rightEncoderValue = getMotorEncoder( rightMotor );
   long leftEncoderValue = getMotorEncoder( leftMotor );
   long avgEncoderValue = ( rightEncoderValue + leftEncoderValue ) / 2;
   robotPosition = wheelRadius * avgEncoderValue / 57.3;
}


/**
 * Reads gyro and estimates angle and angular velocity TODO units?
 */
void readGyro()
{
   // Estimate of the current robotAngleBias due to drifting gyro
   static float robotAngleBias = 0.0;
   // This seems to be rather high. Corresponds to a full new update every ~100-300 ms (for dt = 20 ms and gyroRateBiasUpdateRatio = 0.2)...
   float gyroRateBiasUpdateRatio = 0.2;
   // Update ratio of robotAngleBias; should probably be way lower than gyroRateBiasUpdateRatio
   float robotAngleBiasUpdateRatio = 0.0;
   float currentGyroRateMeasurement = getGyroRate();
   // Estimate gyroRateBias rate by updating the value slowly
   // By multiplying with dt we make scale meanUpdateRatio to 1 sec
   gyroRateBias = gyroRateBias * ( 1 - dt * gyroRateBiasUpdateRatio ) +
      currentGyroRateMeasurement * dt * gyroRateBiasUpdateRatio;
   // Angular velocity can be computed directly now
   gyroRate = currentGyroRateMeasurement - gyroRateBias;
   // We note that the above update is "slow" compared to sample rate; thus
   // for drifting values of the gyro rate bias, the esimated value will always lack behind.
   // This again means that the integrated gyro rate (the angle of the robot) will drift.
   // In principle this is not a problem as the combined sensor value also includes the robot position
   // and the robot position can then make up for the drift (and this can be seen as the robot
   // slowly drifts in its position).
   // On the other hand we would like to have a good estimate of the angle.
   // In order to correct this we use the robot position over time to correct the angle
   // TODO we need a better way to do this; using the encoders really only works for a stationary robot
   //      In any case, we have set robotAngleBiasUpdateRatio = 0.0 , so this part is not used at all
   //      But this also means that the robotAngle eventually drifts a looong way from 0, thus triggering
   //      an error scenario at some point in time
   robotAngleBias = robotAngleBias * ( 1 - dt * robotAngleBiasUpdateRatio ) -
      ( robotPosition * gainRobotPosition / gainAngle ) * dt * robotAngleBiasUpdateRatio;
   // And angle is computed based on integration from last epoch
   robotAngle += gyroRate * dt - robotAngleBias;
}


/**
 * Combines the sensor values ("sensor fusion") in order to have a single error estimator
 * for feeding into the PID controller
 */
float combineSensorValues(
  float angularVelocity,
  float robotAngle,
  float robotSpeed,
  float robotPosition,
  float robotReferencePosition )
{
   return
     (  robotPosition - robotReferencePosition ) * gainRobotPosition +
     robotSpeed * gainRobotSpeed +
     robotAngle * gainAngle +
     angularVelocity * gainAngularVelocity;
}


/**
 * PID controller
 */
float PID( float input, float reference )
{
   // Integration part
   static float accumulatedError = 0.0;
   // Differentiation part
   static float diffError = 0.0;
   static float previousError = 0.0;
   float currentError = input - reference;
   accumulatedError += currentError * dt;
   diffError = ( currentError - previousError ) / dt;
   previousError = currentError;
   return kp * currentError + accumulatedError * ki + diffError * kd;
}


/**
 * Detect erroneous pidOutput
 */
void errors( float pidOutput )
{
   if ( abs( pidOutput ) > 100 ) NowOutOfBound = true;
   if ( NowOutOfBound && PrevOutOfBound ) {
      OutOfBoundCount++;
   } else {
      OutOfBoundCount = 0;
  }
  if ( OutOfBoundCount > 20 ) {
     setMultipleMotors( 0, leftMotor, rightMotor );
     eraseDisplay();
     displayCenteredBigTextLine( 2, "ERROR" );
     sleep( 100 );
  }
}


/**
 * Get steering
 * TODO Currently hardcoded, should be set based on IR receiver or other
 */
float getSteer()
{
   return 0.0;
}


/**
 * Sets motor power and corrects for steering and motors not being fully in sync
 */
void setMotorPower( float requestedSteering, float averagePower )
{
   //float sync_0;
   float extra_pwr = 0.0;
   static float old_steering = 0.0;
   static float pwr_c = 0.0;
   static float pwr_b = 0.0;

   // TODO update the below once steering is implemented
   //if ( requestedSteering < -50 ) requestedSteering = -50;
   //if ( requestedSteering > 50 ) requestedSteering = 50;
   //if ( requestedSteering == 0 ) {
   //   if ( old_steering == 0 ) {
   //      // Do nothing
   //   } else {
   //     // TODO again we should probably just use the value we have read twice before here..
   //      sync_0 = getMotorEncoder( rightMotor ) - getMotorEncoder( leftMotor );
   //   }
   //   // TODO and again we are reading it...
   //   // TODO 0.05 why?
   //   extra_pwr = ( getMotorEncoder( leftMotor ) - getMotorEncoder( rightMotor ) - sync_0 ) * 0.05;
   //} else {
   //   extra_pwr = -0.5 * requestedSteering;
   //}
   // TODO: extra_pwr set to 0, hardcoded...
   extra_pwr = 0.0;
   pwr_c = averagePower - extra_pwr;
   pwr_b = averagePower + extra_pwr;
   old_steering = requestedSteering;
   // TODO 0.021/wheelRadius???
   rightPower = pwr_b; // * 0.021 / wheelRadius;
   leftPower = pwr_c; // * 0.021 / wheelRadius;
   if ( rightPower > POWER_LIMIT ) rightPower = POWER_LIMIT;
   if ( rightPower < -POWER_LIMIT ) rightPower = -POWER_LIMIT;
   if ( leftPower > POWER_LIMIT ) leftPower = POWER_LIMIT;
   if ( leftPower < -POWER_LIMIT ) leftPower = -POWER_LIMIT;
   setMotor( rightMotor, rightPower );
   setMotor( leftMotor, leftPower );
}


/**
 * Main program
 */
task main()
{
  // Counter for counting main loop entries
  // Used for keeping track of stored history and checking elapsed time versus set loop time (dt)
  int counter = 0;
  // Reference position in meters
  float referencePosition = 0.0;
   // Requested speed in m/s
   float requestedSpeed = 0.00;

  // Combined sensor value, computed based on angle, angular rate, speed and position
   float combinedSensors;

   // PID reference in motor power (-100 to 100)
   float pidReference = 0.0;
  // PID output, as computed by PID controller, in motor power (-100 to 100)
   float pidOutput;
  // Number of times the control loop did not meet its deadline (TODO is this actually working? maybe use counter*dt compared to T1 instead?)
   int controlLoopTimerOverflow = 0;

   initialize();

   // Balance loop
  eraseDisplay();
  displayCenteredBigTextLine( 2, "Balancing" );
   writeDebugStreamLine("*** Starting main loop");
   resetTimer( T1 );
   resetTimer( T2 );
#ifdef ENABLE_HISTORY
   repeatUntil( ( getTimer( T1, seconds ) > 20 ) || ( abs( robotAngle ) > 60 ) || ( OutOfBoundCount > 20 ) || ( counter >= HISTORY_SIZE ) ) {
#else
   repeatUntil( ( abs( robotAngle ) > 60 ) || ( OutOfBoundCount > 20 ) ) {
#endif
    // Get expected position based on current position and requested speed
      referencePosition = position( referencePosition, requestedSpeed );
      // Read the encoders and compute robot position and speed at the same time
      readEncoders();
      // Read the gyro and update gyroRateBias and robot angle
      readGyro();
      // Sensor "fusion"
      combinedSensors = combineSensorValues( gyroRate, robotAngle, robotSpeed, robotPosition, referencePosition );
      // PID control
      pidOutput = PID( combinedSensors, pidReference );
      // Any errors?
      errors( pidOutput );
      // Set the motor power
     setMotorPower( getSteer(), pidOutput );

#ifdef ENABLE_HISTORY
      // Save history
     timeHist[ counter ] = getTimer( T1, milliseconds );
     angleHist[ counter ] = robotAngle;
     meanAngularRateHist[ counter ] = gyroRateBias;
     angularRateHist[ counter ] = gyroRate;
     robotSpeedHist[ counter ] = robotSpeed;
      robotPositionHist[ counter ] = robotPosition;
     combinedSensorsHist[ counter ] = combinedSensors;
     pidOutputHist[ counter ] = pidOutput;
     pwrHist[ counter ] = rightPower;
#endif

      counter++;

      // Write out some statistics from time to time
      //if ( ( counter & 0xFF ) == 0xFF ) {
      //  writeDebugStreamLine("%f;%f;%f;%f;%f;%f;%f",
      //     kp, ki, kd,
      //     gainAngularVelocity, gainAngle, gainRobotSpeed, gainRobotPosition );
    //    writeDebugStreamLine("%d;%f;%f;%f;%f;%f;%f;%f;%d",
    //       getTimer( T1, milliseconds ),
    //       robotAngle,
    //       gyroRateBias,
    //       gyroRate,
    //       robotSpeed,
    //       robotPosition,
    //       combinedSensors,
    //       pidOutput,
    //       rightPower );
      //}

      if ( getTimer( T2, milliseconds ) > sampleTime )
      {
         controlLoopTimerOverflow++;
      }
      repeatUntil( getTimer( T2, milliseconds ) > sampleTime  ) {}
      resetTimer( T2 );
   }

  // Post-mortem statistics
  displayCenteredBigTextLine( 2, "OOB: %4d", OutOfBoundCount );
  displayCenteredBigTextLine( 4, "OVF: %4d", controlLoopTimerOverflow );
  displayCenteredBigTextLine( 6, "robotAngle: %4f", robotAngle );
  setMultipleMotors( 0, leftMotor, rightMotor );

#ifdef ENABLE_HISTORY
  // Writing out debug stuff
  // Sleeps are needed, otherwise written data is lost
  writeDebugStreamLine("# kp; ki; kd; gainAngVel; gainAngle; gainMotorSpeed; gainMotorPosition");
  sleep(50);
  writeDebugStreamLine("%f;%f;%f;%f;%f;%f;%f",
     kp, ki, kd,
     gainAngularVelocity, gainAngle, gainRobotSpeed, gainRobotPosition );
  sleep(50);
   writeDebugStreamLine("# time; angle; gyroRateBias; angularRate; robotSpeed; robotPosition; combinedSensors; pidOutput; pwr");
  sleep(50);
   for( counter = 0; counter < HISTORY_SIZE; counter++ ) {
     writeDebugStreamLine("%d;%f;%f;%f;%f;%f;%f;%f;%d",
        timeHist[ counter ],
        angleHist[ counter ],
        meanAngularRateHist[ counter ],
        angularRateHist[ counter ],
        robotSpeedHist[ counter ],
        robotPositionHist[ counter ],
        combinedSensorsHist[ counter ],
        pidOutputHist[ counter ],
        pwrHist[ counter ] );
    sleep( 50 );
   }
#endif

  sleep(30000);

}


Last edited by pmb on Sat Nov 01, 2014 3:19 am, edited 1 time in total.



Thu Oct 30, 2014 5:27 am
Profile
Site Admin
Site Admin
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3654
Location: Rotterdam, The Netherlands
Post Re: EV3 Balanc3r code
Thank you for sharing this!

= Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Fri Oct 31, 2014 4:59 am
Profile WWW
Rookie

Joined: Tue Aug 19, 2014 12:15 pm
Posts: 37
Post Re: EV3 Balanc3r code
I used 4.27 and received the following errors.

File ".\SourceFile002.c" compiled on Oct 31 2014 09:28:18
**Error**:Undefined procedure 'setMultipleMotors'.
**Error**:Undefined procedure 'setMotor'.
**Error**:Undefined procedure 'resetTimer'.
**Error**:Undefined procedure 'getTimer'.
**Error**:Undefined variable 'milliseconds'. 'short' assumed.
**Error**:Undefined procedure 'setMultipleMotors

Any ideas?


Fri Oct 31, 2014 9:35 am
Profile
Site Admin
Site Admin
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3654
Location: Rotterdam, The Netherlands
Post Re: EV3 Balanc3r code
Yup! It's an easy fix! Just enable Natural Language:
Attachment:
Screenshot at 15-22-23.png
Screenshot at 15-22-23.png [ 28.62 KiB | Viewed 18091 times ]

After that it should be a smooth compile :)

= Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Fri Oct 31, 2014 10:23 am
Profile WWW
Rookie

Joined: Tue Oct 28, 2014 9:17 am
Posts: 7
Post Re: EV3 Balanc3r code
Thank you for pointing that out, Xander, I didn't even realize that I had this option enabled... Is there any pragma or other I can set in the file to specify that I am using this option? Or should I simply "port" the program to the text-based commands as given in http://help.robotc.net/WebHelpMindstorms/index.htm?


Fri Oct 31, 2014 10:42 am
Profile
Site Admin
Site Admin
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3654
Location: Rotterdam, The Netherlands
Post Re: EV3 Balanc3r code
You could chose not to use those functions. I can quickly convert it for you, if you'd like and post the code here. Shouldn't take more than about 10 minutes, or so :)

= Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Fri Oct 31, 2014 10:47 am
Profile WWW
Site Admin
Site Admin
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3654
Location: Rotterdam, The Netherlands
Post Re: EV3 Balanc3r code
This should compile without NL enabled.
Code:
#pragma config(Sensor, S2,     gyroSensor,     sensorEV3_Gyro, modeEV3Gyro_Rate)
#pragma config(Sensor, S4,     remoteSensor,   sensorEV3_IRSensor, modeEV3IR_Remote)
#pragma config(Motor,  motorA,          rightMotor,    tmotorEV3_Large, openLoop, driveRight, encoder)
#pragma config(Motor,  motorB,           ,             tmotorEV3_Large, openLoop)
#pragma config(Motor,  motorC,          leftMotor,     tmotorEV3_Large, openLoop, driveLeft, encoder)
#pragma config(Motor,  motorD,           ,             tmotorEV3_Large, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Pragmas for showing different debug windows
//#pragma DebuggerWindows("Locals")
//#pragma DebuggerWindows("Globals")
#pragma DebuggerWindows("DebugStream")
// #pragma DebuggerWindows("EV3LCDScreen")


// Maximum number of entries stored in encoder values array, used for computing speed over some dt's
#define MAX_ENCODER_VALUES ( 7 )
// Enable history?
//#define ENABLE_HISTORY
// History size for debugging
#define HISTORY_SIZE ( 300 )
// Power limit (+/- POWER_LIMIT) for motors
// For debugging this can be set to a low value (< 100)
#define POWER_LIMIT ( 100 )


// We use a lot of global variables
// Not very nice...
// TODO clean up at some point in time...

// Requested sample time in milliseconds
int sampleTime = 22;
// With a sampleTime = 22 and basic balancing, the T1 timer samples
// shows an average of 23.5 ms actual sample time, +/- around 2 ms
// This needs to be taken into account when calculating dt below
// Sample time difference is dt * 1000 - sampleTime, as calculated based on T1 samples
// In milliseconds
// The original Balanc3r code had -2 implicitly here
#define SAMPLE_TIME_DIFFERENCE ( 1.5 )
// Sample time, converted to seconds and corrected for delays in loop
float dt = ( 1.0 * sampleTime + SAMPLE_TIME_DIFFERENCE ) / 1000.0;

// Wheel diameter in mm
short wheelDiameterInMm = 42.0;
// Convert wheel diameter to radius in meters
float wheelRadius = wheelDiameterInMm / 2000.0;

// Encoder index for indexing encoder values array for motor speed computation
int encoderValueIndex = 0;
// Running encoder value for the last MAX_ENCODER_VALUES epochs
// Used for computing the average speed over MAX_ENCODER_VALUES epochs
long encoderValues[ MAX_ENCODER_VALUES ];

// Angle (integrated from angular rate) in degrees
float robotAngle = 0.0;
// Current of gyro rate bias in deg/s
float gyroRateBias;
// Currently measured gyro rate in deg/s
float gyroRate;

//
// PID constants
//
// P constant in TODO unit, original Balanc3r value 0.6
float kp = 0.6;
// I constant in TODO unit, original Balanc3r value 14, changed due to another dt
float ki = 12.2;
// D constant in TODO unit, original Balanc3r value 0.005, changed due to another dt
float kd = 0.0058;

//
// Sensor "fusion" constants
//
// Gain for sensor fusion for angular velocity in TODO unit, original Balanc3r value 1.3
float gainAngularVelocity = 1.3;
// Gain for sensor fusion for angle TODO unit, original Balanc3r value 25, changed due to another dt
float gainAngle = 21.7;
// Gain for sensor fusion for motor speed TODO units, , original Balanc3r value 75, changed due to another dt
float gainRobotSpeed = 86.3;
// Gain for sensor fusion for motor position TODO units, baseline 350
float gainRobotPosition = 350.0;

//
// Current speed, position and power
//
// Robot speed in m/s
float robotSpeed = 0.0;
// Robot position in m
float robotPosition = 0.0;

// Current power
int rightPower, leftPower;

//
// Error stuff
//
// Out-of-bounds variables and counter
bool NowOutOfBound = false;
bool PrevOutOfBound = false;
int OutOfBoundCount = 0;

//
// Debugging
//

#ifdef ENABLE_HISTORY

// Time history
int timeHist[ HISTORY_SIZE ];
// Angle history in deg
float angleHist[ HISTORY_SIZE ];
// Mean angular rate history in deg/s
float meanAngularRateHist[ HISTORY_SIZE ];
// Current angular rate history in deg/s
float angularRateHist[ HISTORY_SIZE ];
// Robot speed history in TODO units
float robotSpeedHist[ HISTORY_SIZE ];
// Robot position history in meters
float robotPositionHist[ HISTORY_SIZE ];
// Combined sensor history in TODO units
float combinedSensorsHist[ HISTORY_SIZE ];
// PID output history in TODO units
float pidOutputHist[ HISTORY_SIZE ];
// Storing of past motor powers
int pwrHist[ HISTORY_SIZE ];

#endif



/**
* Get gyro rate based on a single reading
*/
float getGyroRate()
{
   // Originally an average was used in Balanc3r, but a single reading actually seems to be more stable
   return getGyroRate( gyroSensor );
}


/**
* Calibration of gyro
*/
float calibrate()
{
   int numberOfReadings = 100;
   playSound( soundBeepBeep );
   writeDebugStreamLine( "Resetting gyro, keep robot still" );
   resetGyro( gyroSensor );
   // This sleep is very important!
   // If not used, the gyro readings will be way off afterwards
   sleep( 3000 );

   gyroRateBias = 0.0;
   // Make a few test readings prior to actual calibration
   for( int i = 0; i < numberOfReadings; i++ ) {
      gyroRateBias += getGyroRate();
   }
   gyroRateBias /= numberOfReadings;
   writeDebugStreamLine("Initial gyroRateBias (not used): %f", gyroRateBias );
   sleep( 500 );
   gyroRateBias = 0.0;
   sleep(100);
   for( int i = 0; i < numberOfReadings; i++ ) {
      gyroRateBias += getGyroRate();
   }
   gyroRateBias /= numberOfReadings;
   writeDebugStreamLine("Second gyroRateBias (not used): %f", gyroRateBias );
   sleep(500);
   gyroRateBias = 0.0;
   // And now do the actual calibration
   for( int i = 0; i < numberOfReadings; i++ ) {
      gyroRateBias += getGyroRate();
   }
   gyroRateBias /= numberOfReadings;
   writeDebugStreamLine("Third gyroRateBias (used): %f", gyroRateBias );
   playSound( soundDownwardTones );
   sleep( 100 );
   return gyroRateBias;
}


/**
* Initialize
*/
void initialize()
{
   writeDebugStreamLine("dt %f", dt );
   for ( int i = 0; i < MAX_ENCODER_VALUES; i++ ) encoderValues[ i ] = 0;
   resetMotorEncoder( rightMotor );
   resetMotorEncoder( leftMotor );
   sleep( 100 );
   gyroRateBias = calibrate();
}


/**
* Propagates reference position based on wanted speed and time step.
* Returns updated referencePosition
*/
float position( float lastReferencePosition, float requestedSpeed )
{
   // TODO why 0.002?
   //   return lastReferencePosition + requestedSpeed * dt * 0.002;
   return lastReferencePosition + requestedSpeed * dt;
}


/**
* Returns the motor speed in deg/s
* based on the last MAX_ENCODER_VALUES averages of encodervalues for left/right encoders
*/
float myGetMotorSpeed()
{
   encoderValueIndex++;
   if ( encoderValueIndex == MAX_ENCODER_VALUES ) encoderValueIndex = 0;
   int compare_index = encoderValueIndex + 1;
   if ( compare_index == MAX_ENCODER_VALUES ) compare_index = 0;
   long rightEncoderValue = getMotorEncoder( rightMotor );
   long leftEncoderValue = getMotorEncoder( leftMotor );
   long avgEncoderValue = ( rightEncoderValue + leftEncoderValue ) / 2;
   encoderValues[ encoderValueIndex ] = avgEncoderValue;
   float tmp = ( encoderValues[ encoderValueIndex ] - encoderValues[ compare_index ] ) / ( dt * MAX_ENCODER_VALUES );
   return tmp;
}


/**
* Read encoders and determine current robot speed and robot position in m/s and m.
*/
void readEncoders()
{
   robotSpeed = wheelRadius * myGetMotorSpeed() / 57.3; // 57.3 = 180/pi, needed for converting deg->rad
   // TODO Here we read the encoders again; this can be optimized away as we have just read them above...
   long rightEncoderValue = getMotorEncoder( rightMotor );
   long leftEncoderValue = getMotorEncoder( leftMotor );
   long avgEncoderValue = ( rightEncoderValue + leftEncoderValue ) / 2;
   robotPosition = wheelRadius * avgEncoderValue / 57.3;
}


/**
* Reads gyro and estimates angle and angular velocity TODO units?
*/
void readGyro()
{
   // Estimate of the current robotAngleBias due to drifting gyro
   static float robotAngleBias = 0.0;
   // This seems to be rather high. Corresponds to a full new update every ~100-300 ms (for dt = 20 ms and gyroRateBiasUpdateRatio = 0.2)...
   float gyroRateBiasUpdateRatio = 0.2;
   // Update ratio of robotAngleBias; should probably be way lower than gyroRateBiasUpdateRatio
   float robotAngleBiasUpdateRatio = 0.0;
   float currentGyroRateMeasurement = getGyroRate();
   // Estimate gyroRateBias rate by updating the value slowly
   // By multiplying with dt we make scale meanUpdateRatio to 1 sec
   gyroRateBias = gyroRateBias * ( 1 - dt * gyroRateBiasUpdateRatio ) +
   currentGyroRateMeasurement * dt * gyroRateBiasUpdateRatio;
   // Angular velocity can be computed directly now
   gyroRate = currentGyroRateMeasurement - gyroRateBias;
   // We note that the above update is "slow" compared to sample rate; thus
   // for drifting values of the gyro rate bias, the esimated value will always lack behind.
   // This again means that the integrated gyro rate (the angle of the robot) will drift.
   // In principle this is not a problem as the combined sensor value also includes the robot position
   // and the robot position can then make up for the drift (and this can be seen as the robot
   // slowly drifts in its position).
   // On the other hand we would like to have a good estimate of the angle.
   // In order to correct this we use the robot position over time to correct the angle
   // TODO we need a better way to do this; using the encoders really only works for a stationary robot
   //      In any case, we have set robotAngleBiasUpdateRatio = 0.0 , so this part is not used at all
   //      But this also means that the robotAngle eventually drifts a looong way from 0, thus triggering
   //      an error scenario at some point in time
   robotAngleBias = robotAngleBias * ( 1 - dt * robotAngleBiasUpdateRatio ) -
   ( robotPosition * gainRobotPosition / gainAngle ) * dt * robotAngleBiasUpdateRatio;
   // And angle is computed based on integration from last epoch
   robotAngle += gyroRate * dt - robotAngleBias;
}


/**
* Combines the sensor values ("sensor fusion") in order to have a single error estimator
* for feeding into the PID controller
*/
float combineSensorValues(
float angularVelocity,
float robotAngle,
float robotSpeed,
float robotPosition,
float robotReferencePosition )
{
   return
   (  robotPosition - robotReferencePosition ) * gainRobotPosition +
   robotSpeed * gainRobotSpeed +
   robotAngle * gainAngle +
   angularVelocity * gainAngularVelocity;
}


/**
* PID controller
*/
float PID( float input, float reference )
{
   // Integration part
   static float accumulatedError = 0.0;
   // Differentiation part
   static float diffError = 0.0;
   static float previousError = 0.0;
   float currentError = input - reference;
   accumulatedError += currentError * dt;
   diffError = ( currentError - previousError ) / dt;
   previousError = currentError;
   return kp * currentError + accumulatedError * ki + diffError * kd;
}


/**
* Detect erroneous pidOutput
*/
void errors( float pidOutput )
{
   if ( abs( pidOutput ) > 100 ) NowOutOfBound = true;
   if ( NowOutOfBound && PrevOutOfBound ) {
      OutOfBoundCount++;
      } else {
      OutOfBoundCount = 0;
   }
   if ( OutOfBoundCount > 20 ) {
      setMotorSpeed( rightMotor, 0 );
      setMotorSpeed( leftMotor, 0 );
      eraseDisplay();
      displayCenteredBigTextLine( 2, "ERROR" );
      sleep( 100 );
   }
}


/**
* Get steering
* TODO Currently hardcoded, should be set based on IR receiver or other
*/
float getSteer()
{
   return 0.0;
}


/**
* Sets motor power and corrects for steering and motors not being fully in sync
*/
void setMotorPower( float requestedSteering, float averagePower )
{
   //float sync_0;
   float extra_pwr = 0.0;
   static float old_steering = 0.0;
   static float pwr_c = 0.0;
   static float pwr_b = 0.0;

   // TODO update the below once steering is implemented
   //if ( requestedSteering < -50 ) requestedSteering = -50;
   //if ( requestedSteering > 50 ) requestedSteering = 50;
   //if ( requestedSteering == 0 ) {
   //   if ( old_steering == 0 ) {
   //      // Do nothing
   //   } else {
   //     // TODO again we should probably just use the value we have read twice before here..
   //      sync_0 = getMotorEncoder( rightMotor ) - getMotorEncoder( leftMotor );
   //   }
   //   // TODO and again we are reading it...
   //   // TODO 0.05 why?
   //   extra_pwr = ( getMotorEncoder( leftMotor ) - getMotorEncoder( rightMotor ) - sync_0 ) * 0.05;
   //} else {
   //   extra_pwr = -0.5 * requestedSteering;
   //}
   // TODO: extra_pwr set to 0, hardcoded...
   extra_pwr = 0.0;
   pwr_c = averagePower - extra_pwr;
   pwr_b = averagePower + extra_pwr;
   old_steering = requestedSteering;
   // TODO 0.021/wheelRadius???
   rightPower = pwr_b; // * 0.021 / wheelRadius;
   leftPower = pwr_c; // * 0.021 / wheelRadius;
   if ( rightPower > POWER_LIMIT ) rightPower = POWER_LIMIT;
   if ( rightPower < -POWER_LIMIT ) rightPower = -POWER_LIMIT;
   if ( leftPower > POWER_LIMIT ) leftPower = POWER_LIMIT;
   if ( leftPower < -POWER_LIMIT ) leftPower = -POWER_LIMIT;
   setMotorSpeed( rightMotor, rightPower );
   setMotorSpeed( leftMotor, leftPower );
}


/**
* Main program
*/
task main()
{
   // Counter for counting main loop entries
   // Used for keeping track of stored history and checking elapsed time versus set loop time (dt)
   int counter = 0;
   // Reference position in meters
   float referencePosition = 0.0;
   // Requested speed in m/s
   float requestedSpeed = 0.00;

   // Combined sensor value, computed based on angle, angular rate, speed and position
   float combinedSensors;

   // PID reference in motor power (-100 to 100)
   float pidReference = 0.0;
   // PID output, as computed by PID controller, in motor power (-100 to 100)
   float pidOutput;
   // Number of times the control loop did not meet its deadline (TODO is this actually working? maybe use counter*dt compared to T1 instead?)
   int controlLoopTimerOverflow = 0;

   initialize();

   // Balance loop
   eraseDisplay();
   displayCenteredBigTextLine( 2, "Balancing" );
   writeDebugStreamLine("*** Starting main loop");
   clearTimer( T1 );
   clearTimer( T2 );
#ifdef ENABLE_HISTORY
   repeatUntil( ( getTimer( T1, seconds ) > 20 ) || ( abs( robotAngle ) > 60 ) || ( OutOfBoundCount > 20 ) || ( counter >= HISTORY_SIZE ) ) {
#else
      repeatUntil( ( abs( robotAngle ) > 60 ) || ( OutOfBoundCount > 20 ) ) {
#endif
         // Get expected position based on current position and requested speed
         referencePosition = position( referencePosition, requestedSpeed );
         // Read the encoders and compute robot position and speed at the same time
         readEncoders();
         // Read the gyro and update gyroRateBias and robot angle
         readGyro();
         // Sensor "fusion"
         combinedSensors = combineSensorValues( gyroRate, robotAngle, robotSpeed, robotPosition, referencePosition );
         // PID control
         pidOutput = PID( combinedSensors, pidReference );
         // Any errors?
         errors( pidOutput );
         // Set the motor power
         setMotorPower( getSteer(), pidOutput );

#ifdef ENABLE_HISTORY
         // Save history
         timeHist[ counter ] = getTimer( T1, milliseconds );
         angleHist[ counter ] = robotAngle;
         meanAngularRateHist[ counter ] = gyroRateBias;
         angularRateHist[ counter ] = gyroRate;
         robotSpeedHist[ counter ] = robotSpeed;
         robotPositionHist[ counter ] = robotPosition;
         combinedSensorsHist[ counter ] = combinedSensors;
         pidOutputHist[ counter ] = pidOutput;
         pwrHist[ counter ] = rightPower;
#endif

         counter++;

         // Write out some statistics from time to time
         //if ( ( counter & 0xFF ) == 0xFF ) {
         //  writeDebugStreamLine("%f;%f;%f;%f;%f;%f;%f",
         //     kp, ki, kd,
         //     gainAngularVelocity, gainAngle, gainRobotSpeed, gainRobotPosition );
         //    writeDebugStreamLine("%d;%f;%f;%f;%f;%f;%f;%f;%d",
         //       getTimer( T1, milliseconds ),
         //       robotAngle,
         //       gyroRateBias,
         //       gyroRate,
         //       robotSpeed,
         //       robotPosition,
         //       combinedSensors,
         //       pidOutput,
         //       rightPower );
         //}

         if ( time1( T2 ) > sampleTime )
         {
            controlLoopTimerOverflow++;
         }
         repeatUntil( time1( T2 ) > sampleTime  ) {}
         clearTimer( T2 );
      }

      // Post-mortem statistics
      displayCenteredBigTextLine( 2, "OOB: %4d", OutOfBoundCount );
      displayCenteredBigTextLine( 4, "OVF: %4d", controlLoopTimerOverflow );
      displayCenteredBigTextLine( 6, "robotAngle: %4f", robotAngle );
      setMotorSpeed( rightMotor, 0 );
      setMotorSpeed( leftMotor, 0 );

#ifdef ENABLE_HISTORY
      // Writing out debug stuff
      // Sleeps are needed, otherwise written data is lost
      writeDebugStreamLine("# kp; ki; kd; gainAngVel; gainAngle; gainMotorSpeed; gainMotorPosition");
      sleep(50);
      writeDebugStreamLine("%f;%f;%f;%f;%f;%f;%f",
      kp, ki, kd,
      gainAngularVelocity, gainAngle, gainRobotSpeed, gainRobotPosition );
      sleep(50);
      writeDebugStreamLine("# time; angle; gyroRateBias; angularRate; robotSpeed; robotPosition; combinedSensors; pidOutput; pwr");
      sleep(50);
      for( counter = 0; counter < HISTORY_SIZE; counter++ ) {
         writeDebugStreamLine("%d;%f;%f;%f;%f;%f;%f;%f;%d",
         timeHist[ counter ],
         angleHist[ counter ],
         meanAngularRateHist[ counter ],
         angularRateHist[ counter ],
         robotSpeedHist[ counter ],
         robotPositionHist[ counter ],
         combinedSensorsHist[ counter ],
         pidOutputHist[ counter ],
         pwrHist[ counter ] );
         sleep( 50 );
      }
#endif

      sleep(30000);

   }

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Fri Oct 31, 2014 10:55 am
Profile WWW
Rookie

Joined: Tue Oct 28, 2014 9:17 am
Posts: 7
Post Re: EV3 Balanc3r code
Cool, great job! :-) I'll correct my code once I have tested it...


Fri Oct 31, 2014 11:42 am
Profile
Rookie

Joined: Tue Oct 28, 2014 9:17 am
Posts: 7
Post Re: EV3 Balanc3r code
I think I just leave it as is; although I have corrected all other natural language commands, the use of getTimer() instead of the time1, time10 and time100 variables is really convenient. I could of course wrap them into my own definitions, but that seems like a stupid idea as they are already implemented in the natural language...


Sat Nov 01, 2014 3:13 am
Profile
Rookie

Joined: Tue Aug 19, 2014 12:15 pm
Posts: 37
Post Re: EV3 Balanc3r code
The program compiled! Thanks Ronald


Sat Nov 01, 2014 12:07 pm
Profile
Rookie

Joined: Tue Aug 19, 2014 12:15 pm
Posts: 37
Post Re: EV3 Balanc3r code
Left off mightor name.


Sat Nov 01, 2014 12:08 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 11 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.