//************************************************************************** //* Self-balancing Chopper project * //* V2 code with direction stabiliser * //* FOR Arduino MEGA1280 01.01.15 * //* 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 Fabio Varesano FreeSix IMU library. * //* * //* This version uses the horizontal plane gyro to counteract * //* suddden turns when wanting to go in a straight line, * //* i.e. if one wheel hits a small stone for example. * //* Think of it as a direction stabiliser when going in a straight line. * //* It will not keep you going perfectly straight but it will try to * //* stop you being spun off if one wheel hits something. * //* * //* Also will lean back 5 degrees if you press both steering levers * //* at the same time - a kind of manual emergency brake, (in addition * //* to twisting the twistgrip backwards). * //* * //* Also the steering turn rate is set as an angular rate, monitored * //* by a gyro. Turn rate can be altered in the code. I may make it * //* speed-sensitive (slower when going fast). * //************************************************************************** // Serial1.write is commands to Sabertooth and // Serial2.print is text to LCD display. #include #include #include #include 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 // Reading the rate of tilting 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; int safetylimit = -70; //percentage power above which the safety leanback feature kicks in to limit top speed. -ve value as when moving forwards all values are -ve and when backwards all are +ve. //Needs to be set to less than 100% so it can apply more power to create the lean back effect, so has to have some power in reserve at all times. //To self-balance you always need some power held in reserve i.e. should never go up to 100% power else may just topple over from then on. //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; int rightleverstate; int leftleverstate; float overallgain; float level; float leanback; 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. // This is actually a relic from when I was using analog output IMU's. 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); // Serial to Serial LCD display delay(500); Serial2.write(12); //clears screen on LCD display delay(50); Serial2.write(4); //puts off flashing cursor delay(100); Wire.begin(); delay(5); sixDOF.init(); //Begin the IMU delay(1000); Serial2.write(19); //backlight on delay(2000); Serial2.print("Chopper controller"); delay(200); Serial2.write(13); //newline delay(200); Serial2.print("J.Dingley 01/01/15"); delay(200); Serial2.write(13); //newline delay(200); Serial2.print("With auto leanback"); delay(200); Serial2.write(13); //newline delay(200); Serial2.print("plus emergency brake"); delay(3000); waituntillevel(); //tipstart etc } void sample_inputs() { updateAngle(); //get the new tilt angle from kalman filter i.e. freesix IMU code // Read the controls k1 = analogRead(forwardbackPin); //read the potentiometer twist grip rightleverstate = digitalRead(turnrightPin); //read the lever operated switch being used to turn right leftleverstate = digitalRead(turnleftPin); //read the lever operated switch being used to turn left balancetrim = (float)(0.8 * balancetrim) + (0.2 * (k1/180)); //range 0-5 ish output change divisor to change range // XXXXXXXXXXXXXXXXXXX SAFETY LEABACK TO SLOW DOWN FEATURE XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX // Leans back 5 degrees if going too fast and/or if BOTH steering levers are pressed together, a kind of emergency overspeed and manual braking system // Leanback code which will make it lean back and so slow down if commanded motor power is over 80% of maximum or whatever the value of safetylimit is set to. // NOTE: when accelerating forwards, angle will be -ve and level and motorpercents will be -ve, when moving backwards they aill all be +ve, just to be confusing. if (level < safetylimit){leanback = (float)leanback + 0.02;} if (leftleverstate == LOW && rightleverstate == LOW){leanback = (float)leanback + 0.02;} //i.e. pressing both leversso want to lean back if (leftleverstate == HIGH && rightleverstate == HIGH && level >= safetylimit) {leanback = (float)leanback - 0.02; } //i.e. not pressing both levers and not going too fast EITHER then normalise the leanback to zero if (leanback < 0) { leanback = 0; } //stops crazy -ve value developing if (leanback > 5) {leanback = 5;} //limits the leanback to a max of 5 degrees backwards //XXXXXXXXXXXXXXXXXXXXXX end of leanback code XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX // IN DEGREES, CHECKING FOR CRAZY VALUES 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; //angular rate of tilting in degrees per second. 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 - leanback; 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. // STEERING SECTION PART 1: WE DO NOT WANT TO STEER LEFT OR RIGHT if ((leftleverstate == HIGH && rightleverstate == HIGH) || (leftleverstate == LOW && rightleverstate == LOW)){ //i.e. you are NOT pressing either steering lever OR you are pressing both levers together cos you want machine to lean back and slow down. //i.e. either way, 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 if ((SteerValue < 513) && (SteerValue > 511)) {SteerValue = 512;} // When has been returned to a value very close to 512, it restores it to exactly 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 { // STEERING SECTION, HERE WE WANT TO STEER LEFT if (leftleverstate == LOW && rightleverstate == HIGH){ //i.e. pulling one brake lever but not the other i.e. not pulling both together if (rawvalues[5] > 300) { //will turn clockwise at 50 arbitrary gyro units per sec and if not, more power fed into steering until it does SteerValue = (float)SteerValue + 0.4; } if (rawvalues[5] < 300) { SteerValue = (float)SteerValue - 0.4; } } // end of if turnleftPin is low // STEERING SECTION, HERE WE WANT TO STEER RIGHT if (rightleverstate == LOW && leftleverstate == HIGH){ //exact opposite of the above, turns you the other way. if (rawvalues[5] > -300) { //will turn counter clockwise at 50 arbitrary gyro units per sec and if not, more power fed into steering until it does SteerValue = (float)SteerValue + 0.4; } if (rawvalues[5] < -300) { SteerValue = (float)SteerValue - 0.4; } } // end of if turnrightPin is low 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 SECTION XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX level = (float)(balance_torque + cur_speed) * overallgain; level = (float)(level * 200); //changes it to a scale of about -100 to +100 ready to work out motor torque percentages } //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 = (float)((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 integer 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; //sanity checker 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 -2 to 0 to +2 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"); delay(200); Serial2.write(12); //clears screen delay(200); Serial2.print("When LED goes out"); delay(200); Serial2.write(13); //newline delay(200); Serial2.print("SLOWLY bring level"); delay(200); digitalWrite(ledonePin, HIGH); delay(200); //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 < 1) { //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; } // end of if angle <-1 or > 1 etc else { tipstart = 1; } } //end of while tipstart < 1 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(){ // Sends selected data to the LCD screen every now and then. // NOTE: If too much data sent here, the delay sending it all out makes the machine judder so keep it minimal, just what you want to monitor. Serial2.write(12); //clears screen Serial2.print("A:"); lcdnumber = (int)angle; Serial2.print(lcdnumber); Serial2.print(" M1:"); Serial2.print(Motor1percent); Serial2.print(" M2:"); Serial2.print(Motor2percent); }