main.cpp

//====================================================//
/*
            Project: Discover Bot
            Authors: Daniel Fulford
                     James Fulford
*/
//====================================================//
// PID code was found at: https://developer.mbed.org/cookbook/mbed-Rover


#include "mbed.h"
#include "Motor.h"
#include "PID.h"
#include "HALLFX_ENCODER.h"
#include "LSM9DS1.h"
#include "rtos.h"
#include "Servo.h"

#define PI 3.14159
#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.

// Various objects:
Motor rightMotor(p26, p18, p15);  //pwm, AIN2, AIN1
Motor leftMotor(p21, p16, p17); //pwm, BIN1, BIN2
HALLFX_ENCODER encR(p28);
HALLFX_ENCODER encL(p27);
LSM9DS1 imu(p9, p10, 0xD6, 0x3C); // Sensor object
AnalogIn IRout(p20);//analog out from sensor to analog in on mbed
Servo myservo(p22);

Serial pc(USBTX, USBRX); //RawSerial for UART communication
RawSerial BLE(p13,p14); // BLE's rx, tx pins

// Leds for communcation notification
DigitalOut led1(LED1);
DigitalOut led4(LED4);

//Tuning parameters calculated from step tests;
//see http://mbed.org/cookbook/PID for examples.
PID leftPid(1.7, 0.45, 2, 0.1);  //Kc, Ti, Td
PID rightPid(1.7, 0.45, 2, 0.1); //Kc, Ti, Td


//=====================================================//
/*
                    Various Variables
*/
//=====================================================//

int desiredHeading = 300; // Heading for the bot to travel along
int leftPulses      = 0; //How far the left wheel has travelled.
int leftPrevPulses  = 0; //The previous reading of how far the left wheel
                         //has travelled.
float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
                            //second.
int rightPulses     = 0; //How far the right wheel has travelled.
int rightPrevPulses = 0; //The previous reading of how far the right wheel
                        //has travelled.
float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
                            //second.
int length     = 1800; //Number of pulses to travel.

bool active = false; // bot sleeps until this changes to true;
bool started = false; // used to check if the user has started the system

//=====================================================//
/*                     
                Calibration/Setup Functions
*/
//=====================================================//

void thread2(void const *argument)
{
    // This thread spins the bot for calibrations
    leftMotor.speed(0.4);
    rightMotor.speed(-0.4);
}

void calibrateIMU()
{
    imu.calibrate(1);
    Thread thread1(thread2); // start spinning bot for calibration
    imu.calibrateMag(0);
    thread1.terminate(); // stop thread
    leftMotor.speed(0.0);
    rightMotor.speed(0.0);   
    encR.reset();
    encL.reset();
    wait(1);    
}

void PIDsetup()
{
    leftMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
    rightMotor.period(0.00005);
    
    //Input and output limits have been determined
    //empirically with the specific motors being used.
    //Change appropriately for different motors.
    //Input  units: counts per second.
    //Output units: PwmOut duty cycle as %.
    //Default limits are for moving forward.
    leftPid.setInputLimits(0, 3200);
    leftPid.setOutputLimits(0.0, 1.0);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(0, 3200);
    rightPid.setOutputLimits(0.0, 1.0);
    rightPid.setMode(AUTO_MODE);

    //Velocity to mantain in pulses per second
    leftPid.setSetPoint(200);
    rightPid.setSetPoint(325);
}

//=====================================================//
/*                     
           Bot Setup for Traveling Functions
*/
//=====================================================//

float calculateHeading()
{
    while(!imu.magAvailable(X_AXIS));
    imu.readMag();
    float mx = imu.calcMag(imu.mx);
    mx = -mx;
    float my = imu.calcMag(imu.my);
    float heading;
    if (my == 0.0)
        heading = (mx < 0.0) ? 180.0 : 0.0;
    else
        heading = atan2(mx, my)*360.0/(2.0*PI);
            
    heading -= DECLINATION; //correct for geo location
    if(heading>180.0) heading = heading - 360.0;
    else if(heading<-180.0) heading = 360.0 + heading;
    else if(heading<0.0) heading = 360.0  + heading;
    
    return heading;
}

