Skip to content

Instantly share code, notes, and snippets.

@teos0009

teos0009/gist:e0d8f1ac0d39146ef8a8

Created November 17, 2014 01:04
Show Gist options
  • Save teos0009/e0d8f1ac0d39146ef8a8 to your computer and use it in GitHub Desktop.
Save teos0009/e0d8f1ac0d39146ef8a8 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
gistfile1.cpp
//**************************************************************************
//* Self-balancing Chopper project *
//* *
//* FOR Arduino MEGA1280 8.11.14 *
//* Pin 20 is SDA and 21 is SCL on a Mega 1280 *
//* *
//* John Dingley *
//* Clearly the part of this code that reads the digital IMU *
//* is based on the Varaseno FreeSix IMU library *
//* *
//**************************************************************************
//source:
//http://www.instructables.com/id/Raleigh-Chopper-inspired-self-balancing-scooter/
//https://sites.google.com/site/onewheeledselfbalancing/wiring-the-micro-to-the-osmc-pin-arrangement
// Serial1.write is commands to Sabertooth and
// Serial2.print is text to LCD display.
#include <Wire.h>
#include <FreeSixIMU.h>
#include <FIMU_ADXL345.h>
#include <FIMU_ITG3200.h>
FreeSixIMU sixDOF = FreeSixIMU();
//NOTE: Set dip switches on the Sabertooth for simplified serial and 9600 Buadrate. Diagram of this on my Instructables page
//simplifierd serial limits for each motor
#define SABER_MOTOR1_FULL_FORWARD 127
#define SABER_MOTOR1_FULL_REVERSE 1
#define SABER_MOTOR2_FULL_FORWARD 255
#define SABER_MOTOR2_FULL_REVERSE 128
//motor level to send when issuing full stop command
#define SABER_ALL_STOP 0
int d;
const int AvgAngles = 3;
float prevTargetAngle = 0;
float targetAngle = 0;
float angles[5];
int rawvalues[6];
float currAngle, prevAngle;
float prevAngles[AvgAngles];
int prevAngleI = 0;
//setup all variables. Terms and constants may have strange names but this software has been inspired by all sorts of bits and bobs done by other segway clone builders
//In particular, early on around 2008, by the work of Trevor Blackwell. It has evolved since then bit by bit.
// This is code that keeps loop time at 10ms per cycle of main program loop.
int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
// Main program loops 100 times per second reading the sensors each time and updating the "torque" command to be sent to the motor power controller i.e. the OSMC
// Reading the sensors and working out tilt angle from vertical only works if the program reliably takes 10ms per cycle so this code fixes it at 10ms.
//XXXXXXXXXXXXXXXXXXXXXXXXXXX USER ADJUSTABLE VARIABLES XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
//Increase the value of Start_Balance_Point to bring the initial balance point further backwards. This value adjusts the angle the scooter balances at when it thinks it is "level"
//Adjust it to suit your machine and riding position (or, alternatively mount IMU on rotating bracket and tilt it forward or back a tiny bit until it suits your riding position).
float Start_Balance_point = 5.0; //this is correct value for my machine, yours may be different.
//Starting values for the PID constants before they are adjusted by reading the 3 potentiometers
//If the 3 potentiometers are left at their mid-points then the values below are what machine will use for calculations.
float P_constant = 4.5; //previously 4.0 I always record the "previous" value that worked OK in case the change makes things worse and I want to go back to earlier value.
float D_constant = 0.5; //previously 0.4
float I_constant = 0.4; //previously 0.75
float overallgainstart = 0.1; //starting value before softstart. When machine balances at startup it is "soft" for a few seconds then tightens up to a value called overallgaintarget
//If you want you can change the value for overallgaintarget in the code (about 0.3 works best for this machine by trial and error).
float overallgaintarget = 0.3;
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX END OF USER ADJUSTABLE VARIABLES XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
signed char Motor1percent;
signed char Motor2percent;
signed char Motor1percentold;
signed char Motor2percentold;
float SteerValue = 512;
float SteerCorrect;
float steersum;
int Steer = 0;
float overallgain;
float level=0;
int firstloop;
int torquevalue;
int motortorque;
int motortorqueold;
int c;
int lcdnumber;
int n;
float gangleratedeg;
int adc1;
int adc4;
int cut = 100;
float gangleraterads;
float k1;
int k4;
float angle;
float anglerads;
float balance_torque;
float cur_speed;
float balancetrim;
int i;
int j;
int tipstart;
// Big red LED goes out when machine is ready for you to bring it level at startup and all sensors are zeroed.
int ledonePin = 12;
//Analog inputs
int forwardbackPin = 1; //analog voltage from a potentiometer I have connected to a "twist grip" on left handlebar.
//digital inputs
int deadmanPin = 4; // deadman switch pulls it LOW
int turnleftPin = 2;
int turnrightPin = 5;
void setup() // run once, when the sketch starts
{
//analogINPUTS
pinMode(forwardbackPin, INPUT);
//digital outputs
pinMode(ledonePin, OUTPUT);
//digital inputs
pinMode(deadmanPin, INPUT);
digitalWrite(deadmanPin, HIGH);
pinMode(turnleftPin, INPUT);
digitalWrite(turnleftPin, HIGH);
pinMode(turnrightPin, INPUT);
digitalWrite(turnrightPin, HIGH);
delay(100);
Serial.begin(9600);
delay(500);
Serial1.begin(9600);// Serial to Sabertooth
delay(500);
Serial2.begin(9600);
delay(500);
Serial2.write(12); //clears screen
delay(50);
Serial2.write(4); //puts off flashing cursor
delay(100);
Wire.begin();
delay(5);
sixDOF.init(); //Begin the IMU
delay(50);
Serial2.write(19); //backlight on
delay(100);
Serial2.print("Chopper controller");
delay(50);
Serial2.write(13); //newline
delay(50);
Serial2.print("J.Dingley 26/10/14");
delay(50);
Serial.println("Chopper controller");
delay(3000);
waituntillevel(); //tipstart etc
}
void sample_inputs() {
updateAngle();
k1 = analogRead(forwardbackPin); //read the potentiometer twist grip
balancetrim = (float)(0.8 * balancetrim) + (0.2 * (k1/180)); //range 0-5 ish output change divisor to change range
//balancetrim = 0; //temporarily set to zero until rest of code is working DELETE ONCE TWISTGRIP POT HAS BEEN FITTED
// IN DEGREES
if (balancetrim < -7) balancetrim = -7; //stops you going too far with this This value is "adjusted" by using the twistgrip potentiometer.
//As said above, not essential, wanted to try it out, you can use machine fine just by leaning forwards or back.
if (balancetrim > 7) balancetrim = 7; //stops you going too far the other way
if (currAngle < -72) currAngle = -72; //rejects silly values to stop it going berserk!
if (currAngle > 72) currAngle = 72;
gangleratedeg = (float)((currAngle - prevAngle) * (1000/lastLoopTime)); //lastloopptime is in millisec so we divide into 1000
if (gangleratedeg < -200) gangleratedeg = -200; //stops crazy values entering rest of the program
if (gangleratedeg > 200) gangleratedeg =200; //limits it to 200 degrees per second
if (firstloop == 1){
lastLoopTime = 10;
firstloop = 0;
gangleratedeg = 0;
cur_speed = 0;
}
gangleraterads = (float) (gangleratedeg * 0.017453); //convert to radians
angle = (float) currAngle + balancetrim - Start_Balance_point;
anglerads = (float) angle * 0.017453; //converting to radians again a historic scaling issue from past software
balance_torque = (float) (P_constant * anglerads) + (D_constant * gangleraterads); //power to motors (will be adjusted for each motor later to create any steering effects
//balance torque is motor control variable. It is what is required to make the thing balance only.
//the values of 4.5 and 0.5 for P and D (see top of sketch) came from Trevor Blackwell's segway clone experiments and were derived by good old trial and error
//I have also found them to be about right
//We set the torque proportionally to the actual angle of tilt (anglerads), and also proportional to the RATE of tipping over (ganglerate rads)
//the 4.5 and the 0.5 set the amount of each we use - play around with them if you want.
//Much more on all this, PID controlo etc on web. PID tuning is a whole black art in itself
cur_speed = (float) (cur_speed + (I_constant * balance_torque * 0.001 * lastLoopTime)) * 0.999;
if (cur_speed < -1){ cur_speed = -1;} //stop complete runaway of this accelerator term
if (cur_speed > 1){cur_speed = 1;}
//this is not truly the current speed. We do not know actual speed as we have no wheel rotation encoders. This is a type of "accelerator pedal" effect:
//this variable increases with each loop of the program IF board is deliberately held at an angle (by rider for example, in this case by leaning or twisting the twistgrip)
//So it means "if we are STILL tilted, speed up a bit" and it keeps accelerating as long as you hold it tilted.
//You do NOT need this to just balance, but to go up a slight incline for example you would need it: if board hits incline and then stops - if you hold it
//tilted for long eneough, it will eventually go up the slope (so long as motors powerfull enough and motor controller powerful enough)
//Why the 0.999 value? I got this from the SegWii project code - thanks!
//If you have built up a large cur_speed value and you tilt it back to come to a standstill, you will have to keep it tilted back even when you have come to rest
//i.e. machine will stop moving OK but will now not be level as you are tilting it back other way to counteract this large cur_speed value that has built up.
//The 0.999 means that if you bring machine level after a long period tilted forwards, the cur_speed value magically decays away to nothing and your machine
//is now not only stationary but also level, very useful!
//NOTE: We do not multiply the I_constant by the previous cur_speed as other software tends to, but by the previously derived balance_torque. This makes the cur_speed value
//ramp upwards quite fast if you lean machine forwards. This is why we have limits on it to stop it running away to some huge value.
//I found by accident this it works better this way when moving scooter away from a standstill, it really pulls away rapidly just like a 2 wheeled scooter,
//you lift up feet as it accelerates and put them on the footpegs.
if ((digitalRead(turnleftPin) == HIGH) && (digitalRead(turnrightPin) == HIGH)){ //i.e. you are NOT pressing the either steering button
//i.e. we do NOT want to steer but just want to go in a straight line
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX Deviation correction XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
SteerCorrect = 0; //blocks the direction stabiliser unless rate of turn exceeds -100 or +100 arbitrary lateral raw gyro values per sec
if ((rawvalues[5] > 100 || rawvalues[5] < -100) && (SteerValue == 512)) { //resists turning if turn rate exceeds 100 arbitrary values per sec and SteerValue has decayed back to 512 after a recent turn
SteerCorrect = (float) 0.1 * rawvalues[5]; //vary the 0.1 according to how much "resistance" to being nudged off course you want.
} // end of if rawvalues[5] etc
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX End of deviation correction XXXXXXXXXXXXXXXXXXXXXXXXXX
if (SteerValue > 512){SteerValue = (float)SteerValue - 0.4;} //if have just turned, SteerValue will be greater or less than 512.
if (SteerValue < 512){SteerValue = (float)SteerValue + 0.4;} //to keep movements smooth we want it to decay gradually back to 512 not a sudden step up or down
//SteerValue = 512;
SteerCorrect = 0; //blocks the direction stabiliser for now, will come back and work on this later
} //end of if joysteer in midzone i.e. we don't want to turn
else {
//i.e. we DO want to steer
//steer one way
if (digitalRead(turnleftPin) == LOW){
SteerValue = (float)SteerValue - 0.1;
}
//steer the other way
if (digitalRead(turnrightPin) == LOW){ //exact opposite of the above, turns you the other way.
SteerValue = (float)SteerValue + 0.1;
}
if (SteerValue < 362) {
SteerValue = 362; //limiting max rate of turning (512 is no turning)
}
if (SteerValue > 662) {
SteerValue = 662; //limiting max rate of turning
}
SteerCorrect = 0;
} //end of ELSE
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX END OF STEERING XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
//SteerValue = 512; //************COMMENT this out if you want steering to work
SteerCorrect = 0; //************COMMENT this out if you want auto stability in straight line to work
level = (float)(balance_torque + cur_speed) * overallgain;
level = (float)(level * 200); //changes it to a scale of about -100 to +100
} //end of sample inputs
void set_motor() {
unsigned char cSpeedVal_Motor1 = 0;
unsigned char cSpeedVal_Motor2 = 0;
Steer = (float) SteerValue - SteerCorrect; //at this point is on the 0-1023 scale
//SteerValue is either 512 for dead ahead or bigger/smaller if you are pressing steering joystick left or right
//SteerCorrect is the "adjustment" made by the yaw sensor that is there to "resist" sudden turns if one wheel hits a small object for example.
Steer = ((Steer - 512) * 0.19) + 0.5; //gets it down from 0-1023 (with 512 as the middle no-steer point) to -100 to +100 with 0 as the middle no-steer point on scale
//adding 0.5 means it gets rounded up or down, not just always down when converted from float to an integer
level = (float) level + 0.5; //add 0.5 before we cast it as integer (always rounds down) this trick makes it round up or down to correct ingere value, not just always down
level = (int)level; //add 0.5 so when float value is truncated (NOT rounded up/down) you get a rounded up or down integer value
Steer = (int)Steer; //add 0.5 so when float value is truncated (NOT rounded up/down) you get a rounded up or down integer value
if (level < -100) {level = -100;}
if (level > 100) {level = 100;}
//set motors using the simplified serial Sabertooth protocol (same for smaller 2 x 5 Watt or bigger 2 x 50 Sabertooth motor power controller by the way)
Motor1percent = (signed char) level + Steer;
Motor2percent = (signed char) level - Steer;
if (Motor1percent > 100) Motor1percent = 100;
if (Motor1percent < -100) Motor1percent = -100;
if (Motor2percent > 100) Motor2percent = 100;
if (Motor2percent < -100) Motor2percent = -100;
//if not pressing deadman button on hand controller - cut everything
if (digitalRead(deadmanPin) == HIGH) { //i.e. if you are NOT pressing the deadman pin which would pull it LOW
cut = cut - 1;
if (cut < 0){ cut = 0;}
}
if (digitalRead(deadmanPin) == LOW) { //i.e. you ARE pressing the deadman button
cut = cut + 1;
if (cut > 50){ cut = 50;} //if cut was 100 it would take 1 second before motors actually cut. I have set it here to half a second.
//Why do this? Beacuse if a bit of dirt on deadman button contacts then it would only have to disconnect for a moment and machine would crash to a stop.
//This way it will run OK if you let go accidentally for a (very) tiny moment or contacts dirty for example.
//Deadman button needs to be HIGH QUALITY as well, reduces faceplant risk, so this code quite useful to have.
}
if (cut == 0) { //if cut decays to zero over the half second allowed, then machine irreversibly stop and you have to power up again from scratch. For safety.
//If you fell off you would not want it running into your head for example or innocent bystanders.
level = 0;
//digitalWrite(osmcdisablePin, HIGH); //5V to osmc pin 4 will DISABLE the OSMC
motortorque = 0;
torquevalue = 0;
level = 0;
Steer = 0;
Motor1percent = 0;
Motor2percent = 0;
Serial.print(" YOU NEED TO"); // text
Serial.print("RESET FROM SCRATCH"); //text
Serial2.write(12); //clears screen
delay(50);
Serial2.write(19); //backlight on
delay(100);
Serial2.print("RESET FROM SCRATCH");
while(1) { //having cut power it now loops endlessly until reset
digitalWrite(ledonePin, HIGH);
delay(500);
digitalWrite(ledonePin, LOW);
delay(500);
cSpeedVal_Motor1 = map (Motor1percent,
-100,
100,
SABER_MOTOR1_FULL_REVERSE,
SABER_MOTOR1_FULL_FORWARD);
cSpeedVal_Motor2 = map (Motor2percent,
-100,
100,
SABER_MOTOR2_FULL_REVERSE,
SABER_MOTOR2_FULL_FORWARD);
Serial1.write (cSpeedVal_Motor1); // we just set Motor1percent to zero and Motor2percent to zero
Serial1.write (cSpeedVal_Motor2); // so now we send it to the Sabertooth to make it stop.
} // end of while 1
} //end of if cut == 0
//What is this???
//It stops sudden jolts of motors when crossing from tilt of -1 to 0 to +1 or the other way.
//Program loops at 100 cycles/sec. This code means that if Motor1percent for example is calculated as say 5%, BUT the previous one was 0%,
//then when angle of tilt is between -2 and +2 degrees, IT FORCES THE MOTORPERCENT TO ONLY GO UP OR DOWN IN INCREMENTS OF 1 (ONLY WHEN AROUND BALANCE POINT)
//I tried all sorts of things to achieve this and this is best solution I have found so far.
if (anglerads < 0.034906 && anglerads > -0.034906){ //i.e. we are within + or 2 degrees of tilt from zero balanced point
if (Motor1percent > Motor1percentold && ((Motor1percent - Motor1percentold) > 1)){Motor1percent = Motor1percentold + 1;}
if (Motor1percent < Motor1percentold && ((Motor1percentold - Motor1percent) > 1)){Motor1percent = Motor1percentold - 1;}
if (Motor2percent > Motor2percentold && ((Motor2percent - Motor2percentold) > 1)){Motor2percent = Motor2percentold + 1;}
if (Motor2percent < Motor2percentold && ((Motor2percentold - Motor2percent) > 1)){Motor2percent = Motor2percentold - 1;}
}
Motor1percentold = Motor1percent;
Motor2percentold = Motor2percent;
cSpeedVal_Motor1 = map (Motor1percent,
-100,
100,
SABER_MOTOR1_FULL_REVERSE,
SABER_MOTOR1_FULL_FORWARD);
cSpeedVal_Motor2 = map (Motor2percent,
-100,
100,
SABER_MOTOR2_FULL_REVERSE,
SABER_MOTOR2_FULL_FORWARD);
Serial1.write (cSpeedVal_Motor1);
Serial1.write (cSpeedVal_Motor2);
} // end of set motors
void loop () { //this is the main program loop
//XXXXXXXXXXXXXXXXXXXXX loop timing control keeps it at 100 cycles per second XXXXXXXXXXXXXXX
lastLoopUsefulTime = millis()-loopStartTime;
if (lastLoopUsefulTime < STD_LOOP_TIME) {
delay(STD_LOOP_TIME-lastLoopUsefulTime);
}
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
//XXXXXXXXXXXXXXXXXXXXXX end of loop timing control XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
sample_inputs();
set_motor();
if (d > 200){
lcddatasend(); //sends string of data to the "display" arduino
d = 0;
}
d = d + 1;
//XXXXXXXXXXXXXXXXXXXX softstart function: machine a bit squishy when you first bring it to balanced point,
//then ride becomes firmer over next few seconds as value for overallgain increases from starting value of 0.1 to about 0.3 (set by a potentiometer) we have here
if (overallgain < overallgaintarget) {
overallgain = (float)overallgain + 0.005;
}
if (overallgain > overallgaintarget) {overallgain = overallgaintarget;}
//XXXXXXXXXXXXXXX end of softstart code XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
} // end of main loop
void waituntillevel(){
tipstart = 0;
overallgain = 0;
cur_speed = 0;
level = 0;
balancetrim = 0;
Serial.println("When LED goes out slowly bring machine level");
Serial2.write(12); //clears screen
Serial2.print("When LED goes out");
delay(50);
Serial2.write(13); //newline
delay(50);
Serial2.print("SLOWLY bring level");
delay(100);
digitalWrite(ledonePin, HIGH);
delay(100);
//Tilt machine down with one end on floor and NOT MOVING. Turn it on.
for (i=0; i<200; i++) {
sample_inputs();
}
for (i=0; i<1000; i++) { //I did this to let various averaged variables like the values from the potentiometers settle down to nice steady values
//XXXXXXXXXXXXXXXXXXXXX TIMEKEEPER loop timing control keeps it at 100 cycles per second XXXXXXXXXXXXXXX
lastLoopUsefulTime = millis()-loopStartTime;
if (lastLoopUsefulTime < STD_LOOP_TIME) {
delay(STD_LOOP_TIME-lastLoopUsefulTime);
}
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
//XXXXXXXXXXXXXXXXXXXXXX end of loop timing control XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
sample_inputs();
}
digitalWrite(ledonePin, LOW);
//tiltstart routine now comes in. It is reading the angle from accelerometer. When you first tilt the board past the level point
//the self balancing algorithm will go "live". If it did not have this, it would fly across the room as you turned it on (tilted)!
while (tipstart < 5) { //don't know why I chose 5 but there we are
//XXXXXXXXXXXXXXXXXXXXX TIMEKEEPER loop timing control keeps it at 100 cycles per second XXXXXXXXXXXXXXX
lastLoopUsefulTime = millis()-loopStartTime;
if (lastLoopUsefulTime < STD_LOOP_TIME) {
delay(STD_LOOP_TIME-lastLoopUsefulTime);
}
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
//XXXXXXXXXXXXXXXXXXXXXX end of loop timing control XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
sample_inputs();
//when tilt angle from accel is within 1 degree above or below our "zero" tilt (i.e. balanced) value, the self-balancing will turn itself on.
if ((angle < -1) || (angle > 1)){
n = n + 1;
if (n == 100){ //every 100 cycles I update the LCD screen and display the tilt angle so you can see,
//as you bring it level for first time, when it is about to go live
Serial.print("Angle:");
Serial.println(angle);
Serial2.write(12); //clears screen
Serial2.print("Angle:");
Serial2.print(angle);
n = 0;
} // end of n counter makes it update display once per sec
tipstart = 0;
overallgain = 0;
cur_speed = 0;
level = 0;
balancetrim = 0;
}
else {
tipstart = 5;
}
} //end of while tipstart < 5
Serial.println("BALANCING");
Serial2.write(12); //clears screen
Serial2.print(" BALANCING");
overallgain = overallgainstart; //softstart value. Gain will now rise to final of about 0.3 set by the potentiometer at rate of 0.005 per program loop.
//i.e. it will go from 0.1 (overallgainstart value) to 0.3 over the first few seconds after tipstart has been activated
//so, ride will be mushy to begin with but less violent, then will tighten up over first few seconds after self-balancing has engaged as you brought it
//level for the first time.
angle = 0;
cur_speed = 0;
balancetrim = 0;
firstloop = 1;
//end of tiltstart code. If go beyond this point then machine is active
//main balance routine, just loops forever. Machine is just trying to stay level. You "trick" it into moving by leaning it forwards or backwards.
} // end of void waituntillevel
void updateAngle() {
sixDOF.getYawPitchRoll(angles);
sixDOF.getRawValues(rawvalues); //rawvalues[5] is the lateral gyroscope value as an integer
prevAngles[prevAngleI] = angles[1];
prevAngleI = (prevAngleI + 1) % AvgAngles;
float sum = 0;
for (int i = 0; i < AvgAngles; i++)
sum += prevAngles[i];
currAngle = sum / AvgAngles;
prevAngle = currAngle;
}
void lcddatasend(){
Serial2.write(12); //clears screen
Serial2.print("A:");
lcdnumber = (int)angle;
Serial2.print(lcdnumber);
//Serial2.write(13); //newline
Serial2.print(" M1:");
Serial2.print(Motor1percent);
Serial2.print(" M2:");
Serial2.print(Motor2percent);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment