File content as of revision 0:efef62b55c86:

/*/least updates:
this program is based on exploration 7 which is PI controller on theta only.
##done steps:
-------------
1)avoiding obstacle.
2)switching modes must be in the navigation function.
3)case if Obs_opp=0.
4)line 413 if Robs=0 or Lobs=0 must be =1.5

##next steps to be done:
-----------------------
1)position of Xtow.
2)dot product check line 431&459.
3)exiting conditions check.
*///
/**
 * this algorithm is Exploration algo.
 */
#include "Motor.h"
#include "QEI.h"
#include"math.h"
#include "SpaceSensor.h"
#include "SHARPIR.h"


#define PPDFactor 0.83638870665653517347249697967419*0.8349813227862008 //pulse per Distance Factor
#define LPPD 84.5374/PPDFactor //Left Pulse Per Distance
#define RPPD 84.0804/PPDFactor //Left Pulse Per Distance

#define delta 55

#define acquizitiontimeinterrupt 0.2
#define navigationtimeinterrupt 0.6
//functions identification
void read_string();
void motor_intialize();
void PWM_cal();
void Distance_calculation();
void Get_Window(int j,float *vec,float *vec2);
void Energy_cal(float *Energy,float *W,float Threshold1[3]);
void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3]);
void move(float Lperiod,float Rperiod);
void navigation_model();
void RotateLeft(float Lperiod,float Rperiod);
void RotateRight(float Lperiod,float Rperiod);
void Stop();
void rotation();
void errorfun();
void GTG_mode();
void FollowWall();
void First_FW();
void Wall_Following();
void min_Obs_opp();
void reset_parameters();
            
DigitalOut myled(LED1);
Serial xbee(p9, p10); //Creates a variable for serial comunication through pin 28 and 27
SHARPIR IR_front(p20); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
SHARPIR IR_right(p19); //the output of the sharpIR sensor is connected to the MBEDs pin 10.
SHARPIR IR_left(p18); //the output of the sharpIR sensor is connected to the MBEDs pin 10.


Motor leftFrontMotor(p21, p16, p15);  //pwm, inB, inA
Motor rightFrontMotor(p22, p12, p11); //pwm, inA, inB
Motor leftBackMotor(p23, p8, p7);  //pwm, inB, inA
Motor rightBackMotor(p24, p6, p5); //pwm, inA, inB

QEI leftBackQei(p25, p26, NC, 151);  //chanA, chanB, index, ppr
QEI rightBackQei(p29, p30, NC, 147); //chanB, chanA, index, ppr
//Tuning parameters calculated from step tests;
//see http://mbed.org/cookbook/PID for examples.
SpaceSensor SS(p13, p14);
//Left motor working variables.
int leftPulses     = 0;
int leftPrevPulses = 0;
float leftVelocity = 0.0;

//Right motor working variables.
int rightPulses     = 0;
int rightPrevPulses = 0;
float rightVelocity = 0.0;

//angle parameters.
float sfgyro=2;
float FIRtheta[3],FIR_W[3],FIR_E[3],sigmoid,error,errorI=0,LTP,RTP;
float FIR_Thresh[3]= {0.00145,0.000674,0.006452};
float EulerAngles[3];
float compass[3];

bool theta_update;
float LPWM,RPWM;
float dis=0,rightDis,leftDis;


//read string parameters
float X,Y,R,Theta_base_st,Fobstacles_dis,Lobstacles_dis,Robstacles_dis;
char Ar_st[30];
char X_coord[10];
char Y_coord[10];
char st[128];

//navigation model parameters
float prev_dis[3];
float post_dis[3];
float current_theta,rot_theta;

//Obasticle avoidance Parameters
float current_Obs_dis,Obs_h,gradient_Obs;
int Obs_factor;
bool FWM,GTGM,ccw,cw;
float Obs_adj,Obs_opp,Obs_hyp,Obs_dis,Obs_alpha_opp,Obs_alpha,FWtheta,Xtow;
float GTGtheta,dot_product_cond;
float h_Obsinv,error_inv;
//time parameters
Timer t;
Ticker timer,timer2;

void get_theta()
{
    angle_cal(FIRtheta,FIR_W,FIR_E,FIR_Thresh);
    SS.ReadfilteredtaredEulerAngles(EulerAngles);
    SS.Readcompass(compass);
    Fobstacles_dis=IR_front.cm();//low range will return -1.0& for far range will return 0.
    Lobstacles_dis=IR_left.cm();
    Robstacles_dis=IR_right.cm();
}