void CW(float spd) //clockwise rotation
{   
    leftMotor.speed(spd);
    rightMotor.speed(-spd);
}
void CCW(float spd)
{
    leftMotor.speed(-spd);
    rightMotor.speed(spd);
}
    
    
void rotateToDesired(float spd)
{
    float currHeading = calculateHeading();
    float offset = desiredHeading - currHeading;
    
 if(offset <= 0)//if offset is negative add 360 shift back to correct terminal angle 
    {
        offset = offset + 360;
        if(abs(offset) - 180 < 0 )
            {
            while(abs(calculateHeading()-desiredHeading) >= 2) //turn cw
            {
                CW(spd);
            }
        }
        else //turn cw
        {
           while(abs(calculateHeading()-desiredHeading) >= 2) //turn cw
            {
                CCW(spd);
            } 
        }
    }
    else //if absolute distance is less than 180
    {
        if(abs(offset) - 180 < 0)//and is negative
        {
            while(abs(calculateHeading()-desiredHeading) >= 2) //turn cw
            {
                CW(spd);
            }
        }
        else //turn ccw
        {
           while(abs(calculateHeading()-desiredHeading) >= 2) //turn ccw
            {
                CCW(spd);
            } 
        }
        
    }
    
    leftMotor.brake();
    rightMotor.brake();
    encR.reset();
    encL.reset();
    wait(1);
    
}

void BLE_recv()
{
    
    if(!started)
    {
        
        char trash; // Used to clear any possible garbage data on the buffer
    
        while(!BLE.readable()); // wait for an input to start the system
        started = true;
        while(BLE.readable())
        {
            trash = BLE.getc(); // clear the BLE UART buffer
            wait_ms(100);
        }
    }
    
    
    if(started)
    {
        BLE.printf("Please Enter the Heading.");
        while(!BLE.readable());
        char input1[] = ""; 
        int count = 0;
        
        while(BLE.readable())
        {
            input1[count] = BLE.getc();
            count++;
            led1 = 1;
            wait_ms(50); // to make sure aa data has arrived in buffer before 
                        // accidentally leaving the loop
            led1 = 0;
        }
        
        sscanf(input1, "%d", &desiredHeading); //convert to integer
        
        BLE.printf("Please Enter the Distance.");
        while(!BLE.readable());
        char input2[] = ""; 
        count = 0;
        
        while(BLE.readable())
        {
            input2[count] = BLE.getc();
            count++;
            led1 = 1;
            wait_ms(50); // to make sure aa data has arrived in buffer before 
                        // accidentally leaving the loop
            led1 = 0;
        }
        
        sscanf(input2, "%d", &length);
  
        active = true;
    }   
}

//=====================================================//
/*                     
                Robot Movement Functions
*/
//=====================================================//

void objectAdvoidIR()
{
    float IRvolts = 0.0;//variable to store read voltages from IR sensor 
    float distance;

        IRvolts=IRout.read();//read voltage from analog in (0-1 scale)
        distance = .95/IRvolts*10;
        if(distance < 15)
        {
            
                CCW(0.25);
                wait(1.5);
                leftMotor.speed(0.0);
                rightMotor.speed(0.0);
                wait(0.1);
                leftMotor.speed(0.25);
                rightMotor.speed(0.25);
                wait(2);
                leftMotor.speed(0.0);
                rightMotor.speed(0.0);
                wait(0.1);
                CW(0.25);
                wait(1);
                rotateToDesired(0.3); 
                
        }
       // pc.printf("Current Min Value is: %f\r\n", curMin);
     
}   

