File content as of revision 0:35cf52223527:

#include "mbed.h" 
#include "Servo.h"
#include "Motor.h"
#include "PID.h"
#include "rtos.h"
#include <stdlib.h>

#define PI 3.1415926
#define threshold 27


//objects
Motor rightMotor(p24, p28, p27); // pwm, fwd(in1), rev(in2) Motor B (right wheel)
Motor leftMotor(p22, p29, p30); // pwm, fwd(in1), rev(in2) Motor A (left wheel)
Timer timer;
AnalogIn ain(p20);
AnalogIn ainL(p18);
AnalogIn ainR(p19);
Servo myservo(p26);
Serial pc(USBTX, USBRX);
Serial xbee1(p13, p14);
Mutex wheelMutex; 


//globals
float max_angle = 0.0;
int leftPrevPulses  = 0;
int leftActPulses=0; //pulses from left motor
float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per second
int rightPrevPulses = 0;
int rightActPulses=0; //pulses from right motor
float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
int turnDistance     = 0; //Number of pulses to travel.
int distanceR = 0; // in mm
int distanceL = 0; // in mm
int distanceRold = 0; // in mm
int distanceLold = 0; // in mm
float X  = 0.0;
float Y = 0.0;
float inc_X = 1.0;
float inc_Y = 0.0;
float cur_dir = 0.0;
bool go;
int DistancesR[4];
int DistancesL[4];


//functions
void lookaround(void);
void go_fwd(void);
void turn_R(float);
void turn_L(float);
void stop(void);
void distR(int);
void distL(int);
int distTransform(float input);
void tunePID(void);
float find_avg(float a[5]);
float to_Rad(float);
void go_fwd_thread(void const *argument);
void grab_init_direct(void);

class Counter {     //interrupt driven rotation counter to be used with the feedback control
    public:
    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
        _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
    }
 
    void increment() {
        _count++;
    }
 
    int read() {
        return _count;
    }
 
private:
    InterruptIn _interrupt;
    volatile int _count;
};

Counter leftPulses(p9), rightPulses (p10);
//PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
//PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td

PID leftPid(0.462, 0.1, 0.2, 0.01);  //Kc, Ti, Td
PID rightPid(0.462, 0.1, 0.1, 0.01); //Kc, Ti, Td

int main() 
{
    tunePID();
    xbee1.baud(115200);
    myservo = 0.65;
    leftPid.setSetPoint(975);  //set velocity goals for PID
    rightPid.setSetPoint(990);
    Thread fwdThread(go_fwd_thread);
    grab_init_direct();
    go = true;
    float middle[5];
    float left[5];
    float right[5];
    
    while(1){
        float mid_avg = 0.0; 
        float left_avg = 0.0;
        float right_avg = 0.0;
        timer.start();
        while (mid_avg <0.4 && left_avg <0.7 && right_avg <0.4) 
        {
            middle[0] = ain;
            middle[1] = ain;
            middle[2] = ain;
            middle[3] = ain;
            middle[4] = ain;
            mid_avg = find_avg(middle);
            
            left[0] = ainL;
            left[1] = ainL;
            left[2] = ainL;
            left[3] = ainL;
            left[4] = ainL;
            left_avg = find_avg(left);
            
            right[0] = ainR;
            right[1] = ainR;
            right[2] = ainR;
            right[3] = ainR;
            right[4] = ainR;
            right_avg = find_avg(right);
            
            X = X + inc_X;
            Y = Y + inc_Y;
           xbee1.printf("+\n");
           xbee1.printf("%.2f\n%.2f\n" ,X ,Y);   
        }
        go = false;
        stop();
        xbee1.printf("*\n");
        xbee1.printf("%4d",timer.read_ms());
        timer.stop();
        timer.reset();
        lookaround();
        //fwdThread.signal_set(0x1);
        
        if (max_angle >= 45.0) 
        {
            turn_R((max_angle - 45.0));            
            cur_dir = cur_dir - (max_angle - 45.0);
            inc_X = cos(to_Rad(cur_dir));
            inc_Y = sin(to_Rad(cur_dir));
            xbee1.printf("R\n");
            xbee1.printf("%2.1f",(max_angle - 45.0));
        }
        else if (max_angle < 45.0) 
        {
            turn_L((45.0 - max_angle));
            cur_dir = cur_dir + (45.0 - max_angle);
            inc_X = cos(to_Rad(cur_dir));
            inc_Y = sin(to_Rad(cur_dir));
            xbee1.printf("L\n");
            xbee1.printf("%2.1f",(45.0 - max_angle));
        }
        
        myservo = 0.65;
        stop();         
        wait(0.1);
        go = true;
        
    } // End of while(1)loop - main thread
}// End of main()

