//TWIN WHEELER MODIFIED FOR ARDUINO // ROCKER SWITCH CODE // SWITCHES ALL PULL THE PINS TO GND SO READ "LOW" WHEN PRESSED //GYRO NOTES on old Sparkfun 5dof analog IMU: //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 (Y4.5)(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 //ACCELEROMETER notes for the old 5 d of f analog IMU: //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 #include //Set dip switches on the Sabertooth for simplified serial and 9600 Buadrate. Diagram of this on my Instructables page// //Digital pin 12 is serial transmit pin to sabertooth #define SABER_TX_PIN 11 //Not used but still initialised, Digital pin 12 is serial receive from Sabertooth #define SABER_RX_PIN 12 //set baudrate to match sabertooth dip settings #define SABER_BAUDRATE 9600 //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 SoftwareSerial SaberSerial = SoftwareSerial (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.print(0, BYTE); //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 //xxx 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; //XXXXXXXXXXXXXXXXXXXXXXXXXXX USER ADJUSTALE VARIABLES XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX float gyroscalingfactor = 2.2; //strange constant. It is best thought of as the "balance gyro scaling factor" //Increase the value of Start_Balance_Point to bring the initial balance point further backwards float Start_Balance_point = -2.0; //ideally should be zero but often needs tweaking to get the board to come live just as it gets exactly level float P_constant = 4.5; //previously 4.0 float D_constant = 0.5; //previously 0.4 float I_constant = 1.0; //previously 2.5 float overallgaintarget = 0.7; //previously 0.5 float overallgainstart = 0.3; //starting value before softstart //XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX END OF USER ADJUSTABLE VARIABLES XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX float overallgain; 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; int firstloop; 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 k1; int k4; float k5; float gyroangledt; float angle; float anglerads; float balance_torque; //float softstart; float cur_speed; int balleft; int balright; float balancetrim; 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; signed char Motor2percent; //ANALOG IMU on ebay: http://www.ebay.com/itm/181004141876?ssPageName=STRK:MEWNX:IT&_trksid=p3984.m1439.l2649 //ANALOG INPUTS. Wire up the IMU to analog imputs on the Arduino. //Analog Pin on Arduino Pin on IMU board // 1 Zacc // 2 Y4.5 Higher resolution gyro output 2mV per degree per sec rotation // 3 X4.5 Higher resolution gyro output 2mV per degree per sec rotation int accelPin = 1; // z-acc to analog pin 1 int steergyroPin = 3; //steergyro analog input pin 2. The X4.5 gyro output connects to Pin 2. This spare gyro is used to allows software to "resist" sudden unplanned turns (i.e. when one wheel hits small object) int hiresgyroPin = 2; //hi res balance gyro input is analog pin 0. Y-rate 4.5 to analog pin 0 //DIGITAL OUTPUTS //DEFINE THE LEDS we are using as alarms etc int ledonePin = 6; int ledtwoPin = 7; int ledthreePin = 8; int ledfourPin = 9; //DIGITAL INPUTS int leftPin = 2;//left rocker switch contact on steering rocker switch int rightPin = 3;//as above but right rocler swicth contact. When pressed the digital input pin on the Arduino is pulled to ground GND, so reads LOW when pressed. int forwardPin = 1; // forward back rocker switch used not like a joystick but to fine-tune the balance point when level while you are riding the board int backPin = 0; // as above. It deliberately responds slowly int deadmanbuttonPin = 4; // deadman button, let go in a crisis and the power is cut permanently after a half second delay and you will have to start again. void setup() // run once, when the sketch starts { initSabertooth( ); //analogINPUTS pinMode(accelPin, INPUT); //pinMode(gyroPin, INPUT); pinMode(steergyroPin, INPUT); pinMode(hiresgyroPin, INPUT); //digital inputs pinMode(deadmanbuttonPin, INPUT); digitalWrite(deadmanbuttonPin, HIGH); //enables the Arduino internal pullup. when button pressed it connects it to ground giving a value of zero when pressed pinMode(leftPin, INPUT); digitalWrite(leftPin, HIGH); pinMode(rightPin, INPUT); digitalWrite(rightPin, HIGH); pinMode(forwardPin, INPUT); digitalWrite(forwardPin, HIGH); pinMode(backPin, INPUT); digitalWrite(backPin, HIGH); //digital outputs pinMode(ledonePin, OUTPUT); pinMode(ledtwoPin, OUTPUT); pinMode(ledthreePin, OUTPUT); pinMode(ledfourPin, 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 //Take a set of 7 readings very fast for (j=0; j<7; j++) { adc4 = analogRead(steergyroPin); adc5 = analogRead(hiresgyroPin); steersum = (float) (steersum + adc4); //sum of the 7 readings hiresgyrosum = (float) (hiresgyrosum + adc5); //sum of the 7 readings } k4 = digitalRead(deadmanbuttonPin); //1 when not pressed and 0 when is being pressed balleft = digitalRead(forwardPin); balright = digitalRead(backPin); if (balleft == 0) 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 == 0) 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 // 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 // *****Routine for using second gyro to help maintain a straight line when neither contact of steering rocker switch is being pressed*** gangleratedeg2 = (float)(((steersum/7) - s) / 1.86); //divide by 1.86 if using old sparkfun 5dof IMU Read the horizontal gyro to see if we are turning when we do not want to be turning if (gangleratedeg2 < -400) gangleratedeg2 = -400; //sanity check if (gangleratedeg2 >400) gangleratedeg2 = 400; // NO steering wanted. So we will use second gyro to maintain a (roughly) straight line heading (it will actually drift a bit). if ((digitalRead(rightPin) == HIGH) && (digitalRead(leftPin) == HIGH)) { //i.e. the steering rocker switch is in mid position so we do not want steering so neither contact is being pulled to GND so both read HIGH SteerCorrect = 0; //blocks the direction stabiliser unless rate of turn exceeds -10 or +10 degrees per sec if ((gangleratedeg2 > 10 || gangleratedeg2 < -10) && (SteerValue == 512)) { //resists turning if turn rate exceeds 10deg per sec SteerCorrect = (float) 0.4 * gangleratedeg2; //vary the 0.4 according to how much "resistance" to being nudged off course you want. //a value called SteerCorrect is added to the steering value proportional to the rate of unwanted turning. It keeps getting //larger if this condition is till being satisfied i.e. still turning >10deg per sec until the change has been resisted. //can experiment with the value of 10. Try 5 deg per sec if you want - play around - this can probably be improved //but if you try to turn it fast with your hand while balancing you will feel it resisting you so it does work //also, when coming to a stop, one motor has a bit more friction than the other so as this motor stops first then as board //comes to standstill it spins round and you can fall off. This is original reason I built in this feature. //if motors have same friction you will not notice it so much. } if (SteerValue > 512){SteerValue = (float)SteerValue - 0.5;} //if have just turned, SteerValue will be greater or less than 512. if (SteerValue < 512){SteerValue = (float)SteerValue + 0.5;} //to keep movements smooth we want it to decay gradually back to 512 not a sudden step up or down } else { //i.e. we DO want to steer //ROUTINE FOR STERRING LEFT //SteerValue of 512 is straight ahead if (digitalRead(leftPin) == LOW) { //pressing rocker switch left so pulling the leftsteer pin to GND so ir reads LOW SteerValue = (float)SteerValue + 0.5; } //change the 0.5 and -0.5 values if you want faster turn rates. Could use a potentiometer to control these values so would have proportional control of steering //ROUTINE FOR STEERING RIGHT if (digitalRead(rightPin) == LOW) { //pressing rocker switch right so pulling the rightsteer pin to GND so it reads LOW SteerValue = (float)SteerValue - 0.5; } 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; } //ACCELEROMETER notes for the old sparkfun analog 5 d of f IMU: //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 //***********ADJUST THE VALUE OF 338 FOR YOUR BOARD UNTIL IT BALANCES EXACTLY LEVEL WHEN TIPSTART KICKS IN *************** x_accdeg = (float)((accsum - (338 + balancetrim)) / 1.16); //empirically derived as above for my old 5V accels if (x_accdeg < -72) x_accdeg = -72; //rejects silly values to stop it going berserk! if (x_accdeg > 72) x_accdeg = 72; //GYRO NOTES on old Sparkfun 5dof analog IMU: //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 (Y4.5)(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 // Hi res gyro ideally used to calculate the rate of tipping in degrees per sec, i.e. use to calculate gangleratedeg IF rate is less than 100 deg per sec gangleratedeg = (float)(((hiresgyrosum/7) - t) / 1.86); //divide by 1.86 if (gangleratedeg < -110) gangleratedeg = -110; if (gangleratedeg > 110) gangleratedeg = 110; gyroangledt = (float) (gyroscalingfactor * lastLoopTime * 0.001 * gangleratedeg); gangleraterads = (float) (gangleratedeg * 0.017453); //convert to radians - just a scaling issue from history 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 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 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 //cur_speed = (float) (cur_speed + (anglerads * 10 * cycle_time)) * 0.999; cur_speed = (float) (cur_speed + (I_constant * anglerads * 0.001 * lastLoopTime)); //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) //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. 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 that has built up. //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, very useful! 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 } //end of sample inputs void set_motor() { unsigned char cSpeedVal_Motor1 = 0; unsigned char cSpeedVal_Motor2 = 0; level = level * 200; //changes it to a scale of about -100 to +100 if (level < -100) {level = -100;} if (level > 100) {level = 100;} 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 switch left or right //SteerCorrect is the "adjustment" made by the second horizontal gyro that resists sudden turns if one wheel hits a small object for example. Steer = (Steer - 512) * 0.19; //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 //set motors using the simplified serial Sabertooth protocol (same for smaller 2 x 5 Watt Sabertooth 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 (k4 == 1) { //is 1 when you ARENT pressing the deadman button cut = cut - 1; if (cut < 0){ cut = 0;} } if (k4 < 1){ //is zero when you ARE pressing deadman button cut = cut + 1; if (cut > 50){ cut = 50;} //if cut is 100 takes 1 second off the deadman before motors actually cut } if (cut == 0) { level = 0; Steer = 0; Motor1percent = 0; Motor2percent = 0; 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); SaberSerial.print (cSpeedVal_Motor1, BYTE); //enacts the command for zero power before we then enter the endless while loop SaberSerial.print (cSpeedVal_Motor2, BYTE); //enacts the command for zero power before we then enter the endless while loop //Serial2.write(12); //clears screen Serial.print(" YOU NEED TO"); //lcd text //Serial2.write(13); //newline //delay(50); Serial.println("RESET FROM SCRATCH"); //lcd text digitalWrite(ledonePin, LOW); digitalWrite(ledtwoPin, LOW); while(1) { //loops endlessly until reset delay(500); digitalWrite(ledthreePin, HIGH); digitalWrite(ledfourPin, HIGH); delay(500); digitalWrite(ledthreePin, LOW); digitalWrite(ledfourPin, LOW); } // end of while 1 } //end of if cut == 0 //cut is not 0 so we therefore enact the specified command to the motors 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); SaberSerial.print (cSpeedVal_Motor1, BYTE); SaberSerial.print (cSpeedVal_Motor2, BYTE); } void loop () { tipstart = 0; overallgain = 0; cur_speed = 0; level = 0; Steer = 0; balancetrim = 0; digitalWrite(ledonePin, HIGH); //Tilt board one end on floor. Turn it on. The 3 sec delay 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 200 times when there is no rotational movement to find the average zero point for each gyro. delay(3000); s = 0; t = 0; for (i=0; i<200; i++) { sample_inputs(); //g = (float) g + (gyrosum/7); //gyro balance value when stationary i.e. 1.35V s = (float) s + (steersum/7); //steer gyro value when stationary i.e. about 1.32V t = (float) t + (hiresgyrosum/7); //hiresgyro //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 } s = (float) (s / 200); t = (float) (t / 200); overallgain = 0; cur_speed = 0; level = 0; Steer = 0; balancetrim = 0; angle = 0; accraw = 0; x_acc = 0; accsum = 0; x_accdeg = 0; gyrosum = 0; hiresgyrosum = 0; gangleratedeg = 0; gangleratedeg2 = 0; digitalWrite(ledtwoPin, HIGH); for (i=0; i<2000; i++) { //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(); //serialOut_timing();//optional displays loop time on screen first digit is time loop takes to run in millisec, } Serial.println("Ready for you to slowly bring board level now"); digitalWrite(ledthreePin, HIGH); digitalWrite(ledfourPin, HIGH); //turn all the LEDs on ready for you to bring it level when 3 of them will then go out /* //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(); } */ 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(); //serialOut_timing();//optional displays loop time on screen first digit is time loop takes to run in millisec, if ((angle < (Start_Balance_point -1)) || (angle > (Start_Balance_point + 1))){ tipstart = 0; overallgain = 0; cur_speed = 0; level = 0; Steer = 0; balancetrim = 0; } else { tipstart = 5; Serial.println("Balancing"); digitalWrite(ledtwoPin, LOW); digitalWrite(ledthreePin, LOW); digitalWrite(ledfourPin, LOW); } } //end of while tipstart < 5 overallgain = overallgainstart; //softstart value. Gain will now rise to final of 0.5 at rate of 0.005 per program loop. //i.e. it will go from 0.3 to 0.5 over the first 4 seconds after tipstart has been activated cur_speed = 0; Steering = 512; SteerValue = 512; balancetrim = 0; a0 = accsum; a1 = accsum; a2 = accsum; a3 = accsum; a4 = accsum; a5 = accsum; a6 = accsum; 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 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 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 serialOut_timing();//optional displays loop time on screen first digit is time loop takes to run in millisec, //second digit is final time for loop including the variable added delay to keep it at 100Hz //XXXXXXXXXXXXXXXXXXXX softstart function: board a bit squishy when you first bring it to balanced point, //then ride becomes firmer over next 4 seconds as value for overallgain increases from starting value of 0.3 to 0.5 we have here if (overallgain < overallgaintarget) { overallgain = (float)overallgain + 0.005; } if (overallgain > overallgaintarget) {overallgain = overallgaintarget;} //XXXXXXXXXXXXXXX end of softstart code XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX } } void serialOut_timing(){ static int skip=0; if (skip++==40) { //display every 4000ms (at 100Hz) skip = 0; //Serial.print(lastLoopUsefulTime); Serial.print(","); //Serial.print(lastLoopTime); Serial.println("\\n"); //XXXXXXXXXXXXXX put any variables you want printed to serial window in here uncomment them out XXXXXXXXXXXXXXXXXXXXX //Serial.print("Motor1percent:"); //Serial.print(Motor1percent); //Serial.print(" Motor2percent:"); //Serial.print(Motor2percent); if ((Motor1percent > 25) || (Motor1percent < -25)){ digitalWrite(ledtwoPin, HIGH); } if ((Motor1percent > 50) || (Motor1percent < -50)){ digitalWrite(ledthreePin, HIGH); } if ((Motor1percent > 75) || (Motor1percent < -75)){ digitalWrite(ledfourPin, HIGH); } if ((Motor1percent <= 25) && (Motor1percent >= -25)){ digitalWrite(ledtwoPin, LOW); digitalWrite(ledthreePin, LOW); digitalWrite(ledfourPin, LOW); } //Serial.print("balancegyroDegrees:"); //Serial.print(gangleratedeg); //Serial.print(" accelDegrees:"); //Serial.println(x_accdeg); //Serial.print("Angle: "); //Serial.println(angle); //Serial.print( "cut"); //Serial.println(cut); //Serial.print(" accsum: "); //Serial.println(accsum); //Serial.print(" Overallgaintarget:"); //Serial.println(overallgaintarget); //Serial.print(" level:"); //Serial.println(level); //XXXXXXXXXXXXXX end of watch values in serial window XXXXXXXXXXXXXXXXXXXXXXXXXXX } }