void headOnward()
{
    while ((leftPulses < length) || (rightPulses < length)) {

        //Get the current pulse count and subtract the previous one to
        //calculate the current velocity in counts per second.
        leftPulses = encL.read();
        leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
        leftPrevPulses = leftPulses;
        //Use the absolute value of velocity as the PID controller works
        //in the % (unsigned) domain and will get confused with -ve values.
        leftPid.setProcessValue(fabs(leftVelocity));
        leftMotor.speed(leftPid.compute());

        rightPulses = encR.read();
        rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
        rightPrevPulses = rightPulses;
        rightPid.setProcessValue(fabs(rightVelocity));
        rightMotor.speed(rightPid.compute());

        wait(0.01);
        
        //float heading = calculateHeading(); // get the heading from IMU
        //printf("Heading (degrees): %f2.2\r", heading);
        objectAdvoidIR();//use IR for obstacle avoidance and use basic evasion

    }
    
    leftMotor.brake();
    rightMotor.brake();
    encR.reset();
    encL.reset();
    wait(1);
    
}
void scanHorizon()
{
    float IRvolts = 0.0;//variable to store read voltages from IR sensor 
    float distance;
    float oldMin = 100;//set large to be max distance capable  
    float curMin; 
    float p = 0.0;
    float finalPos = 0.0; 

    while(p < 1.0){
        
        p+=0.02;
        myservo = p;
        IRvolts=IRout.read();//read voltage from analog in (0-1 scale)
        distance = .95/IRvolts*10;
        curMin = distance;
        if(curMin < oldMin)
        {
            oldMin = curMin; //set old min value to current one measured
            finalPos = p;
        }
       // pc.printf("Current Min Value is: %f\r\n", curMin);
        //pc.printf("Old Min Value is: %f\r\n", oldMin);
        wait(0.1);
    }
    
       //final position for when IR detected min value
       myservo = finalPos;
       float deg = finalPos*180; //used to indicate final pos (degrees of servo) 
       //display min value
       pc.printf("The Min Value was: %f\r\n", oldMin);
       pc.printf("The heading was: %f\r\n", finalPos*180.0);
       wait(2); 
       myservo = 0.5;
       wait(1);
       
/*  Note:     
    determine the new heading in line with object of interest based off the
    the finalPos of servo 
    the outer conditional statements conceptualizes the front end of the bot as 
    having the desiredheading pointing straight ahead. This means that the 
    approx. 180 deg span infront of bot is the work space of the servo and
    ultimately the IR sensor. Using this and basic mathematical manipulation
    the inner statements were formed. (A picture is much more helpful here)
*/
       int x = 90 - deg;
       int y = abs(deg-90);
       float currHeading = calculateHeading();
       if(x > 0)
       {
           if(desiredHeading - x < 0)
           {
               desiredHeading = currHeading - x + 360;//add 360 for 
               rotateToDesired(0.4);
           }
           else
           {
               desiredHeading = currHeading - x;
               rotateToDesired(0.4);
           }
       }
       else
       {
           if(desiredHeading + y > 360)
           {
               desiredHeading = currHeading + y - 360;
               rotateToDesired(0.4);
           }
           else
           {
               desiredHeading = currHeading + y;
               rotateToDesired(0.4);
           }
       }               
       
}

int main() 
{

    pc.baud(9600);
    BLE.baud(9600);
    BLE.attach(&BLE_recv, Serial::RxIrq);
    
    while(1)
    { 
        if(active)
        {
            pc.printf("%d\r\n",desiredHeading);
            myservo.calibrate(0.001, 180); // needed to ensure the servo rotates the full 180deg
            wait(1);
            myservo = 0.5; // sets IR sensor to front of bot
            
            // Perform IMU steps
            imu.begin();
            calibrateIMU(); // calibrate the IMU sensor
            
            PIDsetup();
        
            rotateToDesired(0.2); // Spin robot to correct heading
            headOnward(); // Move robot in direction of heading with setpoint speed
            myservo.calibrate(0.001, 180); // calibrate again to set up PWM period for 
                                           // servo. When the period is set for DC motors
                                           // this will interfere with how the servo
                                           // operates.
            
            wait(1);
            scanHorizon(); // scan for object in area
            active = false;
        }
    }
}