Revision 11:0a2944e7be23, committed 2020-01-01

Comitter:

berkyavuz1997

Date:

Wed Jan 01 20:31:22 2020 +0000

Parent:

10:8050817ae610

Commit message:

Committed for sharing.

Changed in this revision

diff -r 8050817ae610 -r 0a2944e7be23 BalancingRobot.cpp
--- 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
+        
     }
 }
 

diff -r 8050817ae610 -r 0a2944e7be23 BalancingRobot.h
--- a/BalancingRobot.h	Thu Aug 25 22:55:36 2016 +0000
+++ b/BalancingRobot.h	Wed Jan 01 20:31:22 2020 +0000
@@ -6,18 +6,14 @@
 BusOut LEDs(LED1, LED2, LED3, LED4);
 
 /* Left motor */
-DigitalOut leftA(p5);
-DigitalOut leftB(p6);
-PwmOut leftPWM(p23);
+DigitalOut leftA(D6);
+DigitalOut leftB(D5);
+PwmOut leftPWM(D7);
 
 /* Right motor */
-DigitalOut rightA(p7);
-DigitalOut rightB(p8);
-PwmOut rightPWM(p24);
-
-/* Encoder variables */
-float LeftWheelPosition = 0.0;
-float RightWheelPosition = 0.0;
+DigitalOut rightA(D4);
+DigitalOut rightB(D3);
+PwmOut rightPWM(D2);
 
 // Results
 double accYangle;
@@ -26,15 +22,24 @@
 int16_t raw_gyroYrate;
 
 /* PID variables */
-double Kp = 0.5; //11 - 7
-double Ki = 0.00001; //1
-double Kd = 0.01; //12
+
+double Kp = 0.085; //0.100; //0.0400; // 11 - 7
+double Ki = 0.00900; //0.005;//0.00320; //1 0.001 sanki gideri var mı ne
+double Kd = 0.050;//0.0125 baya iyi //0.0333; //12
+
+//double Kp = 0.185; //11 - 7 almost well   //0.1
+//double Ki = 0.0000; //1                   //0.005  integralli ilk iyi gibi
+//double Kd = 0.1; //12                     //0.01
+
 double PIDValue = 0.0;
-double targetAngle = 1.05;
+double DesiredAngle;
+double ConstantAngle;
+unsigned int distF;
+unsigned int distB;
 double targetOffset = 0;
 float pitchAngle = 0;
 float rollAngle = 0;
-double error = 0.0;
+double error1 = 0.0;
 double lastError = 0.0;
 double iTerm;
 

diff -r 8050817ae610 -r 0a2944e7be23 HALLFX_ENCODER.cpp
--- a/HALLFX_ENCODER.cpp	Thu Aug 25 22:55:36 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,20 +0,0 @@
-#include "HALLFX_ENCODER.h"
- 
-HALLFX_ENCODER::HALLFX_ENCODER(PinName enc_in): _enc_in(enc_in){
-    _enc_in.mode(PullUp);
-    // Invoke interrupt on both falling and rising edges
-    _enc_in.fall(this, &HALLFX_ENCODER::callback);
-    _enc_in.rise(this, &HALLFX_ENCODER::callback);
-}
- 
-long HALLFX_ENCODER::read(){
-    return count;
-}
- 
-void HALLFX_ENCODER::reset(){
-    count = 0;
-}
- 
-void HALLFX_ENCODER::callback(){
-    count++;   
-}
\ No newline at end of file

diff -r 8050817ae610 -r 0a2944e7be23 HALLFX_ENCODER.h
--- a/HALLFX_ENCODER.h	Thu Aug 25 22:55:36 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,38 +0,0 @@
-#ifndef HALLFX_ENCODER_H
-#define HALLFX_ENCODER_H
- 
-/*
-    Basic Encoder Library for Sparkfun's Wheel Encoder Kit
-    Part# ROB-12629.
-*/
- 
-#include "mbed.h"
- 
-class HALLFX_ENCODER{
-    public:
-        /*
-            Constructor for Encoder objects
-            @param enc_in    The mBed pin connected to encoder output
-        */
-        HALLFX_ENCODER(PinName enc_in);
-        /*
-            read() returns total number of counts of the encoder.
-            Count can be +/- and indicates the overall direction,
-            (+): CW (-): CCW
-            @return     The toltal number of counts of the encoder.
-        */
-        long read();
-        /*
-            reset() clears the counter to 0. 
-        */
-        void reset();
-    private:
-        long count;         // Total number of counts since start.
-        InterruptIn _enc_in;// Encoder Input/Interrupt Pin
-        /*
-            Increments/Decrements count on interrrupt.
-        */
-        void callback();    // Interrupt callback function
-};
- 
-#endif
\ No newline at end of file

diff -r 8050817ae610 -r 0a2944e7be23 HC_SR04_Ultrasonic_Library.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HC_SR04_Ultrasonic_Library.lib	Wed Jan 01 20:31:22 2020 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3

diff -r 8050817ae610 -r 0a2944e7be23 MPU6050.lib
--- a/MPU6050.lib	Thu Aug 25 22:55:36 2016 +0000
+++ b/MPU6050.lib	Wed Jan 01 20:31:22 2020 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/lakshmananag/code/MPU6050/#c3d0207e85ca
+https://os.mbed.com/teams/Berk-Ay/code/SelfBalancingRobot_BerkYasarYavuz_AdilBe/#cccf074499d0

diff -r 8050817ae610 -r 0a2944e7be23 mbed.bld
--- a/mbed.bld	Thu Aug 25 22:55:36 2016 +0000
+++ b/mbed.bld	Wed Jan 01 20:31:22 2020 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/14f4805c468c
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file