int main()
{
    motor_intialize();
    FWM=false;
    GTGM=true;
    ccw=false;
    cw=false;
    while(1) { //big fat loop
 
        char datain;
//        xbee.puts("press B to begin...");
        datain=xbee.getc();
        if(datain=='b'||datain=='B') {
            t.reset();
            t.start();
//            xbee.puts("enter coordinates in the form of BX'x'Y'y'E...\r\n");
            read_string();
            rot_theta=Theta_base_st;
            navigation_model();
            timer.attach(&get_theta, acquizitiontimeinterrupt);
            timer2.attach(&navigation_model, navigationtimeinterrupt);

            if(theta_update)
                rotation();
            rot_theta=0;
            FIRtheta[0]=FIRtheta[0]-Theta_base_st;
            errorI=0;
            while(1) {//////////////////////////////////////////////////////////////////////////////////////////////////////////////in diffrent modes
                GTG_mode();
                if(abs(R-(dis+prev_dis[0]))<3)break;
            }//whileloop
            timer.detach();
            timer2.detach();
            Stop();
            reset_parameters();
            sprintf( st ,"final:distance%f lp:%d rp:%d %f\r\n",abs((dis+prev_dis[0])),leftPulses,rightPulses,t.read());
            xbee.puts(st);

        }//if condition loop enter b
    }//big fat loop
}//main loop

void read_string()
{
    int i=0;
    char c;
    c=xbee.getc();
    if(c=='B') {
        while(1) {
            c=xbee.getc();
            if(c=='E')break;
            Ar_st[i]=c;
            i++;
            myled != myled;

        }

        int count=0,j=0;
        if(Ar_st[j]=='X') {
            while(1) {
                j++;
                if(Ar_st[j]=='Y')break;
                X_coord[count]=Ar_st[j];
                count++;
            }
            X=atof(X_coord);
            count=0;
        }
        if(Ar_st[j]=='Y') {
            while(1) {
                j++;
                Y_coord[count]=Ar_st[j];
                count++;
                if(j==i)break;
            }
            Y=atof(Y_coord);
            count=0;
        }
    }
    xbee.puts("complete");
    R=sqrt(X*X+Y*Y);
    Theta_base_st=atan2(X,Y);
    sprintf( st ,"Rover will move %f cm & %f cm \r\n",X,Y);
    xbee.puts(st);
    sprintf( st ,"Rover will move %f cm & %f Degree\r\n",R,Theta_base_st);
    xbee.puts(st);

}
void motor_intialize()
{
    leftFrontMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
    rightFrontMotor.period(0.00005);
    leftBackMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
    rightBackMotor.period(0.00005);
}

void errorfun()
{
    error=current_theta-FIRtheta[0];
    errorI=error+errorI;
//    sprintf( st ,"error:%f errprI:%f\r\n",error,errorI);
//    xbee.puts(st);
    float h;
    h=6*error;//+0.01*errorI;
    sigmoid=4/(1+exp(h))-2;
    if(sigmoid<0) {
        RTP=1+sigmoid;
        LTP=1;
    } else if(sigmoid>0) {
        RTP=1;
        LTP=1-sigmoid;
    } else {
        RTP=1;
        LTP=1;
    }
}
void PWM_cal()
{
    errorfun();
    RPWM= 0.9*RTP;
    LPWM= 0.9*LTP;
}
void Distance_calculation()
{
    leftPulses  = leftBackQei.getPulses();
    rightPulses = rightBackQei.getPulses();
    leftDis  = (leftPulses)*2*3.14159*3/LPPD;
    rightDis = (rightPulses)*2*3.14159*3/RPPD;
    dis= (rightDis+leftDis)/2;
}
void Get_Window(int j,float *vec,float *vec2)
{
    float vector1[3]= {0,0,0};
    for(int i=0; i<3; i++) {
        vec[i]=0;
        vec2[i]=0;
    }
    int k=0;
    while(k<j) {
        SS.Readgyros(vector1);
        //SS.Readaccelerometer(vector2);
        for(int i=0; i<3; i++) {
            vec[i]=vector1[i]*vector1[i]+vec[i];
        }
        for(int i=0; i<3; i++) {
            vec2[i]=vector1[i]+vec2[i];
        }
        k++;
    }
    for(int i=0; i<3; i++) {
        vec[i]=vec[i]/(j);
    }
    for(int i=0; i<3; i++) {
        vec2[i]=vec2[i]/(j);
    }
}
void Energy_cal(float *Energy,float *W,float Threshold1[3])
{
    Get_Window(10,Energy,W);
    for(int i=0; i<3; i++) {
        if(Energy[i]<Threshold1[i]*sfgyro)W[i]=0;
    }
    for(int i=0; i<3; i++)W[i]=W[i];
}
void angle_cal(float *theta,float *theta_dot,float *E,float Threshold[3])
{
    Energy_cal(E,theta_dot,Threshold);
    theta[0]=theta_dot[0]*(0.2)+theta[0];
    theta[1]=theta_dot[1]*(0.2)+theta[1];
    theta[2]=theta_dot[2]*(0.2)+theta[2];
}
void move(float Lperiod,float Rperiod)
{
    leftFrontMotor.speed(Lperiod);
    leftBackMotor.speed(Lperiod);

    rightFrontMotor.speed(Rperiod);
    rightBackMotor.speed(Rperiod);
}
void Stop()
{
    leftFrontMotor.brake();
    rightFrontMotor.brake();
    leftBackMotor.brake();
    rightBackMotor.brake();
}

