Committed for sharing.
Dependencies: mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library
BalancingRobot.cpp
- Committer:
- berkyavuz1997
- Date:
- 2020-01-01
- Revision:
- 11:0a2944e7be23
- Parent:
- 9:67f2110fce8e
File content as of revision 11:0a2944e7be23:
/* * The code is released under the GNU General Public License. * This is the algorithm for my balancing robot/segway. * It is controlled by Bluetooth */ #include "mbed.h" #include "BalancingRobot.h" #include "MPU6050.h" #include "ledControl.h" #include "ultrasonic.h" //#include "HALLFX_ENCODER.h" /* Setup serial communication */ Serial blue(PTE22, PTE23); // For remote control ps3 Serial pc(USBTX, USBRX); // USB /* IMU */ MPU6050 mpu6050; /* HC-SR04 */ void dist(int distance) { //put code here to happen when the distance is changed } ultrasonic frontUltra(PTA13, PTD5, .1, 1, &dist); ultrasonic backUltra(PTD0, PTD2, .1, 1, &dist); /* Setup timer instance */ Timer t; /* Ticker for led toggling */ Ticker toggler1; int main() { /* Set baudrate */ blue.baud(115200); pc.baud(115200); /* Set PWM frequency */ leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005) rightPWM.period(0.00005); /* Calibrate the gyro and accelerometer relative to ground */ mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading wait(0.5); mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers //pc.printf("Calibration is completed. \r\n"); mpu6050.init(); // Initialize the sensor //pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); /* Setup timing */ t.start(); loopStartTime = t.read_us(); timer = loopStartTime; DesiredAngle = -2.5; ConstantAngle = DesiredAngle; while (1) { /* Calculate pitch angle using a Complementary filter */ mpu6050.complementaryFilter(&pitchAngle, &rollAngle); wait_us(5); timer = t.read_us(); /* Drive motors */ if (abs(pitchAngle)> 20) // Stop if falling or laying down stopAndReset(); else PID(DesiredAngle, targetOffset); /* Update wheel velocity every 100ms loopCounter++; if (loopCounter%10 == 0) { // If remainder is equal 0, it must be 10,20,30 etc. LeftWheelPosition = leftEncoder.read(); RightWheelPosition = rightEncoder.read(); wheelPosition = LeftWheelPosition + RightWheelPosition; wheelVelocity = wheelPosition - lastWheelPosition; lastWheelPosition = wheelPosition; if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if braking targetPosition = wheelPosition; stopped = true; } } */ //pc.printf("Pitch: %f, error1: %f, PIDValue: %f \n\r",pitchAngle,error1,PIDValue); //Check for incoming serial data if (blue.readable()) // Check if there's any incoming data receiveBluetooth(); frontUltra.startUpdates(); distF = frontUltra.getCurrentDistance(); backUltra.startUpdates(); distB = backUltra.getCurrentDistance(); pc.printf("Front distance: %d cm \t Back Distance %d cm \r\n", distF, distB); /* Use a time fixed loop */ lastLoopUsefulTime = t.read_us() - loopStartTime; if (lastLoopUsefulTime < STD_LOOP_TIME) wait_us(STD_LOOP_TIME - lastLoopUsefulTime); lastLoopTime = t.read_us() - loopStartTime; // only used for debugging //pc.printf("STD_LOOP_TIME: %d\n\r",STD_LOOP_TIME); //pc.printf("anlik: %d\n\r",t.read_us()); //pc.printf("lastLoopTime: %d\n\r",lastLoopTime); //pc.printf("lastLoopUsefulTime: %d\n\r",lastLoopTime); loopStartTime = t.read_us(); } } void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);} void toggle_led1() {ledToggle(1);} void toggle_led2() {ledToggle(2);} void PID(double restAngle, double offset) { /* Steer robot */ /*if (steerForward) { /* offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing restAngle -= offset; restAngle -= -1.0; } else if (steerBackward) { //offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing //restAngle += offset; restAngle += 1.0; }*/ /* Brake */ /*else if (steerStop) restAngle = 0.0; { //long positionError = wheelPosition - targetPosition; /* if (abs(positionError) > zoneA) // Inside zone A restAngle -= (double)positionError/positionScaleA; else if (abs(positionError) > zoneB) // Inside zone B restAngle -= (double)positionError/positionScaleB; else // Inside zone C restAngle -= (double)positionError/positionScaleC; */ //restAngle -= (double)wheelVelocity/velocityScaleStop; /*if (restAngle < -5.00) // Limit rest Angle restAngle = -5.00; else if (restAngle > 5.00) restAngle = 5.00; } /* Update PID values */ error1 = restAngle - pitchAngle; double pTerm = Kp * error1; iTerm += Ki * error1; double dTerm = Kd * (error1 - lastError); lastError = error1; PIDValue = pTerm + iTerm + dTerm; //pc.printf("DesiredAngle = %lf", DesiredAngle); //pc.printf("Pitch: %5.2f\tPIDValue: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,Kp,Ki,Kd); /* Steer robot sideways */ double PIDLeft; double PIDRight; if (steerLeft) { PIDLeft = PIDValue-turnSpeed; PIDRight = PIDValue+turnSpeed; } else if (steerRotateLeft) { PIDLeft = PIDValue-rotateSpeed; PIDRight = PIDValue+rotateSpeed; } else if (steerRight) { PIDLeft = PIDValue+turnSpeed; PIDRight = PIDValue-turnSpeed; } else if (steerRotateRight) { PIDLeft = PIDValue+rotateSpeed; PIDRight = PIDValue-rotateSpeed; } else { if (steerForward && (DesiredAngle > ( ConstantAngle - 1.8 ) )) DesiredAngle -= rotateSpeed; else if (steerBackward && (DesiredAngle < (ConstantAngle + 1.8) )) DesiredAngle += rotateSpeed; else { if(!steerForward && DesiredAngle < ConstantAngle) DesiredAngle += rotateSpeed; else if(!steerBackward && DesiredAngle > ConstantAngle) DesiredAngle -= rotateSpeed; } PIDLeft = PIDValue; PIDRight = PIDValue; } //PIDLeft *= 0.95; // compensate for difference in the motors /* Set PWM Values */ if (PIDLeft >= 0.0) move(left, backward, PIDLeft + 0.55); else move(left, forward, (PIDLeft - 0.55) * -1); if (PIDRight >= 0.0) move(right, backward, PIDRight + 0.55); else move(right, forward, (PIDRight - 0.55) * -1); //pc.printf("RestAngle3 %lf" ,restAngle); } void receiveBluetooth() { //char input[16]; // The serial buffer is only 16 characters char phone_char; //int i = 0; //while (1) { //input[i] = blue.getc(); //if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer //break; //i++; //pc.printf("Im here.."); //} phone_char = blue.getc(); /* if(distB < 50 && distB > 4) { phone_char = 'S'; if (distB < 20) phone_char = 'F'; } */ if(distB < 20 && distB > 4) phone_char = 'F'; else if ( distB < 30) phone_char = 'S'; else if (distB < 50) phone_char = 'B'; pc.putc(phone_char); pc.printf("Input: %c\n",phone_char); // Set all false steerForward = false; steerBackward = false; steerStop = false; steerLeft = false; steerRotateLeft = false; steerRight = false; steerRotateRight = false; /* For remote control */ if (/*input[0]*/phone_char == 'F') { // Forward //strtok(/*input*/phone_char, ","); // Ignore 'F' //targetOffset = atof((strtok(NULL, ";")); // read until the end and then convert from string to double //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging //DesiredAnglee = 0.8; pc.printf("F"); steerForward = true; } else if (/*input[0]*/phone_char == 'B') { // Backward //strtok(/*input*/phone_char, ","); // Ignore 'B' //targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging //DesiredAnglee = -5.2; pc.printf("B"); steerBackward = true; } else if (/*input[0]*/phone_char == 'L') { // Left if (/*input[0]*/phone_char == 'L') // Left Rotate steerRotateLeft = true; else steerLeft = true; } else if (/*input[0]*/phone_char == 'R') { // Right if (/*input[0]*/phone_char == 'R') // Right Rotate steerRotateRight = true; else steerRight = true; } else if (/*input[0]*/phone_char == 'S') { // Stop steerStop = true; stopped = false; //targetPosition = wheelPosition; } else if (/*input[0]*/phone_char == 'A') { // Abort stopAndReset(); while (blue.getc() != 'C'); // Wait until continue is send } } void stopAndReset() { stop(both); lastError = 0; iTerm = 0; targetPosition = wheelPosition; } void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f) if (motor == left) { leftPWM = speed; if (direction == forward) { leftA = 0; leftB = 1; } else if (direction == backward) { leftA = 1; leftB = 0; } } else if (motor == right) { rightPWM = speed; if (direction == forward) { rightA = 0; rightB = 1; } else if (direction == backward) { rightA = 1; rightB = 0; } } else if (motor == both) { leftPWM = speed; rightPWM = speed; if (direction == forward) { leftA = 0; leftB = 1; rightA = 0; rightB = 1; } else if (direction == backward) { leftA = 1; leftB = 0; rightA = 1; rightB = 0; } } } void stop(Motor motor) { if (motor == left) { leftPWM = 1; leftA = 1; leftB = 1; } else if (motor == right) { rightPWM = 1; rightA = 1; rightB = 1; } else if (motor == both) { leftPWM = 1; leftA = 1; leftB = 1; rightPWM = 1; rightA = 1; rightB = 1; } }