File content as of revision 5:48c8fb81288e:

#include "mbed.h"
#include "Motor.h"
#include "Servo.h"
#include "LSM9DS1.h"
#include <math.h>

#define TIMEOUT_VAL_MS 15
#define PI 3.14159
#define DECLINATION -4.94
#define DIST_FACTOR 0.107475 //cm/tick Conversion between a tick from Hall effect sensor and linear distance.
                            //Approximately 190 ticks for a full wheel rotation.
                            //Approximately 6.5cm wheel diameter
                            //pi*diameter (cm) / 190(ticks)
volatile float heading;
float prevHeading[3];
void getHeading(float ax, float ay, float az, float mx, float my, float mz);
float PI_cont(float err);

//MOTOR PINS
Motor rwheel(p22, p18, p17); // pwmA, fwd / Ain2, rev / Ain1
Motor lwheel(p23, p15, p16); // pwmB, fwd / Bin2, rev / 
//BLUETOOTH PINS
Serial bt(p28,p27);
//LEFT SONAR
DigitalOut triggerL(p14);
DigitalIn echoL(p12);
DigitalOut echo1RiseWait(LED1);
DigitalOut echo1FallWait(LED2);
//RIGHT SONAR
DigitalOut triggerR(p13);
DigitalIn echoR(p11);
DigitalOut echo2RiseWait(LED3);
DigitalOut echo2FallWait(LED4);
int correction = 0; //Used to adjust software delay for sonar measurement
Timer sonar;
Timer timeout;

//SERVO PINS
Servo angle(p21);
//IMU PINS
LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
Ticker nav;
float posX = 0;
float posY = 0;
int tickDistL = 0;
int tickDistR = 0;

Serial pc(USBTX, USBRX);
Timer timestamp;

// Set up Hall-effect control
InterruptIn EncR(p25);
InterruptIn EncL(p24);
int ticksR = 0; // Encoder wheel state change counts
int ticksL = 0;
float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
float speedR = 0; //Same for right wheel
int dirL = 1; //1 for fwd, -1 for reverse
int dirR = 1; //1 for fwd, -1 for reverse
Ticker Sampler; //Interrupt Routine to sample encoder ticks.
int tracking = 0; //Flag used to indicate right wheel correction
int sweeping = 0;

//PI controller
//global variables
float kp=12.0f;
float ki=4.0f;
float kd=1.0f;
float P=0;
float I=0;
float D=0;
float dt=0.1f;
float out=0.0f;
float max=0.5f;
float min=-0.5f;
float prerr=0.0f;

//SLAM variables
float angle_list[64];
float dist_list[64];
int slam_ind = 0;
float slam_dist = 0.0f;
//Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
void sampleEncoder()
{
    float E; // Error  between the 2 wheel speeds
    if(tracking) { //Right wheel "tracks" left wheel if enabled. 

        E = ticksL - ticksR; //Find error
        //E = (ticksL+1)/(ticksR+1);
        if (dirL == 1)
        {
            speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
            //speedR = E*speedR;
            if (speedR < 0) speedR = 0;
        }
        else if (dirL == -1)
        {
            speedR = speedR - float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
            //speedR = E*speedR;
            if (speedR > 0) speedR = 0;
        }
        rwheel.speed(speedR);
    }
    //pc.printf("tickL: %i   tickR: %i  Error: %i\n\r ",ticksL, ticksR, ticksL - ticksR);
    ticksR = 0; //Restart the counters
    ticksL = 0;
}

float angleAdjust(float a)
{
    while(a<0 || a>360)
    {
        if (a<0) a+=360;
        if (a>360) a-=360;
    }
    return a;
}
void getHeading(float ax, float ay, float az, float mx, float my, float mz)
{
    float roll = atan2(ay, az);
    float pitch = atan2(-ax, sqrt(ay * ay + az * az));
    //Shift previous headings
    prevHeading[2] = prevHeading[1];
    prevHeading[1] = prevHeading[0];
    prevHeading[0] = heading;
// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
    mx = -mx;
    if (my == 0.0)
        heading = (mx < 0.0) ? 180.0 : 0.0;
    else
        heading = atan2(mx, my)*360.0/(2.0*PI);
    //pc.printf("heading atan=%f \n\r",heading);
    heading -= DECLINATION; //correct for geo location
    heading += 90;
    //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;
    heading = angleAdjust(heading);
    heading = (heading + prevHeading[0] + prevHeading[1] + prevHeading[2])/4.0;
    
}

float PI_cont(float err)
{    
     float Pout=kp*err;
     P=Pout;
     //pc.printf("P is: %f \n\r",P);
     
     float Iout=Iout+err*dt;
     I=ki*Iout;
     //pc.printf("I is: %f \n\r",I);
     
     float Dout = (err - prerr)/dt;
     D=kd*Dout;
     
     out=P+I+D;
     
     prerr=err;
     
     //pc.printf("out is: %f \n\r",out); // basically pwm out
     if (out>max)out=max;
     else if (out<min)out=min;        
     return out;
}