void RotateLeft(float Lperiod,float Rperiod)
{
    leftFrontMotor.speed(-Lperiod);
    leftBackMotor.speed(-Lperiod);

    rightFrontMotor.speed(Rperiod);
    rightBackMotor.speed(Rperiod);
}

void RotateRight(float Lperiod,float Rperiod)
{
    leftFrontMotor.speed(Lperiod);
    leftBackMotor.speed(Lperiod);

    rightFrontMotor.speed(-Rperiod);
    rightBackMotor.speed(-Rperiod);
}
//////////////////////////////////////////////////////////////////////////////////////not used yet
void rotation()
{
    timer2.detach();
    if(current_theta-FIRtheta[0]>FIR_Thresh[0]) {
//        xbee.puts("rotate right\r\n");
        while(FIRtheta[0]<current_theta) { //new_disctrete..........................................................
            RotateRight(0.9,0.9);
            Distance_calculation();
        }
    } else if(current_theta-FIRtheta[0]<-1*FIR_Thresh[0]) {
//        xbee.puts("rotate left\r\n");
        while(FIRtheta[0]>current_theta) {
            RotateLeft(0.9,0.9);
            Distance_calculation();
        }
    } else {
//        xbee.puts("will not rotate\r\n");
    }
    theta_update=false;
    timer2.attach(&navigation_model, navigationtimeinterrupt);
}
void GTG_mode()
{
    if(FWM)Wall_Following();
    PWM_cal();
    move(LPWM,RPWM);
    Distance_calculation();
/*    sprintf( st ,"\r\n\r\nR:%f prev:%f theta:%f GTG:%f FIR:%f Base:%f S:%f LTP:%f RTP:%f\r\n",R,prev_dis[0],current_theta*180/3.14,(current_theta-GTGtheta)*180/3.14,FIRtheta[0]*180/3.14,Theta_base_st*180/3.14,sigmoid,LTP,RTP);
    xbee.puts(st);
    sprintf( st ,"\r\n\r\nF:%f R:%f L:%f OD:%f OA:%f GO:%f ccw:%i cw:%i dot:%f Xtow:%f\r\n",Fobstacles_dis,Robstacles_dis,Lobstacles_dis,Obs_dis,Obs_alpha*180/3.14,gradient_Obs,(int)(dot_product_cond>0),(int)(post_dis[0]<Xtow),dot_product_cond,Xtow);
    xbee.puts(st);
*/
/*    sprintf( st ," theta:%f FIR:%f OA:%f GO:%f 1st:%i 2nd:%i 3rd:%i\r\n",current_theta*180/3.14,FIRtheta[0]*180/3.14,Obs_alpha*180/3.14,gradient_Obs,(int)(dot_product_cond>0),(int)(post_dis[0]<Xtow),(int)(abs(current_Obs_dis)>60));
    xbee.puts(st);
    sprintf( st ,"DA:%f GTGA:%f OD:%f F:%f R:%f L:%f COD:%f\r\n",(current_theta-GTGtheta)*180/3.14,GTGtheta*180/3.14,Obs_dis,Fobstacles_dis,Robstacles_dis,Lobstacles_dis,current_Obs_dis);
    xbee.puts(st);
*/
//sprintf( st ,"\r\n\r\nR:%f prev:%f theta:%f FIR:%f Base:%f S:%f LTP:%f RTP:%f\r\n",R,prev_dis[0],current_theta*180/3.14,FIRtheta[0]*180/3.14,Theta_base_st*180/3.14,sigmoid,leftDis,rightDis);
//xbee.puts(st);  
}
void FollowWall()
{
    if(Fobstacles_dis<delta&&Fobstacles_dis>0&&!(cw&&Robstacles_dis==-1||ccw&&Lobstacles_dis==-1))First_FW();
    //else if(FWM)Wall_Following();

}
void Wall_Following()
{
//    xbee.puts("\r\nWF\r\n");
    if(Obs_dis>0) { //clockwise
        current_Obs_dis=Robstacles_dis;
        cw=true;
        ccw=false;
        if(current_Obs_dis==0||current_Obs_dis==-1)
            current_Obs_dis=150;//check conditions in this line
    } else if(Obs_dis<0) { //anti clock wise
        current_Obs_dis=-1*Lobstacles_dis;
        cw=false;
        ccw=true;
        if(current_Obs_dis==0||current_Obs_dis==1)current_Obs_dis=-150;//check conditions in this line
    }

    Obs_h=current_Obs_dis-Obs_dis;
    gradient_Obs=2/(1+exp(-0.05*Obs_h))-1;
    //current_theta=current_theta+3.14*(gradient_Obs);//from here we shall begin.
    h_Obsinv=-log(4/(gradient_Obs+2)-1);
    error_inv=(h_Obsinv-0.01*errorI)/6;
    current_theta=error_inv+current_theta;
//we should write here exiting condition.
    GTGtheta=atan2(post_dis[2],post_dis[1]);
    dot_product_cond=cos(current_theta-GTGtheta);
    if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) {
        FWM=false;
        GTGM=true;
    }
}
void First_FW()
{
//    xbee.puts("\r\nFFW\r\n");
    Xtow=post_dis[0];//check if there is a proper plase to put Xtow more than this
    Obs_adj=Fobstacles_dis;
    min_Obs_opp();
    Obs_hyp=sqrt(Obs_adj*Obs_adj+Obs_opp*Obs_opp);
    Obs_dis=Obs_adj*Obs_opp/Obs_hyp;

    Obs_alpha_opp=sqrt(Obs_adj*Obs_adj-Obs_dis*Obs_dis);
    Obs_alpha=atan2(Obs_alpha_opp,Obs_dis);

    if(Obs_alpha<3.1415/2)
        Obs_alpha= Obs_alpha-1.57079;
    else if(Obs_alpha>3.1415/2)
        Obs_alpha= 3.1415-Obs_alpha;
    if(Obs_alpha<0.0698&&Obs_alpha>-1*0.0698)
        Obs_alpha=1.57079;
    current_theta=current_theta+Obs_alpha;//FWtheta;
    if(Obs_dis==0)Obs_dis=Obs_adj;//done.

    FWM=true;
    GTGM=false;
    //exiting conditions.
    GTGtheta=atan2(post_dis[2],post_dis[1]);
    dot_product_cond=cos(current_theta-GTGtheta);//dot product check.
    if((dot_product_cond>0)&&(post_dis[0]<Xtow)&&(abs(current_Obs_dis)>60)) {
        FWM=false;
        GTGM=true;
    }
}
void min_Obs_opp()
{
    if(Robstacles_dis<0&&Lobstacles_dis<0)Obs_opp=0;
    else if(Robstacles_dis<0&&Lobstacles_dis>0)Obs_opp=-1*Lobstacles_dis;
    else if(Robstacles_dis>0&&Lobstacles_dis<0)Obs_opp=Robstacles_dis;
    else if(Robstacles_dis>0&&Lobstacles_dis>0)Obs_opp = Lobstacles_dis>Robstacles_dis ? Robstacles_dis : -1*Lobstacles_dis;
}
void navigation_model()   //edit here
{       
    prev_dis[1]=prev_dis[1]+dis*cos(FIRtheta[0]);
    prev_dis[2]=prev_dis[2]+dis*sin(FIRtheta[0]);
    prev_dis[0]=sqrt(prev_dis[1]*prev_dis[1]+prev_dis[2]*prev_dis[2]);

    post_dis[1]=R*cos(rot_theta)-prev_dis[1];
    post_dis[2]=R*sin(rot_theta)-prev_dis[2];
    post_dis[0]=sqrt(post_dis[1]*post_dis[1]+post_dis[2]*post_dis[2]);
    if(FWM&&!GTGM||Fobstacles_dis<delta&&Fobstacles_dis>0&&post_dis[0]>Fobstacles_dis)//check conditions in this line
        FollowWall();
    if(GTGM&&!FWM) {
        current_theta=atan2(post_dis[2],post_dis[1]);
        Obs_alpha=0;
        ccw=false;
        cw=false;
//        xbee.puts("\r\nGTG\r\n");
    }
    leftBackQei.reset();
    rightBackQei.reset();
    dis=0;
    theta_update=true;
}
void reset_parameters()
{
}