So, about a week or two ago, I came across this post on hackaday, which led me to this site with instructions for building a self-balancing robot. I’ve been wanting to make one of these for a while, so I figured I would give it a shot.
I mostly copied the same design from Joop Brokking, so if you are building one, I’d suggest following his youtube video series on the design and construction of the robot.
I used the parts that I had on hand, so my stepper drivers are different and I used an IR remote and receiver instead of the RF that Joop used.
I began by building the circuit on a breadboard to make sure Joop’s code would work with my stepper drivers.
Everything looked good, so I moved the circuit to protoboard.
I built the frame for the robot out of some scrap pieces of wood and mdf. The wheels were parts of a meccano set that I just forced onto the stepper axle.
I modified the code from the original for this machine to work. It is pasted below.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 |
/////////////////////////////////////////////////////////////////////////////////////// //Terms of use /////////////////////////////////////////////////////////////////////////////////////// //THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR //IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, //FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE //AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER //LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, //OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN //THE SOFTWARE. /////////////////////////////////////////////////////////////////////////////////////// #include "PinChangeInterrupt.h" // Include PinChangeInterrupt to run IR library interrupt on alternate pin #include "IRLremote.h" // Include IR library #include <Wire.h> //Include the Wire.h library so we can communicate with the gyro #define IRPIN 11 CHashIR IRLremote; unsigned long right = 4285958208; // IR code to turn right unsigned long left = 36908096; // IR code to turn left unsigned long back = 2139216528; // IR code to move back unsigned long fwd = 1535075151; // IR code to move forward int gyro_address = 0x68; //MPU-6050 I2C address (0x68 or 0x69) int acc_calibration_value = 480; //Enter the accelerometer calibration value //Various settings float pid_p_gain = 15; //Gain setting for the P-controller (15) float pid_i_gain = 1.5; //Gain setting for the I-controller (1.5) float pid_d_gain = 30; //Gain setting for the D-controller (30) float turning_speed = 50; //Turning speed (20) float max_target_speed = 800; //Max target speed (100) /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Declaring global variables /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// unsigned long received_cmd; byte start,low_bat; int left_motor, throttle_left_motor, throttle_counter_left_motor, throttle_left_motor_memory; int right_motor, throttle_right_motor, throttle_counter_right_motor, throttle_right_motor_memory; int battery_voltage; int receive_counter; int gyro_pitch_data_raw, gyro_yaw_data_raw, accelerometer_data_raw; long gyro_yaw_calibration_value, gyro_pitch_calibration_value; unsigned long loop_timer; float angle_gyro, angle_acc, angle, self_balance_pid_setpoint; float pid_error_temp, pid_i_mem, pid_setpoint, gyro_input, pid_output, pid_last_d_error; float pid_output_left, pid_output_right; ////////////////////////////r/////////////////////////////////////////////////////////////////////////////////////////////////////////// //Setup basic functions /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void setup(){ if (!IRLremote.begin(IRPIN)) { while(1) { // Blink LED in pattern if IR not working digitalWrite(13, HIGH); delay(200); digitalWrite(13, LOW); delay(100); } } Wire.begin(); //Start the I2C bus as master TWBR = 12; //Set the I2C clock speed to 400kHz //To create a variable pulse for controlling the stepper motors a timer is created that will execute a piece of code (subroutine) every 20us //This subroutine is called TIMER2_COMPA_vect TCCR2A = 0; //Make sure that the TCCR2A register is set to zero TCCR2B = 0; //Make sure that the TCCR2A register is set to zero TIMSK2 |= (1 << OCIE2A); //Set the interupt enable bit OCIE2A in the TIMSK2 register TCCR2B |= (1 << CS21); //Set the CS21 bit in the TCCRB register to set the prescaler to 8 OCR2A = 39; //30us = 59 50us=99 //The compare register is set to 39 => 20us / (1s / (16.000.000MHz / 8)) - 1 TCCR2A |= (1 << WGM21); //Set counter 2 to CTC (clear timer on compare) mode //By default the MPU-6050 sleeps. So we have to wake it up. Wire.beginTransmission(gyro_address); //Start communication with the address found during search. Wire.write(0x6B); //We want to write to the PWR_MGMT_1 register (6B hex) Wire.write(0x00); //Set the register bits as 00000000 to activate the gyro Wire.endTransmission(); //End the transmission with the gyro. //Set the full scale of the gyro to +/- 250 degrees per second Wire.beginTransmission(gyro_address); //Start communication with the address found during search. Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex) Wire.write(0x00); //Set the register bits as 00000000 (250dps full scale) Wire.endTransmission(); //End the transmission with the gyro //Set the full scale of the accelerometer to +/- 4g. Wire.beginTransmission(gyro_address); //Start communication with the address found during search. Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register (1A hex) Wire.write(0x08); //Set the register bits as 00001000 (+/- 4g full scale range) Wire.endTransmission(); //End the transmission with the gyro //Set some filtering to improve the raw data. Wire.beginTransmission(gyro_address); //Start communication with the address found during search Wire.write(0x1A); //We want to write to the CONFIG register (1A hex) Wire.write(0x03); //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz) Wire.endTransmission(); //End the transmission with the gyro pinMode(2, OUTPUT); //Configure pin 2 as output pinMode(3, OUTPUT); //Configure pin 3 as output pinMode(4, OUTPUT); //Configure pin 4 as output pinMode(5, OUTPUT); //Configure pin 5 as output pinMode(13, OUTPUT); //Configure pin 6 as output for(receive_counter = 0; receive_counter < 500; receive_counter++){ //Create 500 loops if(receive_counter % 15 == 0)digitalWrite(13, !digitalRead(13)); //Change the state of the LED every 15 loops to make the LED blink Wire.beginTransmission(gyro_address); //Start communication with the gyro Wire.write(0x43); //Start reading the Who_am_I register 75h Wire.endTransmission(); //End the transmission Wire.requestFrom(gyro_address, 4); //Request 2 bytes from the gyro gyro_yaw_calibration_value += Wire.read()<<8|Wire.read(); //Combine the two bytes to make one integer gyro_pitch_calibration_value += Wire.read()<<8|Wire.read(); //Combine the two bytes to make one integer delayMicroseconds(3700); //Wait for 3700 microseconds to simulate the main program loop time } gyro_pitch_calibration_value /= 500; //Divide the total value by 500 to get the avarage gyro offset gyro_yaw_calibration_value /= 500; //Divide the total value by 500 to get the avarage gyro offset loop_timer = micros() + 4000; //Set the loop_timer variable at the next end loop time } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Main program loop /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void loop(){ if(IRLremote.available()){ //If there is IR data available auto data = IRLremote.read(); received_cmd = data.command; if (received_cmd == fwd || received_cmd == back || received_cmd == left || received_cmd == right) { receive_counter = 0; //Reset the receive_counter variable } } if(receive_counter <= 25)receive_counter ++; //The received command will be valid for 25 program loops (100 milliseconds) else received_cmd = 0; //After 100 milliseconds the received_cmd is reset //Load the battery voltage to the battery_voltage variable. //85 is the voltage compensation for the diode. //Resistor voltage divider => (3.3k + 3.3k)/2.2k = 2.5 //12.5V equals ~5V @ Analog 0. //12.5V equals 1023 analogRead(0). //1250 / 1023 = 1.222. //The variable battery_voltage holds 1050 if the battery voltage is 10.5V. battery_voltage = (analogRead(0) * 1.222) + 85; if(battery_voltage < 1050 && battery_voltage > 800){ //If battery voltage is below 10.5V and higher than 8.0V digitalWrite(13, HIGH); //Turn on the led if battery voltage is to low low_bat = 1; //Set the low_bat variable to 1 } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Angle calculations /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// Wire.beginTransmission(gyro_address); //Start communication with the gyro Wire.write(0x3F); //Start reading at register 3F Wire.endTransmission(); //End the transmission Wire.requestFrom(gyro_address, 2); //Request 2 bytes from the gyro accelerometer_data_raw = Wire.read()<<8|Wire.read(); //Combine the two bytes to make one integer accelerometer_data_raw += acc_calibration_value; //Add the accelerometer calibration value if(accelerometer_data_raw > 8200)accelerometer_data_raw = 8200; //Prevent division by zero by limiting the acc data to +/-8200; if(accelerometer_data_raw < -8200)accelerometer_data_raw = -8200; //Prevent division by zero by limiting the acc data to +/-8200; angle_acc = asin((float)accelerometer_data_raw/8200.0)* 57.296; //Calculate the current angle according to the accelerometer if(start == 0 && angle_acc > -0.5&& angle_acc < 0.5){ //If the accelerometer angle is almost 0 angle_gyro = angle_acc; //Load the accelerometer angle in the angle_gyro variable start = 1; //Set the start variable to start the PID controller } Wire.beginTransmission(gyro_address); //Start communication with the gyro Wire.write(0x43); //Start reading at register 43 Wire.endTransmission(); //End the transmission Wire.requestFrom(gyro_address, 4); //Request 4 bytes from the gyro gyro_yaw_data_raw = Wire.read()<<8|Wire.read(); //Combine the two bytes to make one integer gyro_pitch_data_raw = Wire.read()<<8|Wire.read(); //Combine the two bytes to make one integer gyro_pitch_data_raw -= gyro_pitch_calibration_value; //Add the gyro calibration value angle_gyro += gyro_pitch_data_raw * 0.000031; //Calculate the traveled during this loop angle and add this to the angle_gyro variable /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //MPU-6050 offset compensation /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Not every gyro is mounted 100% level with the axis of the robot. This can be cause by misalignments during manufacturing of the breakout board. //As a result the robot will not rotate at the exact same spot and start to make larger and larger circles. //To compensate for this behavior a VERY SMALL angle compensation is needed when the robot is rotating. //Try 0.0000003 or -0.0000003 first to see if there is any improvement. gyro_yaw_data_raw -= gyro_yaw_calibration_value; //Add the gyro calibration value //Uncomment the following line to make the compensation active //angle_gyro -= gyro_yaw_data_raw * 0.0000003; //Compensate the gyro offset when the robot is rotating angle_gyro = angle_gyro * 0.9996 + angle_acc * 0.0004; //Correct the drift of the gyro angle with the accelerometer angle /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //PID controller calculations /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //The balancing robot is angle driven. First, the difference between the desired angle (setpoint) and actual angle (process value) //is calculated. The self_balance_pid_setpoint variable is automatically changed to make sure that the robot stays balanced all the time. //The (pid_setpoint - pid_output * 0.015) part functions as a brake function. pid_error_temp = angle_gyro - self_balance_pid_setpoint - pid_setpoint; if(pid_output > 10 || pid_output < -10)pid_error_temp += pid_output * 0.015 ; pid_i_mem += pid_i_gain * pid_error_temp; //Calculate the I-controller value and add it to the pid_i_mem variable if(pid_i_mem > 400)pid_i_mem = 400; //Limit the I-controller to the maximum controller output else if(pid_i_mem < -400)pid_i_mem = -400; //Calculate the PID output value pid_output = pid_p_gain * pid_error_temp + pid_i_mem + pid_d_gain * (pid_error_temp - pid_last_d_error); if(pid_output > 400)pid_output = 400; //Limit the PI-controller to the maximum controller output else if(pid_output < -400)pid_output = -400; pid_last_d_error = pid_error_temp; //Store the error for the next loop if(pid_output < 8 && pid_output > -8)pid_output = 0; //Create a dead-band to stop the motors when the robot is balanced if(angle_gyro > 30 || angle_gyro < -30 || start == 0 || low_bat == 1){ //If the robot tips over or the start variable is zero or the battery is empty pid_output = 0; //Set the PID controller output to 0 so the motors stop moving pid_i_mem = 0; //Reset the I-controller memory start = 0; //Set the start variable to 0 self_balance_pid_setpoint = 0; //Reset the self_balance_pid_setpoint variable } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Control calculations /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// pid_output_left = pid_output; //Copy the controller output to the pid_output_left variable for the left motor pid_output_right = pid_output; //Copy the controller output to the pid_output_right variable for the right motor if(received_cmd == right) { pid_output_left += turning_speed; //Increase the left motor speed pid_output_right -= turning_speed; //Decrease the right motor speed } if(received_cmd == left){ pid_output_left -= turning_speed; //Decrease the left motor speed pid_output_right += turning_speed; //Increase the right motor speed } if(received_cmd == fwd) { if(pid_setpoint > -2.5)pid_setpoint -= 0.05; //Slowly change the setpoint angle so the robot starts leaning forward if(pid_output > max_target_speed * -1)pid_setpoint -= 0.005; //Slowly change the setpoint angle so the robot starts leaning forward } if (received_cmd == back){ if(pid_setpoint < 2.5)pid_setpoint += 0.05; //Slowly change the setpoint angle so the robot starts leaning back if(pid_output < max_target_speed)pid_setpoint += 0.005; //Slowly change the setpoint angle so the robot starts leaning back } if(!(received_cmd == back || received_cmd == fwd)){ if(pid_setpoint > 0.5)pid_setpoint -=0.05; //If the PID setpoint is larger then 0.5 reduce the setpoint with 0.05 every loop else if(pid_setpoint < -0.5)pid_setpoint +=0.05; //If the PID setpoint is smaller then -0.5 increase the setpoint with 0.05 every loop else pid_setpoint = 0; //If the PID setpoint is smaller then 0.5 or larger then -0.5 set the setpoint to 0 } //The self balancing point is adjusted when there is not forward or backwards movement from the transmitter. This way the robot will always find it's balancing point if(pid_setpoint == 0){ //If the setpoint is zero degrees if(pid_output < 0)self_balance_pid_setpoint += 0.0015; //Increase the self_balance_pid_setpoint if the robot is still moving forwards if(pid_output > 0)self_balance_pid_setpoint -= 0.0015; //Decrease the self_balance_pid_setpoint if the robot is still moving backwards } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Motor pulse calculations /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //To compensate for the non-linear behaviour of the stepper motors the folowing calculations are needed to get a linear speed behaviour. if(pid_output_left > 0)pid_output_left = 405 - (1/(pid_output_left + 9)) * 5500; else if(pid_output_left < 0)pid_output_left = -405 - (1/(pid_output_left - 9)) * 5500; if(pid_output_right > 0)pid_output_right = 405 - (1/(pid_output_right + 9)) * 5500; else if(pid_output_right < 0)pid_output_right = -405 - (1/(pid_output_right - 9)) * 5500; //Calculate the needed pulse time for the left and right stepper motor controllers if(pid_output_left > 0)left_motor = 400 - pid_output_left; else if(pid_output_left < 0)left_motor = -400 - pid_output_left; else left_motor = 0; if(pid_output_right > 0)right_motor = 400 - pid_output_right; else if(pid_output_right < 0)right_motor = -400 - pid_output_right; else right_motor = 0; //Copy the pulse time to the throttle variables so the interrupt subroutine can use them throttle_left_motor = left_motor; throttle_right_motor = right_motor; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Loop time timer /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //The angle calculations are tuned for a loop time of 4 milliseconds. To make sure every loop is exactly 4 milliseconds a wait loop //is created by setting the loop_timer variable to +4000 microseconds every loop. while(loop_timer > micros()); loop_timer += 4000; } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Interrupt routine TIMER2_COMPA_vect /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ISR(TIMER2_COMPA_vect){ //Left motor pulse calculations throttle_counter_left_motor ++; //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed if(throttle_counter_left_motor > throttle_left_motor_memory){ //If the number of loops is larger then the throttle_left_motor_memory variable throttle_counter_left_motor = 0; //Reset the throttle_counter_left_motor variable throttle_left_motor_memory = throttle_left_motor; //Load the next throttle_left_motor variable if(throttle_left_motor_memory < 0){ //If the throttle_left_motor_memory is negative PORTD &= 0b11110111; //Set output 3 low to reverse the direction of the stepper controller throttle_left_motor_memory *= -1; //Invert the throttle_left_motor_memory variable } else PORTD |= 0b00001000; //Set output 3 high for a forward direction of the stepper motor } else if(throttle_counter_left_motor == 1)PORTD |= 0b00000100; //Set output 2 high to create a pulse for the stepper controller else if(throttle_counter_left_motor == 2)PORTD &= 0b11111011; //Set output 2 low because the pulse only has to last for 20us //right motor pulse calculations throttle_counter_right_motor ++; //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed if(throttle_counter_right_motor > throttle_right_motor_memory){ //If the number of loops is larger then the throttle_right_motor_memory variable throttle_counter_right_motor = 0; //Reset the throttle_counter_right_motor variable throttle_right_motor_memory = throttle_right_motor; //Load the next throttle_right_motor variable if(throttle_right_motor_memory < 0){ //If the throttle_right_motor_memory is negative PORTD |= 0b00100000; //Set output 5 low to reverse the direction of the stepper controller throttle_right_motor_memory *= -1; //Invert the throttle_right_motor_memory variable } else PORTD &= 0b11011111; //Set output 5 high for a forward direction of the stepper motor } else if(throttle_counter_right_motor == 1)PORTD |= 0b00010000; //Set output 4 high to create a pulse for the stepper controller else if(throttle_counter_right_motor == 2)PORTD &= 0b11101111; //Set output 4 low because the pulse only has to last for 20us } |
To get the accelerometer calibration value, you will also need to run this sketch.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 |
/////////////////////////////////////////////////////////////////////////////////////// //Terms of use /////////////////////////////////////////////////////////////////////////////////////// //THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR //IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, //FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE //AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER //LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, //OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN //THE SOFTWARE. /////////////////////////////////////////////////////////////////////////////////////// #include <Wire.h> byte error, MPU_6050_found, nunchuck_found, lowByte, highByte; int address; int nDevices; void setup() { Wire.begin(); TWBR = 12; Serial.begin(9600); } void loop() { Serial.println("Scanning I2C bus..."); nDevices = 0; for(address = 1; address < 127; address++ ) { Wire.beginTransmission(address); error = Wire.endTransmission(); if (error == 0) { Serial.print("I2C device found at address 0x"); if (address<16)Serial.print("0"); Serial.println(address,HEX); nDevices++; if(address == 0x68 || address == 0x69){ Serial.println("This could be a MPU-6050"); Wire.beginTransmission(address); Wire.write(0x75); Wire.endTransmission(); Serial.println("Send Who am I request..."); Wire.requestFrom(address, 1); while(Wire.available() < 1); lowByte = Wire.read(); if(lowByte == 0x68){ Serial.print("Who Am I responce is ok: 0x"); Serial.println(lowByte, HEX); } else{ Serial.print("Wrong Who Am I responce: 0x"); if (lowByte<16)Serial.print("0"); Serial.println(lowByte, HEX); } if(lowByte == 0x68 && address == 0x68){ MPU_6050_found = 1; Serial.println("Starting Gyro...."); set_gyro_registers(); } } if(address == 0x52){ Serial.println("This could be a Nunchuck"); Serial.println("Trying to initialise the device..."); Wire.beginTransmission(0x52); Wire.write(0xF0); Wire.write(0x55); Wire.endTransmission(); delay(20); Wire.beginTransmission(0x52); Wire.write(0xFB); Wire.write(0x00); Wire.endTransmission(); delay(20); Serial.println("Sending joystick data request..."); Wire.beginTransmission(0x52); Wire.write(0x00); Wire.endTransmission(); Wire.requestFrom(0x52,1); while(Wire.available() < 1); lowByte = Wire.read(); if(lowByte > 100 && lowByte < 160){ Serial.print("Data response normal: "); Serial.println(lowByte); nunchuck_found = 1; } else{ Serial.print("Data response is not normal: "); Serial.println(lowByte); } } } else if (error==4) { Serial.print("Unknown error at address 0x"); if (address<16) Serial.print("0"); Serial.println(address,HEX); } } if (nDevices == 0) Serial.println("No I2C devices found\n"); else Serial.println("done\n"); if(MPU_6050_found){ Serial.print("Balance value: "); Wire.beginTransmission(0x68); Wire.write(0x3F); Wire.endTransmission(); Wire.requestFrom(0x68,2); Serial.println((Wire.read()<<8|Wire.read())*-1); delay(20); Serial.println("Printing raw gyro values"); for(address = 0; address < 20; address++ ){ Wire.beginTransmission(0x68); Wire.write(0x43); Wire.endTransmission(); Wire.requestFrom(0x68,6); while(Wire.available() < 6); Serial.print("Gyro X = "); Serial.print(Wire.read()<<8|Wire.read()); Serial.print(" Gyro Y = "); Serial.print(Wire.read()<<8|Wire.read()); Serial.print(" Gyro Z = "); Serial.println(Wire.read()<<8|Wire.read()); } Serial.println(""); } else Serial.println("No MPU-6050 device found at address 0x68"); if(nunchuck_found){ Serial.println("Printing raw Nunchuck values"); for(address = 0; address < 20; address++ ){ Wire.beginTransmission(0x52); Wire.write(0x00); Wire.endTransmission(); Wire.requestFrom(0x52,2); while(Wire.available() < 2); Serial.print("Joystick X = "); Serial.print(Wire.read()); Serial.print(" Joystick y = "); Serial.println(Wire.read()); delay(100); } } else Serial.println("No Nunchuck device found at address 0x52"); while(1); } void set_gyro_registers(){ //Setup the MPU-6050 Wire.beginTransmission(0x68); //Start communication with the address found during search. Wire.write(0x6B); //We want to write to the PWR_MGMT_1 register (6B hex) Wire.write(0x00); //Set the register bits as 00000000 to activate the gyro Wire.endTransmission(); //End the transmission with the gyro. Wire.beginTransmission(0x68); //Start communication with the address found during search. Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex) Wire.write(0x00); //Set the register bits as 00000000 (250dps full scale) Wire.endTransmission(); //End the transmission with the gyro Wire.beginTransmission(0x68); //Start communication with the address found during search. Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register (1A hex) Wire.write(0x08); //Set the register bits as 00001000 (+/- 4g full scale range) Wire.endTransmission(); //End the transmission with the gyro Wire.beginTransmission(0x68); //Start communication with the address found during search Wire.write(0x1A); //We want to write to the CONFIG register (1A hex) Wire.write(0x03); //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz) Wire.endTransmission(); //End the transmission with the gyro } |
I use parts :
Arduino UNO
A4988
NEMA17 SX17-1005LQCEF – 1,8° (200 Steps on full rotation)
The code you posted does not work. Maybe you know how to modify it to work under these engines?
Hello, What engines did you use in your project?
These are the motors that I used. http://amzn.to/2ftCv3Q
Nice balancing Robot.
Please send all code + libraries?
My email: lamninhan1@gmail.com