void turn(float angle)
{
        float s =0.05;//speed by which the robot should turn
        while(!imu.magAvailable(X_AXIS));
        imu.readMag();
        while(!imu.accelAvailable());
        imu.readAccel();
        while(!imu.gyroAvailable());
        imu.readGyro();
        getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
                      imu.calcMag(imu.my), imu.calcMag(imu.mz));
        float PV = heading;//the original heading
        float SP = heading - angle;//the new required heading
        if(SP>=360){//reseting after 360 degrees to 0
            SP = SP-360;
            }
        pc.printf("PV: %f\n",PV);
        pc.printf("SV: %f\n",SP);
        wait(2);
        //pc.printf("required: %f\n",requiredHeading);
        float error = angle;
         pc.printf("error: %f\n",error);
         int reached = 0;
         while(reached ==0){
         
                while(!imu.magAvailable(ALL_AXIS));
                imu.readMag();
                getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
                      imu.calcMag(imu.my), imu.calcMag(imu.mz));
                PV = heading;//calculate the current heading
                
                error=(PV-SP)/360;//find error between current heading and required heading
                float correction=PI_cont(error);//correction to motor speed
                     if (error> 1.5/360 || error<-1.5/360) { //an error limit of 1.5 degrees allowed.. checking if the robot needs to turn more
                            rwheel.speed((s+correction)/2);
                            lwheel.speed((s-correction)/2);
                            pc.printf("error in 1st if: %f\n\r",error);
                            pc.printf("error in 1st if: %f\n\r",error);
                            }
                    else if (error>=-1.5/360 && error<=1.5/360) { //within within satisfying angular range 
                             rwheel.speed(0);
                             lwheel.speed(0);//stop moving and exit the loop
                             pc.printf("error in 2nd if: %f\n\r",error);
                             break;
                              }       
                              wait(0.01);                  
                        }
   
    rwheel.speed(0);
    lwheel.speed(0);
}


void sendCmd(char cmd, float arg)
{
    bt.printf("!%c%.2f",cmd, arg);
    //unsigned char c[sizeof arg];
    //memcpy(c, &arg, sizeof arg);
    //bt.putc('!');
    //bt.putc(cmd);
    //bt.putc(c[0]);
    //bt.putc(c[1]);
    //bt.putc(c[2]);
    //bt.putc(c[3]);
}

int ping(int i)
{
    float distance = 0;
    switch(i)
    {
        
        case(1): //Ping Left Sonar
        // trigger sonar to send a ping
        //pc.printf("Pinging sonar 1...\n\r");
        triggerL = 1;
        sonar.reset();
        //wait_us(10.0);
        //wait for echo high
        echo1RiseWait = 1;
        timeout.reset();
        timeout.start();
        while (echoL==0){
            if (timeout.read_ms() > TIMEOUT_VAL_MS) return 0;
            };
        echo1RiseWait = 0;
        //echo high, so start timer
        sonar.start();
        triggerL = 0;
        //wait for echo low
        echo1FallWait = 1;
        timeout.reset();
        timeout.start();
        while (echoL==1) {
            if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0;
            };
        echo1FallWait = 0;
        //stop timer and read value
        sonar.stop();
        //subtract software overhead timer delay and scale to cm
        distance = (sonar.read_us()-correction)/58.0;
        //pc.printf("Got distance %f cm back.\n\r", distance);
        //wait so that any echo(s) return before sending another ping
        wait(0.015);
        break;
        case(2): //Ping Right Sonar
        // trigger sonar to send a ping
        //pc.printf("Pinging sonar 2...\n\r");
        triggerR = 1;
        sonar.reset();
        //wait_us(10.0);
        //wait for echo high
        echo2RiseWait = 1;
        timeout.reset();
        timeout.start();
        while (echoR==0) {
            if (timeout.read_ms() > TIMEOUT_VAL_MS) return 0;
            };
        echo2RiseWait = 0;
        //echo high, so start timer
        sonar.start();
        triggerR = 0;
        //wait for echo low
        echo2FallWait = 1;
        timeout.reset();
        timeout.start();
        while (echoR==1) {
            if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0;
            };
        echo2FallWait = 0;
        //stop timer and read value
        sonar.stop();
        //subtract software overhead timer delay and scale to cm
        distance = (sonar.read_us()-correction)/58.0;
        //pc.printf("Got distance %f cm back.\n\r", distance);
        //wait so that any echo(s) return before sending another ping
        wait(0.015);
        break;
        default:
        break;
    } 
    return distance;
}

