//ARDUINO SIMPLIFIED SERIAL PROTOCOL TO SABERTOOTH V2
// MODIFIED FOR STEPHENS UNICYCLE ATEMPT
// FEB 20th 2012
// J DINGLEY
//note: yOU MUST POWER UP BOARD TILTED AND AT REST. IT WILL THEN READ THE TILT RATE GYRO AND USE WHATEVER READING IT GETS AS REPRESENTING
//A ZERO RATE OF TILTING (NOT THE ANGLE (read by the accel) - AS WE KNOW IT IS ALREADY TILTED, BUT RATE OF TILT - i.e. the bit the gyro is reading)
//IF YOU DO NOT POWER IT UP TILTED AND WAIT A FEW SECONDS, IT WILL GET THIS CALCULATION WRONG AND THIS WILL AFFECT HOW IT SUBSEQUENTLY WORKS
//SO, LEAN IT OVER, TURN ON POWER, COUNT TO 5, THEN PRESS IN YOUR DEADMAN KILL SWITCH (nothing should happen at this point) , THEN VERY SLOWLY TILT IT TO LEVEL POINT BY
//HOLDING ONE END OF BOARD IN YOUR HAND
//AS IT COMES LEVEL, THE "TILTSTART" CODE WILL KICK IN AND MACHINE WILL COME "ALIVE" WHEN IT IS ABOUT LEVEL and hopefully try to balance itself.
// HOLDING ONE END OF THE BOARD, SEE HOW IT REACTS IF YOU TILT ONE END DOWN OR UP GENTLY
//J. Dingley July31st For Arduino 328 Duemalinove or similar with a 3.3V accessory power output
//i.e. the current standard Arduino board.
//MAKE SURE ARDUINO IS PROPERLY POWERED BY A BATTERY, NOT JUST THE USB CABLE.
//*************ONLY EXTERNAL CONTROL TO THE ARDUINO IS NOW THE DEAD MAN BUTTON ON DIGITAL INPUT PIN 9 ********************
// HiRes Gyro voltage from Gyro to analog Pin 0
// LowRes Gyro voltage from same gyro to analog Pin 3
// Accelerometer voltage to analog Pin 4
//values to set overall gain, fine tune the balance poit etc are all fixed at present in the code to keep things simple.
//once you get it to just self-balance, the most important thing, everything else can be added back in gradually
#include <SoftwareSerial.h>
//******************** IMPORTANT ***************************
//Set dip switches on the Sabertooth for simplified serial and 9600 Buadrate. Diagram of this on my Instructables page//
//******************** IMPORTANT ***************************
//Digital pin 13 is serial transmit pin to sabertooth
#define SABER_TX_PIN 12
//Not used but still initialised, Digital pin 12 is serial receive from Sabertooth
#define SABER_RX_PIN 13
//set baudrate to match sabertooth dip settings
#define SABER_BAUDRATE 9600
//simplifierd serial limits for each motor
#define SABER_MOTOR1_FULL_FORWARD 255
#define SABER_MOTOR1_FULL_REVERSE 0
//motor level to send when issuing full stop command
#define SABER_ALL_STOP 127
SoftwareSerial SaberSerial(SABER_RX_PIN, SABER_TX_PIN );
void initSabertooth (void) {
//initialise software to communicate with sabertooth
pinMode ( SABER_TX_PIN, OUTPUT );
SaberSerial.begin( SABER_BAUDRATE );
//2 sec delay to let it settle
delay (2000);
SaberSerial.write(127); //kill motors when first switched on
}
//setup all variables. Terms may have strange names but this software has evolved from bits and bobs done by other segway clone builders
float targetoverallgain = 0.3; //THIS IS WHERE YOU SET AND ADJUST THE OVERALLGAIN
//xxxxxxxx code that keeps loop time at 10ms per cycle of main program loop xxxxxxxxxxxxxxx
int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
float level=0;
float aa = 0.005; //this means 0.5% of the accelerometer reading is fed into angle of tilt calculation with every loop of program (to correct the gyro).
//accel is sensitive to vibration which is why we effectively average it over time in this manner. You can increase aa if you want experiment.
//too high though and the board may become too vibration sensitive.
//float Steering;
//float SteerValue;
//float SteerCorrect;
//float steersum;
int Steer = 0;
float accraw;
float x_acc;
float accsum;
float x_accdeg;
float gyrosum;
float hiresgyrosum;
float g;
float s;
float t;
float gangleratedeg;
//float gangleratedeg2;
int adc1;
//int adc4;
int adc5;
int cut = 100;
float gangleraterads;
float ti = 2.2;
//float k1;
//int k2;
//int k3;
int k4;
//float k5;
float overallgain;
float gyroangledt;
float angle;
float anglerads;
float balance_torque;
float softstart;
float cur_speed;
float cycle_time = 0.01;
//float forwardback;
//float leftright;
//float Balance_point;
float balancetrim;
//int balleft;
//int balright;
float a0, a1, a2, a3, a4, a5, a6; //Sav Golay variables. The TOBB bot describes this neat filter for accelerometer called Savitsky Golay filter.
//More on this plus links on my website.
int i;
int j;
int tipstart;
signed char Motor1percent;
//ANALOG INPUTS. Wire up the IMU as in my Instructable and these inputs will be valid.
int accelPin = 4; //pin 4 is analog input pin 4. z-acc to analog pin 4
int gyroPin = 3; //pin 3 is analog balance gyro low-res input pin. Y-rate to analog pin 3
int hiresgyroPin = 0; //hi res balance gyro input is analog pin 0. Y-rate 4.5 to analog pin 0
//The IMU has a low res and a high res output for each gyro. Low res one used by software when tipping over fast and higher res one when tipping gently.
//DIGITAL INPUT
int deadmanbuttonPin = 9; // deadman button is digital input pin 9
//DIGITAL OUTPUT
int oscilloscopePin = 8; //oscilloscope pin is digital pin 8 so you can work out the program loop time (currently about 5.5millisec)
void setup() // run once, when the sketch starts
{
initSabertooth( );
//analogINPUTS
pinMode(accelPin, INPUT);
pinMode(gyroPin, INPUT);
//pinMode(steergyroPin, INPUT);
//pinMode(leftrightPin, INPUT);
// pinMode(forwardbackPin, INPUT);
pinMode(hiresgyroPin, INPUT);
//digital inputs
pinMode(deadmanbuttonPin, INPUT);
digitalWrite(deadmanbuttonPin, HIGH); //enables the Arduino pull-up resistor on the deadman pin. Deadman switch just connects it to GND when pressed (bringing it low)
//digital outputs
pinMode(oscilloscopePin, OUTPUT);
// Serial.begin(9600); // HARD wired Serial feedback to PC for debugging in Wiring
}
void sample_inputs() {
gyrosum = 0;
// steersum = 0;
hiresgyrosum = 0;
accraw = analogRead(accelPin); //read the accelerometer pin (0-1023)
//Take a set of 7 readings very fast
for (j=0; j<7; j++) {
adc1 = analogRead(gyroPin);
adc5 = analogRead(hiresgyroPin);
gyrosum = (float) gyrosum + adc1; //sum of the 7 readings
hiresgyrosum = (float)hiresgyrosum +adc5; //sum of the 7 readings
}
k4 = digitalRead(deadmanbuttonPin); //this is needed - if you let go the motors both stop for your own safety
//NOTE when not pressed the value is HIGH (i.e. 1) and when pressed it goes LOW (i.e. zero)
//NOTE: You can fine-trim the balance point of the board by altering the balue of "balancetrim" so long as IMU orientation is roughly correct
//I have used a 2 position switch to do thius but in this software version, THIS IS DISABLED IN THIS BALANCE-ONLY SOFTWARE.
//You can however still alter the balance point by altering the value of "balancetrim" in the code above,
//...the normal value is zero, alter it in small increments of about plus or minus 0.04 if you need to.
// if (k5 < 150){
// balancetrim = balancetrim - 0.04;
// }
// if (k5 > 700){
// balancetrim = balancetrim + 0.04;
// }
//balleft = digitalRead(balancepointleftPin);
//balright = digitalRead(balancepointrightPin);
//if (balleft == 1) balancetrim = balancetrim - 0.04; //if pressing balance point adjust switch then slowly alter the balancetrim variable by 0.04 per loop of the program
//while you are pressing the switch
//if (balright == 1) balancetrim = balancetrim + 0.04; //same again in other direction
// if (balancetrim < -30) balancetrim = -30; //stops you going too far with this
// if (balancetrim > 30) balancetrim = 30; //stops you going too far the other way
//END OF TEMPORARILY COMMENTED OUT BALANCE TRIM CODE
//ACCELEROMETER READINGS
// Savitsky Golay filter for accelerometer readings. It is better than a simple rolling average which is always out of date.
// SG filter looks at trend of last few readings, projects a curve into the future, then takes mean of whole lot, giving you a more "current" value - Neat!
// Lots of theory on this on net.
a0 = a1;
a1 = a2;
a2 = a3;
a3 = a4;
a4 = a5;
a5 = a6;
a6 = (float) accraw;
accsum = (float) ((-2*a0) + (3*a1) + (6*a2) + (7*a3) + (6*a4) + (3*a5) + (-2*a6))/21;
//accsum isnt really a "sum" (it used to be once),
//now it is the accelerometer value from the rolling SG filter on the 0-1023 scale
digitalWrite(oscilloscopePin, HIGH); //puts out signal to oscilloscope
//ACCELEROMETER notes for the 5 d of f Sparfun IMU I have used in my Instructable:
//300mV (0.3V) per G i.e. at 90 degree angle
//Supply 3.3V is OK from Arduino NOT the 5V supply. Modern Arduinos have a 3.3V power out for small peripherals like this.
//Midpoint is 1.58 Volts when supply to IMU is 3.3V i.e. 323 on the 0-1024 scale when read from a 0-5V Arduino analog input pin
//not the 512 we are all perhaps more used to with 5V powered accel and gyro systems.
//testing with voltmeter over 0-30 degree tilt range shows about 5.666mV per degree. Note: Should use the Sin to get angle i.e. trigonometry, but over our small
//tilt angles (0-30deg from the vertical) the raw value is very similar to the Sin so we dont bother calculating it.
// 1mv is 1024/5000 = 0.2048 steps on the 0-1023 scale so 5.666mV is 1.16 steps on the 0-1023 scale
//NOTE: FINE-TUNE THE VALUE OF 338 BELOW USING METHOD DESCRIBED IN THE IMU TESTER PROCEDURE It sets the balance point
x_accdeg = (float)((accsum - (338 + balancetrim))*(-0.862)); //approx 1.16 steps per degree so divide by 1.16 i.e. multiply by 0.862 (minus sign is due to positioning of the IMU)
// x_accdeg = (float)((accsum - 350) * 0.862); //approx 1.16 steps per degree so divide by 1.16 i.e. multiply by 0.862
if (x_accdeg < -72) x_accdeg = -72; //rejects silly values to stop it going berserk!
if (x_accdeg > 72) x_accdeg = 72;
//GYRO READING NOTES:
//Low resolution gyro output: 2mV per degree per sec up to 500deg per sec. 5V = 1024 units on 0-1023 scale, 1Volt = 204.8 units on this scale.
//2mV = 0.41 units = 1deg per sec
// Hi res gyro output pin(from the same gyro): 9.1mV per degree per sec up to 110deg per sec on hires input. 5V = 1024 units on 0-1023 scale, 1Volt = 204.8 units on this scale.
//9.1mV = 1.86 units = 1 deg per sec
//Low res gyro rate of tipping reading calculated first
gangleratedeg = (float)(((gyrosum/7) - g)*2.44); //divide by 0.41 for low res balance gyro i.e. multiply by 2.44
if (gangleratedeg < -450) gangleratedeg = -450; //stops crazy values entering rest of the program
if (gangleratedeg > 450) gangleratedeg = 450;
//..BUT...Hi res gyro ideally used to re-calculate the rate of tipping in degrees per sec, i.e. use to calculate gangleratedeg IF rate is less than 100 deg per sec
if (gangleratedeg < 100 && gangleratedeg > -100) {
gangleratedeg = (float)(((hiresgyrosum/7) - t)*0.538); //divide by 1.86 i.e. multiply by 0.538
if (gangleratedeg < -110) gangleratedeg = -110;
if (gangleratedeg > 110) gangleratedeg = 110;
}
digitalWrite(oscilloscopePin, LOW); //cuts signal to oscilloscope pin so we have one pulse on scope per cycle of the program so we can work out cycle time.
//Key calculations. Gyro measures rate of tilt gangleratedeg in degrees. We know time since last measurement is cycle_time (5.5ms) so can work out much we have tipped over since last measurement
//What is ti variable? Strictly it should be 1. However if you tilt board, then it moves along at an angle, then SLOWLY comes back to level point as it is moving along
//this suggests the gyro is slightly underestimating the rate of tilt and the accelerometer is correcting it (slowly as it is meant to).
//This is why, by trial and error, I have increased ti to 1.2 at start of program where I define my variables.
//experiment with this variable and see how it behaves. Temporarily reconfigure the overallgain potentiometer as an input to change ti and experiment with this variable
//potentiometer is useful for this sort of experiment. You can alter any variable on the fly by temporarily using the potentiometer to adjust it and see what effect it has
gyroangledt = (float) ti * cycle_time * gangleratedeg;
gangleraterads = (float) gangleratedeg * 0.017453; //convert to radians - just a scaling issue from history
//angle = (float) ((1-aa) * (angle + gyroangledt)) + (aa * x_accdeg);
angle = (float) ((1-aa) * (angle + gyroangledt)) + (aa * x_accdeg);
//aa allows us to feed a bit (0.5%) of the accelerometer data into the angle calculation
//so it slowly corrects the gyro (which drifts slowly with tinme remember). Accel sensitive to vibration though so aa does not want to be too large.
//this is why these boards do not work if an accel only is used. We use gyro to do short term tilt measurements because it is insensitive to vibration
//the video on my instructable shows the skateboard working fine over a brick cobbled surface - vibration +++ !
anglerads = (float) angle * 0.017453; //converting to radians again a historic scaling issue from past software
//debugging
//Serial.print("Angle = ");
//Serial.println(angle);
balance_torque = (float) (4.5 * anglerads) + (0.5 * gangleraterads); //power to motors (will be adjusted for each motor later to create any steering effects
//balance torque is motor control variable we would use even if we just ahd one motor. It is what is required to make the thing balance only.
//the values of 4.5 and 0.5 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 my website
// Serial.print("curspeed:");
// Serial.println(cur_speed);
cur_speed = (float) (cur_speed + (anglerads * 10 * cycle_time)) * 0.999;
//this is not 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)
//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 SeWii 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. board will stop moving OK but will now not be level as you are tiliting it back other way to counteract this large cur_speed value
//The 0.999 means that if you bring board level after a long period tilted forwards, the cur_speed value magically decays away to nothing and your board
//is now not only stationary but also level!
level = (float)(balance_torque + cur_speed) * overallgain;
//level = (float)balance_torque * overallgain; //TEMP You can omit cur speed term during testing while just getting it to initially balance if you want to
//avoids confusion
}
void set_motor() {
unsigned char cSpeedVal_Motor1 = 0;
level = level * 200; //changes it to a scale of about -100 to +100
//debugging
//Serial.print("level= ");
//Serial.println(level);
Steer = 0; //this "Steer" value is a value from your steering control system to turn left and right. It is either +ve or -ve in value.
//All steering is currently removed from this program.
//All input from the second gyro to resist sudden deviations from the dead ahead movement has also been removed from this program.
//IF BOARD DRIFTS TO LEFT OR RIGHT WHEN MOVING THIS IS UNAVOIDABLE UNTIL YOU ADD SOME SORT OF STEERING CONTROL,
//I.E. A METHOD OF VARYING THE VALUE OF "Steer" BACK INTO THE CODE.
//set motors using the simplified serial Sabertooth protocol (same for smaller 2 x 5 Watt Sabertooth by the way)
Motor1percent = (signed char) level + Steer;
//Motor1percent = (signed char) level;//temp
if (Motor1percent > 100) Motor1percent = 100;
if (Motor1percent < -100) Motor1percent = -100;
// if ((Motor1percent > 0) && (Motor1percent < 5)) Motor1percent = 5;
// if ((Motor1percent < 0) && (Motor1percent > -5)) Motor1percent = -5; // Set's minimum motor speed according to experiments +/- 5%
/*if not pressing deadman button on hand controller - cut everything for a moment
if (k4 == 1){
level = 0;
Steer = 0;
Motor1percent = 0;
}*/
/* if ((k4 == 1) && (tipstart == 5) && (cut > 0)){ //i.e. we ARENT pressing deadman (K4 goes HIGH i.e ==1) AND tipstart is active AND cut has not decayed to zero yet
cut = cut - 1;
if (cut < 0){ cut = 0;}
} //if not pressing deadman AND tipstart has started (i.e. you are balancing) then you get countdown of
//cut value from resting value of 100 to zero then it stops board dead forever, you have to restart the board.
//This stops a momentary lapse of pressing the deadman down just for a few milliseconds from cutting board out forever
//i.e motors cut irreversibly if you let go of deadman for more than 1 sec (as program loops 100 x per sec).*/
if (k4 == 0 && tipstart == 5 && cut > 0){ //i.e. deadman IS being pressed again (k4 goes low i.e. ==0) within approx 1 sec gap (before "cut" has decayed to zero)
//this strange effect is there to stop motors cutting if you let go of deadman just for a tiny moment OR dirty contacts on switch for example.
cut = cut + 1;
if (cut > 100){cut = 100; } //if press deadman again within 1 second then 1 added to cut value again once per cycle
//until value of cut is restored to its resting value of 100 BUT only if "cut" has never decreased all the way to zero from its resting value of 100
}
/*If deadman not pressed for 1 second (100 cycles of the program) then cut will decay to zero
// when cut reaches zero machine will stop PERMANENTLY until you restart it
if (cut == 0){
level = 0;
Steer = 0;
Motor1percent = 0;
while(1){ //loops forever to lock program until it has been reset
delay(2000);
}
} */
cSpeedVal_Motor1 = map (Motor1percent,
-100,
100,
SABER_MOTOR1_FULL_REVERSE,
SABER_MOTOR1_FULL_FORWARD);
SaberSerial.write (cSpeedVal_Motor1);
//debugging
//Serial.print("Motor1percent = ");
//Serial.print(Motor1percent);
//Serial.print (" level = ");
//Serial.println (level);
//Serial.print("cSpeedVal_Motor1 = ");
//Serial.println(cSpeedVal_Motor1);
}
void loop () {
tipstart = 0;
overallgain = 0;
cur_speed = 0;
level = 0;
Steer = 0;
balancetrim = 0;
//Tilt board one end on floor. Turn it on. This 200 loop creates a delay while you finish turning it on and let go i.e. stop wobbling it about
//as now the software will read the gyro values when there is no rotational movement to find the zero point for each gyro. I could have used a simple delay command.
for (i=0; i<200; i++) {
sample_inputs();
}
//now you have stepped away from baord having turned it on, we get 7 readings from each gyro (and a hires reading from the balance gyro)
g = (float) gyrosum/7; //gyro balance value when stationary i.e. 1.35V
// s = (float) steersum/7; //steer gyro value when stationary i.e. about 1.32V
t = (float) hiresgyrosum/7; //hiresgyro balance output when stationary i.e. about 1.38V
//we divide sum of the 7 readings by 7 to get mean for each
//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) {
for (i=0; i<10; i++) {
sample_inputs();
}
//x_accdeg is tilt angle from accelerometer in degrees
if (x_accdeg < -2 || x_accdeg > 2) {
tipstart = 0;
overallgain = 0;
cur_speed = 0;
level = 0;
Steer = 0;
balancetrim = 0;
}
else {
tipstart = 5;
}
Serial.print("xaccdeg:");
Serial.println(x_accdeg);
}
overallgain = targetoverallgain/2; //THIS JUST SETS IT THE FIRST TIME should be lower than final target overallgain
//It will gradually increase to target overallgain over a few sec once tipstart has become active
// This gives you a "softstart" when tipstart first goes active.
angle = 0;
cur_speed = 0;
balancetrim = 0;
//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 tilting one end down
//works best if keep legs stiff so you are more rigid like a broom handle is if you are balancing it vertically on end of your finger
//if you are all wobbly, the board will go crazy trying to correct your own flexibility.
//NB: This is why a segway has to have vertical handlebar otherwise ankle joint flexibility in fore-aft direction would make it oscillate wildly.
//NB: This is why the handlebar-less version of Toyota Winglet still has a vertical section you jam between your knees.
while (1) {
sample_inputs();
set_motor();
//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
//XXXXXXXXXXXXXXXXXXXX softstart function: board a bit squishy when you first bring it to balanced point, it starts out at 0.3 remember, then ride becomes firmer over next 4 seconds XXXXXXXXXXXXXXX
if (overallgain < targetoverallgain) {
overallgain = (float)overallgain + 0.0002;
}
if (overallgain > targetoverallgain) {overallgain = targetoverallgain;}
//XXXXXXXXXXXXXXX end of softstart code XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
}
}