Prototype program for balancing robot based on various examples from other users.

Dependencies:   HCSR04 MPU6050_1 Motor Servo ledControl2 mbed

Files at this revision

API Documentation at this revision

Comitter:
lakshmananag
Date:
Fri Aug 26 07:11:47 2016 +0000
Commit message:
Prototype program for balancing robot.

Changed in this revision

HALLFX_ENCODER.cpp Show annotated file Show diff for this revision Revisions of this file
HALLFX_ENCODER.h Show annotated file Show diff for this revision Revisions of this file
HCSR04.lib Show annotated file Show diff for this revision Revisions of this file
MPU6050.lib Show annotated file Show diff for this revision Revisions of this file
Motor.lib Show annotated file Show diff for this revision Revisions of this file
Servo.lib Show annotated file Show diff for this revision Revisions of this file
ledControl.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r cfae0986065f HALLFX_ENCODER.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HALLFX_ENCODER.cpp	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,20 @@
+#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 000000000000 -r cfae0986065f HALLFX_ENCODER.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HALLFX_ENCODER.h	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,38 @@
+#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 000000000000 -r cfae0986065f HCSR04.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04.lib	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/prabhuvd/code/HCSR04/#71da0dbf4400
diff -r 000000000000 -r cfae0986065f MPU6050.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/lakshmananag/code/MPU6050_1/#f434122e7695
diff -r 000000000000 -r cfae0986065f Motor.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Motor/#f265e441bcd9
diff -r 000000000000 -r cfae0986065f Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r cfae0986065f ledControl.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ledControl.lib	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/BaserK/code/ledControl2/#1655ee0ec2ca
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
diff -r 000000000000 -r cfae0986065f mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Aug 26 07:11:47 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4f6c30876dfa
\ No newline at end of file