void sweep()
{
    __disable_irq();
    float AL = 0.0;
    float AR = 0.0;
    int dL = 0;
    int dR = 0;
    sweeping = 1;
    if (angle <=0.5) //Set to min angle and execute forward sweep.
    {
        angle = 0;
        wait(.2);
        while( angle >= 0 && angle < 1)
        {
            dL = ping(1);
            dR = ping(2);
            AL = angleAdjust(heading + (angle-0.5)*55 - 45);
            AR = angleAdjust(heading + (angle-0.5)*55 + 45);
            if (dL > 20 && dL < 200)
            {
                sendCmd('A',AL);
                sendCmd('D',dL);
                dist_list[slam_ind] = dL;
                angle_list[slam_ind] = AL-heading;
                if (slam_ind < 63) slam_ind++;
                pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL);
            }
            if (dR > 20 && dR < 200)
            {    
                sendCmd('A',AR);
                sendCmd('D',dR);
                dist_list[slam_ind] = dL;
                angle_list[slam_ind] = AR-heading;
                if (slam_ind < 63) slam_ind++;
                pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR);
            }
            angle = angle + 0.05;
        }
        angle = 1;
    }
    else if (angle > 0.5) //Set to max angle and execute backward sweep.
    {
        angle = 1;
        wait(.2);
        while( angle > 0 && angle <= 1)
        {
            dL = ping(1);
            dR = ping(2);
            AL = angleAdjust(heading + (angle-0.5)*55 - 45);
            AR = angleAdjust(heading + (angle-0.5)*55 + 45);
            if (dL > 20 && dL < 200)
            {
                sendCmd('A',AL);
                sendCmd('D',dL);
                dist_list[slam_ind] = dL;
                angle_list[slam_ind] = AL-heading;
                if (slam_ind < 63) slam_ind++;
                pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL);
            }
            if (dR > 20 && dR < 200)
            {    
                sendCmd('A',AR);
                sendCmd('D',dR);
                dist_list[slam_ind] = dL;
                angle_list[slam_ind] = AR-heading;
                if (slam_ind < 63) slam_ind++;
                pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR);
            }
            angle = angle - 0.05;
        }
        angle = 0;
    }
    sweeping = 0;
    __enable_irq();
}

void updatePos()
{
    if(!sweeping)
    {
        float deltaX;
        float deltaY;
        float dist;
        while(!imu.magAvailable(X_AXIS));
        imu.readMag();
        while(!imu.accelAvailable());
        imu.readAccel();
        while(!imu.gyroAvailable());
        imu.readGyro();
        getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
                      imu.calcMag(imu.my), imu.calcMag(imu.mz));
        dist = (tickDistR + tickDistL)/2.0f*DIST_FACTOR;
        dist = (dirL + dirR)*dist/2.0; //Average the ticks and convert to linear distance.
        slam_dist += dist;
       // pc.printf("Magnetic Heading: %f degress    ",heading);
        //pc.printf("L ticks: %d   R ticks: %d       ",tickDistL, tickDistR);
        tickDistR = 0;//Reset the ticks.
        tickDistL = 0;
        deltaX = dist*(float)sin((double)heading*PI/180.0f); //Do trig.
        deltaY = dist*(float)cos((double)heading*PI/180.0f);
        posX = posX + deltaX;
        posY = posY + deltaY;
        //pc.printf("deltaX: %f     deltaY: %f       ",deltaX,deltaY);
        //pc.printf("posX: %f     posY:%f      \n\r",posX,posY);    
        sendCmd('X',posX);
        sendCmd('Y',posY);     
        sendCmd('H',heading);
    }  
}

void incTicksR()
{
    ticksR++;
    tickDistR++;
}

void incTicksL()
{
    ticksL++;
    tickDistL++;
}

void moveFwd()
{
    dirL = 1;
    dirR = 1;
    speedL = 0.9f;
    speedR = 0.9f;
    rwheel.speed(speedR);
    lwheel.speed(speedL);
    tracking = 1; //Turn on wheel feedback updates
    tickDistL = 0; //Reset the distance ticks.
    tickDistR = 0;
}

void moveBwd()
{
    dirL = -1;
    dirR = -1;
    speedL = -0.9f;
    speedR = -0.9f;
    rwheel.speed(speedR);
    lwheel.speed(speedL);
    tracking = 1;
    tickDistL = 0; //Reset the distance ticks.
    tickDistR = 0;
}

void moveStp()
{
    tracking = 0; //Turn off wheel feedback updates
    speedL = 0;
    speedR = 0;
    lwheel.speed(0);
    rwheel.speed(0);
}

void turnL()
{
    tracking = 0;
    dirL = -1;
    dirR = 1;
    speedL = -0.9;
    speedR = .9;
    lwheel.speed(speedL);
    rwheel.speed(speedR);
}