void grab_init_direct(void)
{
    char initdirect[4];
    int Count = 0;
    initdirect[Count] = xbee1.getc();
    while(initdirect[Count] != '+')
    {
        Count++;
        initdirect[Count] = xbee1.getc();
    }
    
    char initdirect2[Count];
    for(int i=0; i<Count; i++)
    {
        initdirect2[i] = initdirect[i];
    }
    
    float init_turn = atof(initdirect2);
    cur_dir = init_turn;
    turn_R(init_turn);
        
}

float find_avg(float a[5])
{
    return ((a[0] + a[1] + a[2] + a[3] + a[4])/5.0);
}

void tunePID(void){
    leftPid.setInputLimits(0, 3000);    
    leftPid.setOutputLimits(0.0, 0.5);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(0, 3000);
    //rightPid.setOutputLimits(0.0, 0.465);
    rightPid.setOutputLimits(0.0, 0.48);
    rightPid.setMode(AUTO_MODE);
}


void stop(void)
{
    wheelMutex.lock();
    
    leftMotor.speed(0.0);    
    rightMotor.speed(0.0);
    
    wheelMutex.unlock();
}


void turn_R(float deg)
{
    wheelMutex.lock();
    
    leftActPulses=leftPulses.read();
    rightActPulses=rightPulses.read();
    turnDistance=rightActPulses+(int)deg;     
    while (rightActPulses<turnDistance) { //turn approximately half a revolution
    leftMotor.speed(0.4);  //rotate to the right
    rightMotor.speed(-0.4);  //open loop, because the PID class can't handle negative values
    leftActPulses=leftPulses.read();
    rightActPulses=rightPulses.read();
    wait(0.005);
    }
    
    wheelMutex.unlock();
}
    
void turn_L(float deg)
{
    
    wheelMutex.lock();
    
    leftActPulses=leftPulses.read();
    rightActPulses=rightPulses.read();
    turnDistance=leftActPulses+(int)deg;     
    while (leftActPulses<turnDistance) { //turn approximately half a revolution
    leftMotor.speed(-0.4);  //rotate to the right
    rightMotor.speed(0.4);  //open loop, because the PID class can't handle negative values
    leftActPulses=leftPulses.read();
    rightActPulses=rightPulses.read();
    wait(0.005);
    }
    
    wheelMutex.unlock();
}
 
void lookaround(void) //returns a value between 0 and 90 that represents the best path in the cone of observation
{
    float max;
    max_angle = 0.0;
    max = ain;
    for(float p=0; p<1.0; p += 0.05) 
    {
        myservo = p;
        if (max > ain)
            {
                max = ain;
                max_angle = p;
            }
            
        //wait(0.25);
        wait(0.1);
    }
    
    if (max>0.45){
        turn_L(180.0);
        myservo = 0.65;
        wait(0.5);
        }
    else{        
    myservo = max_angle;
    wait(0.5);
    max_angle = max_angle*90.0 - 13.5;
    //max_angle = max_angle*90.0;
    }    
   
}

float to_Rad(float deg)
{
 return deg * ((PI)/180);   
}

void go_fwd_thread(void const *argument) {
    while(1){
        if(!go) {Thread::yield();}
        while (go) {
            wheelMutex.lock();
            
            leftActPulses=leftPulses.read();
            leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
            leftPrevPulses = leftActPulses;
            rightActPulses=rightPulses.read();
            rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
            rightPrevPulses = rightActPulses;    
            leftPid.setProcessValue(fabs(leftVelocity));
            leftMotor.speed(leftPid.compute()+0.0075);
            rightPid.setProcessValue(fabs(rightVelocity));
            rightMotor.speed(rightPid.compute());
            
            wheelMutex.unlock();
        }
    }
}