File content as of revision 0:cfae0986065f:

#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();
            }
 }