void turnR()
{
    tracking = 0;
    dirL = 1;
    dirR = -1;
    speedL = 0.9;
    speedR = -0.9;
    lwheel.speed(speedL);
    rwheel.speed(speedR);
}

int slam_sweep()
{
    //Reset the SLAM variables;
    memset(angle_list,0,sizeof(angle_list));
    memset(dist_list,0,sizeof(dist_list));
    slam_ind = 0;
    //And sweep
    sweep();
    //Iterate to determine if detection in "danger zone"
    for (int i = 0; i<64; i++)
    {
        if ((angle_list[i]) < 80 && (angle_list[i]) > -80)
        {
            if (dist_list[i] < 50 && dist_list[i] > 1)
            {
                return 1;
                
            }
        }
    }
    return 0;
}

    
void slam()
{
    //Initialize arrays
    int detect = 0;
    wait(1);
    while(1)
    {
        while(!detect)//while an object is not within 30cm of front 60 degrees of robot...
        {
            //move forward 20cm and scan
            moveFwd();
            //while(slam_dist < 20)pc.printf("SlamDist: %f\n\r",slam_dist);
            wait(.8);
            moveStp();
            slam_dist = 0;
            detect = slam_sweep();
        }
        //Else, turn left slightly.
        detect = 0;
        turnL();
        wait(.8);
        moveStp();
        //and scan
        detect = slam_sweep();
    }
    
}

int main() 
{
    char cmd;
    timestamp.start(); 
    angle = 0.0f;
    //Calibrate the IMU
    if (!imu.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    imu.calibrate();
    turnR();
    imu.calibrateMag(0);
    moveStp();
    // Initialize hall-effect control
    Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms
    EncL.mode(PullUp); // Use internal pullups
    EncR.mode(PullUp);
    EncR.rise(&incTicksR);
    EncL.rise(&incTicksL);
    memset(prevHeading, 0, sizeof(prevHeading));
    nav.attach(&updatePos,1);//Update X,Y,H every second
    while(1) 
    {
        //Check if char is ready to be read and put into command buffer;
        if(bt.getc() =='!')
        {
            cmd = bt.getc();
            switch (cmd)
            {
                case 'B':
                //pc.printf("A button was pressed!\n\r");
                cmd = bt.getc();
                switch(cmd)
                {
                    case '5': //Up
                    cmd = bt.getc();
                    switch(cmd)
                    {
                        case '1': //Pressed
                        moveFwd();
                        break;
                        
                        case '0': //Released
                        moveStp();
                        sweep();
                        break;
                    }
                    break;
                    
                    case '6': //Down
                    cmd = bt.getc();
                    switch(cmd)
                    {
                        case '1': //Pressed
                        moveBwd();
                        break;
                        
                        case '0': //Released
                        moveStp();
                        sweep();
                        break;
                    }
                    break;
                    
                    case '7': //Left
                    cmd = bt.getc();
                    switch(cmd)
                    {
                        case '1': //Pressed
                        turnL();
                        break;
                        
                        case '0': //Released
                        moveStp();
                        sweep();
                        break;
                    }
                    break;
                    
                    case '8': //Right
                    cmd = bt.getc();
                    switch(cmd)
                    {
                        case '1': //Pressed
                        turnR();
                        break;
                        
                        case '0': //Released
                        moveStp();
                        sweep();
                        break;
                    }
                    break;
                    
                    case '9': //Right
                    cmd = bt.getc();
                    switch(cmd)
                    {
                        case '1': //Pressed
                        slam();
                        break;
                        
                        case '0': //Released
                        break;
                    }
                    break;
                }
                
                break;
                
                case 'S': //Stop moving
                pc.printf("Got Command STOP!\n\r");
                tracking = 0; //Turn off wheel feedback updates
                speedL = 0;
                speedR = 0;
                lwheel.speed(0);
                rwheel.speed(0);
                break;
                
                case 'F': //Forward
                pc.printf("Got Command FORWARD!\n\r");
                dirL = 1;
                dirR = 1;
                speedL = 0.9f;
                speedR = 0.9f;
                rwheel.speed(speedR);
                lwheel.speed(speedL);
                tracking = 1; //Turn on wheel feedback updates
                tickDistL = 0; //Reset the distance ticks.
                tickDistR = 0;
                break;
                
                case 'R': //Reverse
                pc.printf("Got Command REVERSE!\n\r");
                dirL = -1;
                dirR = -1;
                speedL = -0.9f;
                speedR = -0.9f;
                rwheel.speed(speedR);
                lwheel.speed(speedL);
                tracking = 1;
                tickDistL = 0; //Reset the distance ticks.
                tickDistR = 0;
                break;
                
                case 'P': //Sweep
                pc.printf("Got Command SWEEP!\n\r");
                sweep();
                break;
                
                default:
                pc.printf("Unknown Command!\n\r");
                break;
                
            }
        }
    }
}