├── .gitmodules ├── BalancingRobot.h ├── BalancingRobotArduino.ino ├── README.md └── gpl2.txt /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "KalmanFilter"] 2 | path = KalmanFilter 3 | url = git://github.com/TKJElectronics/KalmanFilter.git 4 | [submodule "USB_Host_Shield_2.0"] 5 | path = USB_Host_Shield_2.0 6 | url = git://github.com/felis/USB_Host_Shield_2.0.git 7 | [submodule "BalanduinoAndroidApp"] 8 | path = BalanduinoAndroidApp 9 | url = git://github.com/TKJElectronics/BalanduinoAndroidApp.git 10 | [submodule "BalanduinoProcessingApp"] 11 | path = BalanduinoProcessingApp 12 | url = https://github.com/TKJElectronics/BalanduinoProcessingApp.git 13 | -------------------------------------------------------------------------------- /BalancingRobot.h: -------------------------------------------------------------------------------- 1 | #ifndef _balancingrobot_h_ 2 | #define _balancingrobot_h_ 3 | 4 | #include // Needed for uint8_t 5 | 6 | //#define PROMINI 7 | 8 | char stringBuf[30]; 9 | 10 | bool sendData; 11 | bool sendPIDValues; 12 | uint8_t dataCounter; 13 | 14 | #define PWM_FREQUENCY 20000 // The motor driver can handle a pwm frequency up to 20kHz 15 | #define PWMVALUE F_CPU/PWM_FREQUENCY/2 // Frequency is given by F_CPU/(2*N*ICR) - where N is the prescaler, we use no prescaling so frequency is given by F_CPU/(2*ICR) - ICR = F_CPU/PWM_FREQUENCY/2 16 | 17 | /* Used for the PS3 Communication and motor functions */ 18 | int lastCommand; // This is used set a new targetPosition 19 | enum Command { 20 | stop, 21 | forward, 22 | backward, 23 | left, 24 | right, 25 | imu, 26 | joystick, 27 | }; 28 | 29 | /* These are used to read and write to the port registers - see http://www.arduino.cc/en/Reference/PortManipulation 30 | I do this to save processing power - see this page for more information: http://www.billporter.info/ready-set-oscillate-the-fastest-way-to-change-arduino-pins/ */ 31 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 32 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 33 | 34 | /* Left motor */ 35 | #ifdef PROMINI 36 | #define leftPort PORTC 37 | #define leftPortDirection DDRC 38 | #define leftA PINC2 // PC2 - pin A2 (2INA on the Pololu motor driver) 39 | #define leftB PINC3 // PC3 - pin A3 (2INB on the Pololu motor driver) 40 | #else 41 | #define leftPort PORTD 42 | #define leftPortDirection DDRD 43 | #define leftA PIND0 // PD0 - pin 0 (2INA on the Pololu motor driver) 44 | #define leftB PIND1 // PD1 - pin 1 (2INB on the Pololu motor driver) 45 | #endif 46 | 47 | #define leftPwmPortDirection DDRB 48 | #define leftPWM PINB1 // PB1 - pin 9 (OC1A) - (2PWM on the Pololu motor driver) 49 | 50 | /* Right motor */ 51 | #define rightPort PORTC 52 | #define rightPortDirection DDRC 53 | #define rightPwmPortDirection DDRB 54 | 55 | #define rightA PINC4 // PC4 - pin A4 (1INA on the Pololu motor driver) 56 | #define rightB PINC5 // PC5 - pin A5 (1INB on the Pololu motor driver) 57 | #define rightPWM PINB2 // PB2 - pin 10 (OC1B) - (1PWM on the Pololu motor driver) 58 | 59 | /* 60 | Note that the right motor is connected as so to the Pololu motor driver: 61 | Black wire - output 1A 62 | Red wire - output 1B 63 | And the left motor is connected as so to the Pololu motor driver: 64 | Red wire - output 2A 65 | Black wire - output 2B 66 | */ 67 | 68 | /* Encoders */ 69 | #define leftEncoder1 2 // Yellow wire 70 | #define leftEncoder2 4 // White wire 71 | #define rightEncoder1 3 // White wire 72 | #define rightEncoder2 5 // Yellow wire 73 | 74 | volatile long leftCounter = 0; 75 | volatile long rightCounter = 0; 76 | 77 | /* IMU */ 78 | #define gyroY A0 79 | #ifdef PROMINI 80 | #define accY A6 81 | #define accZ A7 82 | #else 83 | #define accY A1 84 | #define accZ A2 85 | #endif 86 | 87 | #define buzzer 6 // Connected to a BC547 transistor - there is a protection diode at the buzzer as well 88 | 89 | // Zero voltage values for the sensors - [0] = gyroY, [1] = accY, [2] = accZ 90 | double zeroValues[3] = { 0 }; 91 | 92 | // Results 93 | double accYangle; 94 | double gyroYrate; 95 | double gyroAngle; 96 | double pitch; 97 | 98 | /* PID variables */ 99 | double Kp = 7; 100 | double Ki = 2; 101 | double Kd = 8; 102 | double targetAngle = 180; 103 | 104 | double lastError; // Store position error 105 | double iTerm; // Store integral term 106 | 107 | /* Used for timing */ 108 | unsigned long timer; 109 | 110 | #define STD_LOOP_TIME 10000 // Fixed time loop of 10 milliseconds 111 | unsigned long lastLoopUsefulTime = STD_LOOP_TIME; 112 | unsigned long loopStartTime; 113 | 114 | bool steerForward; 115 | bool steerBackward; 116 | bool steerStop = true; // Stop by default 117 | bool steerLeft; 118 | bool steerRight; 119 | 120 | bool stopped; // This is used to set new target position after breaking 121 | 122 | bool layingDown = true; // Use to indicate if the robot is laying down 123 | 124 | double targetOffset = 0; // Offset for going forward and backward 125 | double turningOffset = 0; // Offset for turning left and right 126 | 127 | double sppData1 = 0; 128 | double sppData2 = 0; 129 | 130 | uint8_t loopCounter = 0; // Used to update wheel velocity 131 | long wheelPosition; 132 | long lastWheelPosition; 133 | long wheelVelocity; 134 | long targetPosition; 135 | int zoneA = 4000; 136 | int zoneB = 2000; 137 | double positionScaleA = 250; // One resolution is 464 pulses 138 | double positionScaleB = 500; 139 | double positionScaleC = 1000; 140 | double velocityScaleMove = 35; 141 | double velocityScaleStop = 30; 142 | double velocityScaleTurning = 35; 143 | #endif 144 | -------------------------------------------------------------------------------- /BalancingRobotArduino.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * The code is released under the GNU General Public License. 3 | * Developed by Kristian Lauszus 4 | * This is the algorithm for my balancing robot/segway. 5 | * It is controlled by either an Android app or a Processing application via bluetooth. 6 | * The Android app can be found at the following link: https://github.com/TKJElectronics/BalanduinoAndroidApp 7 | * The Processing application can be found here: https://github.com/TKJElectronics/BalancingRobotArduino/tree/master/ProcessingApp 8 | * The SPP Bluetooth Library can be found at the following link: https://github.com/felis/USB_Host_Shield_2.0 9 | * For details, see http://blog.tkjelectronics.dk/2012/02/the-balancing-robot/ 10 | */ 11 | 12 | #include "BalancingRobot.h" 13 | #include // Kalman filter library see: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/ 14 | Kalman kalman; // See https://github.com/TKJElectronics/KalmanFilter for source code 15 | 16 | #include // SS is rerouted to 8 and INT is rerouted to 7 - see http://www.circuitsathome.com/usb-host-shield-hardware-manual at "5. Interface modifications" 17 | USB Usb; 18 | BTD Btd(&Usb); // Uncomment DEBUG in "BTD.cpp" to save space 19 | SPP SerialBT(&Btd,"BalancingRobot","0000"); // Also uncomment DEBUG in "SPP.cpp" 20 | 21 | void setup() { 22 | /* Setup encoders */ 23 | pinMode(leftEncoder1,INPUT); 24 | pinMode(leftEncoder2,INPUT); 25 | pinMode(rightEncoder1,INPUT); 26 | pinMode(rightEncoder2,INPUT); 27 | attachInterrupt(0,leftEncoder,RISING); // pin 2 28 | attachInterrupt(1,rightEncoder,RISING); // pin 3 29 | 30 | /* Setup motor pins to output */ 31 | sbi(leftPwmPortDirection,leftPWM); 32 | sbi(leftPortDirection,leftA); 33 | sbi(leftPortDirection,leftB); 34 | sbi(rightPwmPortDirection,rightPWM); 35 | sbi(rightPortDirection,rightA); 36 | sbi(rightPortDirection,rightB); 37 | 38 | /* Set PWM frequency to 20kHz - see the datasheet http://www.atmel.com/Images/doc8025.pdf page 128-135 */ 39 | // Set up PWM, Phase and Frequency Correct on pin 9 (OC1A) & pin 10 (OC1B) with ICR1 as TOP using Timer1 40 | TCCR1B = _BV(WGM13) | _BV(CS10); // Set PWM Phase and Frequency Correct with ICR1 as TOP and no prescaling 41 | ICR1H = (PWMVALUE >> 8); // ICR1 is the TOP value - this is set so the frequency is equal to 20kHz 42 | ICR1L = (PWMVALUE & 0xFF); 43 | 44 | /* Enable PWM on pin 9 (OC1A) & pin 10 (OC1B) */ 45 | // Clear OC1A/OC1B on compare match when up-counting 46 | // Set OC1A/OC1B on compare match when downcountin 47 | TCCR1A = _BV(COM1A1) | _BV(COM1B1); 48 | setPWM(leftPWM,0); // Turn off pwm on both pins 49 | setPWM(rightPWM,0); 50 | 51 | /* Setup pin for buzzer to beep when finished calibrating */ 52 | pinMode(buzzer,OUTPUT); 53 | 54 | /* Setup IMU Inputs */ 55 | #ifndef PROMINI 56 | analogReference(EXTERNAL); // Set voltage reference to 3.3V by connecting AREF to 3.3V 57 | #endif 58 | pinMode(gyroY,INPUT); 59 | pinMode(accY,INPUT); 60 | pinMode(accZ,INPUT); 61 | 62 | if (Usb.Init() == -1) // Check if USB Host Shield is working 63 | while(1); // Halt 64 | 65 | /* Calibrate the gyro and accelerometer relative to ground */ 66 | calibrateSensors(); 67 | 68 | /* Setup timing */ 69 | loopStartTime = micros(); 70 | timer = loopStartTime; 71 | } 72 | 73 | void loop() { 74 | /* Calculate pitch */ 75 | accYangle = getAccY(); 76 | gyroYrate = getGyroYrate(); 77 | gyroAngle += gyroYrate*((double)(micros()-timer)/1000000); 78 | // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.htm 79 | pitch = kalman.getAngle(accYangle, gyroYrate, (double)(micros() - timer)/1000000); // Calculate the angle using a Kalman filter 80 | timer = micros(); 81 | 82 | /* Drive motors */ 83 | // If the robot is laying down, it has to be put in a vertical position before it starts balancing 84 | // If it's already balancing it has to be ±45 degrees before it stops trying to balance 85 | if((layingDown && (pitch < 170 || pitch > 190)) || (!layingDown && (pitch < 135 || pitch > 225))) { 86 | layingDown = true; // The robot is in a unsolvable position, so turn off both motors and wait until it's vertical again 87 | stopAndReset(); 88 | } 89 | else { 90 | layingDown = false; // It's no longer laying down 91 | PID(targetAngle,targetOffset,turningOffset); 92 | } 93 | 94 | /* Update wheel velocity every 100ms */ 95 | loopCounter++; 96 | if (loopCounter == 10) { 97 | loopCounter = 0; // Reset loop counter 98 | wheelPosition = readLeftEncoder() + readRightEncoder(); 99 | wheelVelocity = wheelPosition - lastWheelPosition; 100 | lastWheelPosition = wheelPosition; 101 | if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if braking 102 | targetPosition = wheelPosition; 103 | stopped = true; 104 | } 105 | } 106 | 107 | /* Read the SPP connection */ 108 | readSPP(); 109 | 110 | if(SerialBT.connected) { 111 | Usb.Task(); 112 | if(sendPIDValues) { 113 | sendPIDValues = false; 114 | strcpy(stringBuf,"P,"); 115 | strcat(stringBuf,SerialBT.doubleToString(Kp,2)); 116 | strcat(stringBuf,","); 117 | strcat(stringBuf,SerialBT.doubleToString(Ki,2)); 118 | strcat(stringBuf,","); 119 | strcat(stringBuf,SerialBT.doubleToString(Kd,2)); 120 | strcat(stringBuf,","); 121 | strcat(stringBuf,SerialBT.doubleToString(targetAngle,2)); 122 | SerialBT.println(stringBuf); 123 | dataCounter = 1; 124 | } else if(sendData) { 125 | switch(dataCounter) { 126 | case 0: 127 | strcpy(stringBuf,"V,"); 128 | strcat(stringBuf,SerialBT.doubleToString(accYangle,2)); 129 | strcat(stringBuf,","); 130 | strcat(stringBuf,SerialBT.doubleToString(gyroAngle,2)); 131 | strcat(stringBuf,","); 132 | strcat(stringBuf,SerialBT.doubleToString(pitch,2)); 133 | SerialBT.println(stringBuf); 134 | break; 135 | } 136 | dataCounter++; 137 | if(dataCounter > 4) 138 | dataCounter = 0; 139 | } 140 | } 141 | 142 | /* Use a time fixed loop */ 143 | lastLoopUsefulTime = micros() - loopStartTime; 144 | if (lastLoopUsefulTime < STD_LOOP_TIME) { 145 | while((micros() - loopStartTime) < STD_LOOP_TIME) 146 | Usb.Task(); 147 | } 148 | loopStartTime = micros(); 149 | } 150 | void PID(double restAngle, double offset, double turning) { 151 | /* Steer robot */ 152 | if (steerForward) { 153 | offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing 154 | restAngle -= offset; 155 | } 156 | else if (steerBackward) { 157 | offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing 158 | restAngle += offset; 159 | } 160 | /* Brake */ 161 | else if (steerStop) { 162 | long positionError = wheelPosition - targetPosition; 163 | if (abs(positionError) > zoneA) // Inside zone A 164 | restAngle -= (double)positionError/positionScaleA; 165 | else if (abs(positionError) > zoneB) // Inside zone B 166 | restAngle -= (double)positionError/positionScaleB; 167 | else // Inside zone C 168 | restAngle -= (double)positionError/positionScaleC; 169 | restAngle -= (double)wheelVelocity/velocityScaleStop; 170 | if (restAngle < 160) // Limit rest Angle 171 | restAngle = 160; 172 | else if (restAngle > 200) 173 | restAngle = 200; 174 | } 175 | /* Update PID values */ 176 | double error = (restAngle - pitch); 177 | double pTerm = Kp * error; 178 | iTerm += Ki * error; 179 | double dTerm = Kd * (error - lastError); 180 | lastError = error; 181 | double PIDValue = pTerm + iTerm + dTerm; 182 | 183 | /* Steer robot sideways */ 184 | double PIDLeft; 185 | double PIDRight; 186 | if (steerLeft) { 187 | turning -= abs((double)wheelVelocity/velocityScaleTurning); // Scale down at high speed 188 | if(turning < 0) 189 | turning = 0; 190 | PIDLeft = PIDValue-turning; 191 | PIDRight = PIDValue+turning; 192 | } 193 | else if (steerRight) { 194 | turning -= abs((double)wheelVelocity/velocityScaleTurning); // Scale down at high speed 195 | if(turning < 0) 196 | turning = 0; 197 | PIDLeft = PIDValue+turning; 198 | PIDRight = PIDValue-turning; 199 | } 200 | else { 201 | PIDLeft = PIDValue; 202 | PIDRight = PIDValue; 203 | } 204 | 205 | PIDLeft *= 0.95; // compensate for difference in the motors 206 | 207 | /* Set PWM Values */ 208 | if (PIDLeft >= 0) 209 | moveMotor(left, forward, PIDLeft); 210 | else 211 | moveMotor(left, backward, PIDLeft * -1); 212 | if (PIDRight >= 0) 213 | moveMotor(right, forward, PIDRight); 214 | else 215 | moveMotor(right, backward, PIDRight * -1); 216 | } 217 | void readSPP() { 218 | Usb.Task(); 219 | if(SerialBT.connected) { 220 | if(SerialBT.available()) { 221 | char input[30]; 222 | uint8_t i = 0; 223 | while (1) { 224 | input[i] = SerialBT.read(); 225 | if(input[i] == -1) // Error while reading the string 226 | return; 227 | if (input[i] == ';') // Keep reading until it reads a semicolon 228 | break; 229 | i++; 230 | } 231 | /*Serial.print("Data: "); 232 | Serial.write((uint8_t*)input,i); 233 | Serial.println();*/ 234 | if(input[0] == 'A') { // Abort 235 | stopAndReset(); 236 | while(SerialBT.read() != 'C') // Wait until continue is send 237 | Usb.Task(); 238 | } 239 | 240 | /* Set PID and target angle */ 241 | else if(input[0] == 'P') { 242 | strtok(input, ","); // Ignore 'P' 243 | Kp = atof(strtok(NULL, ";")); 244 | } else if(input[0] == 'I') { 245 | strtok(input, ","); // Ignore 'I' 246 | Ki = atof(strtok(NULL, ";")); 247 | } else if(input[0] == 'D') { 248 | strtok(input, ","); // Ignore 'D' 249 | Kd = atof(strtok(NULL, ";")); 250 | } else if(input[0] == 'T') { // Target Angle 251 | strtok(input, ","); // Ignore 'T' 252 | targetAngle = atof(strtok(NULL, ";")); 253 | } else if(input[0] == 'G') { // The processing/Android application sends when it need the current values 254 | if(input[1] == 'P') // PID Values 255 | sendPIDValues = true; 256 | else if(input[1] == 'B') // Begin 257 | sendData = true; // Send output to processing/Android application 258 | else if(input[1] == 'S') // Stop 259 | sendData = false; // Stop sending output to processing/Android application 260 | } 261 | /* Remote control */ 262 | else if(input[0] == 'S') // Stop 263 | steer(stop); 264 | else if(input[0] == 'J') { // Joystick 265 | strtok(input, ","); // Ignore 'J' 266 | sppData1 = atof(strtok(NULL, ",")); // x-axis 267 | sppData2 = atof(strtok(NULL, ";")); // y-axis 268 | steer(joystick); 269 | } 270 | else if(input[0] == 'M') { // IMU 271 | strtok(input, ","); // Ignore 'I' 272 | sppData1 = atof(strtok(NULL, ",")); // Pitch 273 | sppData2 = atof(strtok(NULL, ";")); // Roll 274 | steer(imu); 275 | //SerialBT.printNumberln(sppData1); 276 | //SerialBT.printNumberln(sppData2); 277 | } 278 | } 279 | } else 280 | steer(stop); 281 | } 282 | void steer(Command command) { 283 | // Set all false 284 | steerForward = false; 285 | steerBackward = false; 286 | steerStop = false; 287 | steerLeft = false; 288 | steerRight = false; 289 | if(command == joystick) { 290 | if(sppData2 > 0) { 291 | targetOffset = scale(sppData2,0,1,0,7); 292 | steerForward = true; 293 | } else if(sppData2 < 0) { 294 | targetOffset = scale(sppData2,0,-1,0,7); 295 | steerBackward = true; 296 | } 297 | if(sppData1 > 0) { 298 | turningOffset = scale(sppData1,0,1,0,20); 299 | steerRight = true; 300 | } else if(sppData1 < 0) { 301 | turningOffset = scale(sppData1,0,-1,0,20); 302 | steerLeft = true; 303 | } 304 | } else if(command == imu) { 305 | if(sppData2 > 0) { 306 | targetOffset = scale(sppData2,1,36,0,7); 307 | steerForward = true; 308 | } 309 | else if(sppData2 < 0) { 310 | targetOffset = scale(sppData2,-1,-36,0,7); 311 | steerBackward = true; 312 | } 313 | if(sppData1 > 0) { 314 | turningOffset = scale(sppData1,1,45,0,20); 315 | steerLeft = true; 316 | } 317 | else if(sppData1 < 0) { 318 | turningOffset = scale(sppData1,-1,-45,0,20); 319 | steerRight = true; 320 | } 321 | } 322 | 323 | else if(command == stop) { 324 | steerStop = true; 325 | if(lastCommand != stop) { // Set new stop position 326 | targetPosition = wheelPosition; 327 | stopped = false; 328 | } 329 | } 330 | lastCommand = command; 331 | } 332 | double scale(double input, double inputMin, double inputMax, double outputMin, double outputMax) { // Like map() just returns a double 333 | double output; 334 | if(inputMin < inputMax) 335 | output = (input-inputMin)/((inputMax-inputMin)/(outputMax-outputMin)); 336 | else 337 | output = (inputMin-input)/((inputMin-inputMax)/(outputMax-outputMin)); 338 | if(output > outputMax) 339 | output = outputMax; 340 | else if(output < outputMin) 341 | output = outputMin; 342 | return output; 343 | } 344 | void stopAndReset() { 345 | stopMotor(left); 346 | stopMotor(right); 347 | lastError = 0; 348 | iTerm = 0; 349 | targetPosition = wheelPosition; 350 | } 351 | double getGyroYrate() { 352 | // (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3*1023=1.0323 353 | double gyroRate = -((double)((double)analogRead(gyroY) - zeroValues[0]) / 1.0323); 354 | return gyroRate; 355 | } 356 | double getAccY() { 357 | double accYval = ((double)analogRead(accY) - zeroValues[1]); 358 | double accZval = ((double)analogRead(accZ) - zeroValues[2]); 359 | // Convert to 360 degrees resolution 360 | // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2 361 | // We are then convert it to 0 to 2π and then from radians to degrees 362 | return (atan2(-accYval,-accZval)+PI)*RAD_TO_DEG; 363 | } 364 | void calibrateSensors() { 365 | for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings 366 | zeroValues[0] += analogRead(gyroY); 367 | zeroValues[1] += analogRead(accY); 368 | zeroValues[2] += analogRead(accZ); 369 | delay(10); 370 | } 371 | zeroValues[0] /= 100; // Gyro X-axis 372 | zeroValues[1] /= 100; // Accelerometer Y-axis 373 | zeroValues[2] /= 100; // Accelerometer Z-axis 374 | 375 | if(zeroValues[1] > 500) { // Check which side is lying down - 1g is equal to 0.33V or 102.3 quids (0.33/3.3*1023=102.3) 376 | zeroValues[1] -= 102.3; // +1g when lying at one of the sides 377 | kalman.setAngle(90); // It starts at 90 degress and 270 when facing the other way 378 | gyroAngle = 90; 379 | } else { 380 | zeroValues[1] += 102.3; // -1g when lying at the other side 381 | kalman.setAngle(270); 382 | gyroAngle = 270; 383 | } 384 | 385 | digitalWrite(buzzer,HIGH); 386 | delay(100); 387 | digitalWrite(buzzer,LOW); 388 | } 389 | void moveMotor(Command motor, Command direction, double speedRaw) { // Speed is a value in percentage 0-100% 390 | if(speedRaw > 100) 391 | speedRaw = 100; 392 | int speed = speedRaw*((double)PWMVALUE)/100; // Scale from 100 to PWMVALUE 393 | if (motor == left) { 394 | setPWM(leftPWM,speed); // Left motor pwm 395 | if (direction == forward) { 396 | cbi(leftPort,leftA); 397 | sbi(leftPort,leftB); 398 | } 399 | else if (direction == backward) { 400 | sbi(leftPort,leftA); 401 | cbi(leftPort,leftB); 402 | } 403 | } 404 | else if (motor == right) { 405 | setPWM(rightPWM,speed); // Right motor pwm 406 | if (direction == forward) { 407 | cbi(rightPort,rightA); 408 | sbi(rightPort,rightB); 409 | } 410 | else if (direction == backward) { 411 | sbi(rightPort,rightA); 412 | cbi(rightPort,rightB); 413 | } 414 | } 415 | } 416 | void stopMotor(Command motor) { 417 | if (motor == left) { 418 | setPWM(leftPWM,PWMVALUE); // Set high 419 | sbi(leftPort,leftA); 420 | sbi(leftPort,leftB); 421 | } 422 | else if (motor == right) { 423 | setPWM(rightPWM,PWMVALUE); // Set high 424 | sbi(rightPort,rightA); 425 | sbi(rightPort,rightB); 426 | } 427 | } 428 | 429 | void setPWM(uint8_t pin, int dutyCycle) { // dutyCycle is a value between 0-ICR 430 | if(pin == leftPWM) { 431 | OCR1AH = (dutyCycle >> 8); 432 | OCR1AL = (dutyCycle & 0xFF); 433 | } else if (pin == rightPWM) { 434 | OCR1BH = (dutyCycle >> 8); 435 | OCR1BL = (dutyCycle & 0xFF); 436 | } 437 | } 438 | 439 | /* Interrupt routine and encoder read functions - I read using the port registers for faster processing */ 440 | void leftEncoder() { 441 | if(PIND & _BV(PIND4)) // read pin 4 442 | leftCounter--; 443 | else 444 | leftCounter++; 445 | } 446 | void rightEncoder() { 447 | if(PIND & _BV(PIND5)) // read pin 5 448 | rightCounter--; 449 | else 450 | rightCounter++; 451 | } 452 | long readLeftEncoder() { // The encoders decrease when motors are traveling forward and increase when traveling backward 453 | return leftCounter; 454 | } 455 | long readRightEncoder() { 456 | return rightCounter; 457 | } 458 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | #### Developed by Kristian Lauszus, TKJ Electronics 2012 2 | 3 | The code is released under the GNU General Public License. 4 | _________ 5 | 6 | # This project is deprecated 7 | 8 | This project is no longer maintained as I will instead focus on a Balancing robot kit, the Balanduino. For more information see the new repository: and the Kickstarter campaign: . 9 | 10 |
11 | 12 | This is the code for my balancing robot/segway. It's a ported version of the code for the mbed board which I original used. The original code can be found at the following link: [https://github.com/TKJElectronics/BalancingRobot](https://github.com/TKJElectronics/BalancingRobot). 13 | 14 | The code will work for all boards that features an ATmega328p (Duemilanove, Uno, Pro, Pro Mini etc.) - it's not directly pin compatible with the larger Arduinos (Mega, Mega 2560 etc.) as I use the port registers to save processing resources - see [http://www.arduino.cc/en/Reference/PortManipulation](http://www.arduino.cc/en/Reference/PortManipulation). But just take a look at the pinMapping pages for comparison: [http://arduino.cc/en/Hacking/PinMapping168](http://arduino.cc/en/Hacking/PinMapping168) and [http://arduino.cc/en/Hacking/PinMapping2560](http://arduino.cc/en/Hacking/PinMapping2560) and figure the pins out yourself - for instance OC1A and OC1B are not located on pin 9 and 10, but at pin 11 and 12 on the Arduino Mega. 15 | 16 | I have used the registers to set up 20kHz PWM, Phase and Frequency Correct on pin 9 (OC1A) & pin 10 (OC1B) with ICR1 as TOP using Timer1 - see the [datasheet](http://www.atmel.com/Images/doc8025.pdf) page 128-135. 17 | 18 | I use a 6DOF IMU from Sparkfun: [http://www.sparkfun.com/products/10010](http://www.sparkfun.com/products/10010), though I only use one of the gyro axis, but any IMU can be used. For instance the very popular MPU-6050, see this [example code](https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU6DOF/MPU6050/MPU6050.ino). 19 | 20 | For more info about calculating the pitch see my post at the Arduino forum: [http://arduino.cc/forum/index.php/topic,58048.0.html](http://arduino.cc/forum/index.php/topic,58048.0.html). 21 | 22 | For more information about the Kalman filter see my blog post: [http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/](http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/) and the source code: [https://github.com/TKJElectronics/KalmanFilter](https://github.com/TKJElectronics/KalmanFilter). 23 | 24 | To steer the robot, I use a [USB Host Shield](http://www.circuitsathome.com/products-page/arduino-shields/usb-host-shield-2-0-for-arduino/) together with my SPP Bluetooth Library for Arduino: [https://github.com/felis/USB_Host_Shield_2.0/blob/master/SPP.cpp](https://github.com/felis/USB_Host_Shield_2.0/blob/master/SPP.cpp). 25 | More information can be found at the blog psot: [http://blog.tkjelectronics.dk/2012/07/rfcommspp-library-for-arduino/](http://blog.tkjelectronics.dk/2012/07/rfcommspp-library-for-arduino/). 26 | 27 | You can either use an Android app I wrote: [https://github.com/TKJElectronics/BalanduinoAndroidApp](https://github.com/TKJElectronics/BalanduinoAndroidApp) or the [Processing Application](https://github.com/TKJElectronics/BalanduinoProcessingApp) to control the robot. 28 | 29 | For information about the hardware, see the wiki: [https://github.com/TKJElectronics/BalancingRobot/wiki/Hardware](https://github.com/TKJElectronics/BalancingRobot/wiki/Hardware). 30 | 31 | Also check out the youtube video of it in action: [http://www.youtube.com/watch?v=N28C_JqVhGU](http://www.youtube.com/watch?v=N28C_JqVhGU) - this is actually the mbed version, but they behave the same way. 32 | 33 | For more information see my blog post at [http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/](http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/) or send me an email at kristianl@tkjelectronics.dk. -------------------------------------------------------------------------------- /gpl2.txt: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc. 5 | 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Library General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | 294 | Copyright (C) 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License 307 | along with this program; if not, write to the Free Software 308 | Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 309 | 310 | 311 | Also add information on how to contact you by electronic and paper mail. 312 | 313 | If the program is interactive, make it output a short notice like this 314 | when it starts in an interactive mode: 315 | 316 | Gnomovision version 69, Copyright (C) year name of author 317 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 318 | This is free software, and you are welcome to redistribute it 319 | under certain conditions; type `show c' for details. 320 | 321 | The hypothetical commands `show w' and `show c' should show the appropriate 322 | parts of the General Public License. Of course, the commands you use may 323 | be called something other than `show w' and `show c'; they could even be 324 | mouse-clicks or menu items--whatever suits your program. 325 | 326 | You should also get your employer (if you work as a programmer) or your 327 | school, if any, to sign a "copyright disclaimer" for the program, if 328 | necessary. Here is a sample; alter the names: 329 | 330 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 331 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 332 | 333 | , 1 April 1989 334 | Ty Coon, President of Vice 335 | 336 | This General Public License does not permit incorporating your program into 337 | proprietary programs. If your program is a subroutine library, you may 338 | consider it more useful to permit linking proprietary applications with the 339 | library. If this is what you want to do, use the GNU Library General 340 | Public License instead of this License. 341 | --------------------------------------------------------------------------------