Diff: main.cpp

Revision:

0:cfae0986065f

diff -r 000000000000 -r cfae0986065f main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,210 @@
+#include "mbed.h"
+#include "hcsr04.h"
+#include "MPU6050.h"
+#include "ledControl.h"
+#include "Motor.h"
+#include "HALLFX_ENCODER.h"
+
+Serial pc(USBTX, USBRX);       // Create terminal link
+Serial blue(p28, p27);          // Bluetooth TX, RX
+
+MPU6050 mpu6050;               // mpu6050 object from MPU6050 classs
+
+Ticker toggler1;               // Ticker for led toggling
+Ticker filter;                 // Ticker for periodic call to compFilter funcçs 
+Ticker balance;                 // Periodic routine for PID control of balance system
+Ticker speed;                  // Periodic routine for speed control
+Ticker bluetooth;              // Ticker for navigation
+
+Motor A(p23, p5, p6);       // pwm, fwd, rev
+Motor B(p24, p7, p8);       // pwm, fwd, rev
+
+/* Encoders*/
+HALLFX_ENCODER leftEncoder(p13);
+HALLFX_ENCODER rightEncoder(p14);
+
+HCSR04  usensor(p25,p26);   //trig pin,echo pin
+
+/* Function prototypes */
+void toggle_led1();
+void toggle_led2();
+void compFilter();
+void balancePID();
+void balanceControl();
+void Forward(float);
+void Reverse(float);
+void Stop(void);
+void Navigate();
+void TurnRight(float);
+void TurnLeft(float);
+
+/* Variable declarations */
+float pitchAngle = 0;
+float rollAngle = 0;
+bool dir;           // direction of movement
+float Kp = 0.5;
+float Ki = 0.00001;
+float Kd = 0.01;//0.01;//0.05;
+float set_point = 1.005;         // Angle for staying upright
+float errorPID = 0;             // Proportional term (P) in PID
+float last_errorPID =0;         // Previous error value
+float integral = 0;             // Integral (I)
+float derivative = 0;           // Derivative (D)
+float outputPID = 0;            // Output of PID calculations
+float position = 0.0;
+float dist=0.0;
+float range = 0.000;
+int phone_char;
+DigitalOut myled(LED4);
+
+int main()
+{
+    pc.baud(9600);                               // baud rate: 9600
+    blue.baud(9600);
+    
+    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");
+   
+    filter.attach(&compFilter, 0.005);         // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)  
+    balance.attach(&balancePID,  0.010);       // Same period with balancePID
+    speed.attach(&balanceControl, 0.01);
+    bluetooth.attach(&Navigate, 0.05);
+     
+    while(1)
+    {  
+     
+    if(blue.readable()) 
+        {       
+        phone_char = blue.getc();
+        pc.putc(phone_char);
+        pc.printf("Bluetooth Start\r\n");
+        myled = !myled;
+        }
+        
+        pc.printf("Roll Angle: %.1f, Pitch Angle: %.1f\r\n",rollAngle,pitchAngle);
+        pc.printf("Error = %f\r\n",outputPID);
+        wait_ms(40);
+    }    
+}
+
+void toggle_led1() {ledToggle(1);}
+void toggle_led2() {ledToggle(2);}
+
+/* This function is calls the complementary filter with pitch and roll angles */ 
+void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
+
+void balancePID()
+{
+    errorPID = set_point - pitchAngle;          //Proportional (P) 
+    integral += errorPID;                       //Integral (I)
+    derivative = errorPID - last_errorPID;      //Derivative (D)
+    
+    last_errorPID = errorPID;                   //Save current value for next iteration
+    
+    outputPID = (Kp * errorPID) + (Ki * integral) + (Kd * derivative);  //PID calculation
+    
+    /* errorPID is restricted between -1 and 1 */ 
+    if(outputPID > 0.8)
+        outputPID = 0.8;
+    else if(outputPID < -0.8)
+        outputPID = -0.8;   
+}
+
+void balanceControl() 
+{    
+    int direction=0;                                            // Variable to hold direction we want to drive
+    if (outputPID>0)direction=1;                                 // Positive speed indicates forward
+    if (outputPID<0)direction=2;                                 // Negative speed indicates backwards
+    if((abs(pitchAngle)>10.00))direction=0; 
+  
+    switch( direction) {                                        // Depending on what direction was passed
+        case 0:                                                 // Stop case
+            Stop();
+            break;
+        case 1:                                                 // Forward case
+            Forward(abs(outputPID));
+            break;
+        case 2:                                                 // Backwards
+            Reverse(abs(outputPID));
+            break;
+        default:                                                // Catch-all (Stop)
+            Stop();
+            break;
+    }    
+}
+
+void Stop(void)
+{
+    A.speed(0.0);
+    B.speed(0.0); 
+}
+ 
+void Forward(float fwd_dist)
+ {
+    A.speed(-fwd_dist);
+    B.speed(-fwd_dist); 
+ }
+
+void TurnRight(float ryt_dist)
+ {
+     A.speed(ryt_dist);
+     B.speed(-ryt_dist);
+ }
+ 
+void TurnLeft(float lft_dist)
+ {
+     A.speed(-lft_dist);
+     B.speed(lft_dist);
+ }
+
+void Reverse(float rev_dist)
+ {
+    A.speed(rev_dist);
+    B.speed(rev_dist); 
+ }
+ 
+ void Navigate()
+ {
+     switch (phone_char)
+            {
+                case 'f':
+                Forward(0.7);
+                break;
+                
+                case 'F':
+                Forward(0.7);
+                break;
+                
+                case 'b': 
+                Reverse(0.7);
+                break;
+                
+                case 'B': 
+                Reverse(0.7);
+                break;
+                
+                case 'r':
+                TurnRight(0.7);
+                break;
+                
+                case 'R':
+                TurnRight(0.7);
+                break;
+                
+                case 'l':
+                TurnLeft(0.7);
+                break;
+                
+                case 'L':
+                TurnLeft(0.7);
+                break;
+                
+                default:
+                Stop();
+            }
+ }
+     
\ No newline at end of file