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
+        
     }
 }