Committed for sharing.
Dependencies: mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library
Diff: BalancingRobot.cpp
- Revision:
- 11:0a2944e7be23
- Parent:
- 9:67f2110fce8e
--- a/BalancingRobot.cpp Thu Aug 25 22:55:36 2016 +0000 +++ b/BalancingRobot.cpp Wed Jan 01 20:31:22 2020 +0000 @@ -8,48 +8,73 @@ #include "BalancingRobot.h" #include "MPU6050.h" #include "ledControl.h" -#include "HALLFX_ENCODER.h" +#include "ultrasonic.h" + +//#include "HALLFX_ENCODER.h" /* Setup serial communication */ -Serial blue(p28,p27); // For remote control ps3 +Serial blue(PTE22, PTE23); // For remote control ps3 Serial pc(USBTX, USBRX); // USB /* IMU */ -MPU6050 mpu6050; +MPU6050 mpu6050; + + -/* Encoders*/ -HALLFX_ENCODER leftEncoder(p13); -HALLFX_ENCODER rightEncoder(p14); +/* 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; +Ticker toggler1; + + int main() { + + + + /* Set baudrate */ - blue.baud(9600); - pc.baud(9600); + 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"); + //pc.printf("Calibration is completed. \r\n"); mpu6050.init(); // Initialize the sensor - pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); + //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); @@ -59,9 +84,9 @@ if (abs(pitchAngle)> 20) // Stop if falling or laying down stopAndReset(); else - PID(targetAngle,targetOffset); - - /* Update wheel velocity every 100ms */ + 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. @@ -77,20 +102,37 @@ stopped = true; } } + */ - pc.printf("Pitch: %f, error: %f, PIDValue: %f \n",pitchAngle,error,PIDValue); - pc.printf("Left Wheel Encoder: %f, Right Wheel Encoder: %f \n", LeftWheelPosition, RightWheelPosition); - pc.printf("Wheel Position: %f, Last wheel position:%f, Velocity: %f \n\n", wheelPosition, lastWheelPosition, wheelVelocity); + //pc.printf("Pitch: %f, error1: %f, PIDValue: %f \n\r",pitchAngle,error1,PIDValue); - /* Check for incoming serial data */ + //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(); } } @@ -102,19 +144,26 @@ void PID(double restAngle, double offset) { + /* Steer robot */ - if (steerForward) + /*if (steerForward) { - offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing - restAngle -= offset; - } + /* 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; - } + //offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing + //restAngle += offset; + restAngle += 1.0; + }*/ /* Brake */ - else if (steerStop) + /*else if (steerStop) + + restAngle = 0.0; { //long positionError = wheelPosition - targetPosition; @@ -125,28 +174,29 @@ else // Inside zone C restAngle -= (double)positionError/positionScaleC; */ - restAngle -= (double)wheelVelocity/velocityScaleStop; + //restAngle -= (double)wheelVelocity/velocityScaleStop; - if (restAngle < -5.00) // Limit rest Angle + /*if (restAngle < -5.00) // Limit rest Angle restAngle = -5.00; else if (restAngle > 5.00) restAngle = 5.00; } /* Update PID values */ - error = restAngle - pitchAngle; - double pTerm = Kp * error; - iTerm += Ki * error; - double dTerm = Kd * (error - lastError); - lastError = error; + 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; + double PIDRight; + if (steerLeft) { PIDLeft = PIDValue-turnSpeed; PIDRight = PIDValue+turnSpeed; @@ -160,21 +210,39 @@ 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) - move(left, forward, PIDLeft); + if (PIDLeft >= 0.0) + move(left, backward, PIDLeft + 0.55); else - move(left, backward, PIDLeft * -1); - if (PIDRight >= 0) - move(right, forward, PIDRight); + move(left, forward, (PIDLeft - 0.55) * -1); + if (PIDRight >= 0.0) + move(right, backward, PIDRight + 0.55); else - move(right, backward, PIDRight * -1); + 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; @@ -188,8 +256,24 @@ //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",i); + pc.printf("Input: %c\n",phone_char); // Set all false steerForward = false; @@ -205,11 +289,15 @@ //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 + //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 @@ -224,10 +312,11 @@ } else if (/*input[0]*/phone_char == 'S') { // Stop steerStop = true; stopped = false; - targetPosition = wheelPosition; + //targetPosition = wheelPosition; } else if (/*input[0]*/phone_char == 'A') { // Abort stopAndReset(); while (blue.getc() != 'C'); // Wait until continue is send + } }