File content as of revision 4:c93e1ecd3359:

#include "mbed.h"
#include "L3GD20.h"
#include "LSM303DLHC.h"
#include "Servo.h"
#include "math.h"
#include "PwmIn.h"

#include "iSerial.h"
#include "iostream"
#include "stdio.h"
#include "string.h"

#include <string>
#include <sstream>
#define TS 10                    // sample time [ms]
#define SIMULATION_TIME 10        // simulation time [s]



//serial 
Serial BB(p13, p14);  // tx, rx
Serial pc(USBTX,USBRX);

//period_pwm setup as 20ms
//speed: 0 is off, and 1 is full speed；for servo 0-0.2 duty circle
//direction: 0 clockwise, 1 counter-clockwise

float period_pwm_ac; // Actuator current
float period_pwm;

bool directionA;
bool directionB;
bool directionC;
bool directionE;

float speedA;
float speedB;
float speedC;
float Tail_increasement=0.0;

float AngleLeft=0.08;
float AngleRight=0.04;
float AngleTail=0.08;
float Tailbias=0.0;
float AngleTail_stand_bias=0.0;

long int FA=1000000,FB=1000000,FE=4,CountA=0,CountB=0,CountE=0,CountT=0;

int flag;
int x,y; // Image coordinate

//funtion declaration
void move(int motor, float speed, int direction);
PwmOut PWMA(p25);//LEFT  FIN
PwmOut PWMB(p23);//RIGHT FIN
PwmOut PWMC(p24);//Left servo
PwmOut PWMD(p26);//Right servo
PwmOut PWME(p21);//Tail

DigitalOut STBY (p30);
//DigitalOut STBY2 (p29);
DigitalOut AIN1 (p8);
DigitalOut AIN2 (p11); // Fin1 left
DigitalOut BIN1 (p7);  
DigitalOut BIN2 (p6);  // Fin2 right
DigitalOut myLed (LED1);
DigitalOut myLed1(LED2);


 

void moveA()
{
    STBY=1; //disable standby
    int inPin1=1;
    int inPin2=0;
    if(directionA==0) {
        inPin1 = 0;
        inPin2 = 1;
    }
    AIN1=inPin1;
    AIN2=inPin2;
    //directionA=!directionA;

    //pc.printf("dirA = %d\n\r",directionA);


    if(directionA)
    {
        //speedA=0.4;
        PWMA.pulsewidth(period_pwm*(speedA*1.5));
        //PWMA.pulsewidth(period_pwm*(0.6));
        }
        else
        {
            //speedA=0.2;
            PWMA.pulsewidth(period_pwm*speedA*1.5);
            }
    //PWMA.pulsewidth(period_pwm*speedA);

}

void moveB()
{
    STBY=1; //disable standby
    int inPin1=0;
    int inPin2=1;
    if(directionB==0) {
        inPin1 = 1;
        inPin2 = 0;
    }
    //pc.printf("dirB = %d\n\r",directionB);
    BIN1=inPin1;
    BIN2=inPin2;
    //directionB=!directionB;

    if(!directionB)
    {
        //speedB=0.4;
        PWMB.pulsewidth(period_pwm*(speedB*1.5));
        //PWMB.pulsewidth(period_pwm*(0.6));
        }
        else
        {
            //speedB=0.2;
            PWMB.pulsewidth(period_pwm*speedB*1.5);
            }
    //PWMB.pulsewidth(period_pwm*speedB);

}


void moveC()
{
    STBY=1; //disable standby
    PWMC.pulsewidth(period_pwm*AngleLeft);//0.08 mid 0.04 back

}
void moveD()
{
    STBY=1; //disable standby
    PWMD.pulsewidth(period_pwm*AngleRight);//0.08 mid 0.04 forward

}

void moveE()
{
 /*
 if(directionE)
 {
     AngleTail=0.082+Tailbias;
     directionE=!directionE;
 }
 else
 {
     AngleTail=0.068+Tailbias;
     directionE=!directionE;
 }
 */
 /*
 if(Tailbias==0)
 {
   AngleTail=0.075;
   }
   */
    AngleTail=0.075+Tailbias;
   //AngleTail=0.12;
    //AngleTail=0.075;
    STBY=1; //disable standby
    PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
}


/*
void moveE_stand()
{

     AngleTail=0.075+AngleTail_stand_bias;
 
   //AngleTail=0.12;
    //AngleTail=0.08;
    STBY=1; //disable standby
    PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
}
*/
void stop()
{
//enable standby
    STBY=0;;
}

int main()
{
    //serial to BBB setup    
    char str[9];
    char *token;
 
    pc.baud(9600); 
    BB.baud(9600); 
    
    //Actuator & servo setup
    
        period_pwm_ac=0.0020;                   //500hz
        period_pwm=0.020;                      //20ms
        PWMA.period(period_pwm_ac);          
        PWMB.period(period_pwm_ac);          
        PWMC.period(period_pwm);                // servo requires a 20ms period
        PWMD.period(period_pwm);                // servo requires a 20ms period
        PWME.period(period_pwm);                // servo requires a 20ms period
        
    //initial direction & current    
        directionA=1;
        directionB=0;
        directionE=1;
        flag=1; 
        speedA=0.2;
        speedB=0.2; //Actuator speed control
        
        while(1)
{
            
            
//         moveA();
//         moveB();
//         wait(0.2);
 //        pc.printf(" x=%d y=%d", x,y);
 //        pc.printf(str);
    
    //Talk to BBB
    
         int i;
         if(BB.readable()>0) 
         {
            for(i=0;i<9;i++) 
            str[i] = BB.getc();
           
            if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C))
            {
                token = strtok(str, ",");
                x = atoi(token);                //100-420(width_320);No target:555 
    
                token = strtok(NULL, ",");
                y = atoi(token);                //100-340(height_240); No target:555 
                //pc.printf("%d \r \n",y);
                //pc.printf("\n");
              //pc.printf(str);
            }
         }
         /*
         if(y<300&&y>100)
         {
           Tailbias=(y-200)*0.0006;
           }
           
           else
           {
               Tailbias=0;
               }
               */
         FA=20;//5;
         FB=20;//5;  
         /*
         if(CountT>300&&CountT<=600)
         {
         speedA=0.6;
         Tailbias=0;//(160-200)*0.0006;
         speedB=0.0;
         CountT++;
         }
         else if(CountT<=300)
         {
           speedB=0.6;
           Tailbias=0;//(200-160)*0.0006;
           speedA=0.0;
           CountT++;
             }
             else
             {
                 CountT=0;
                 }
                 */
          
         if(y<350&&y>200)
         {
           
           speedA=(y-200)*0.006; 
           Tail_increasement=speedA*0.0012;
           Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006;
           speedB=0.0; 
             }
             else if (y>50&&y<=200)
             {
                 speedB=(200-y)*0.006;
                 Tail_increasement=speedB*0.0012;
                 Tailbias=Tailbias+Tail_increasement;//0.0;//(y-200)*0.0006;
                 speedA=0.0;
                 }
                 else
                 {
                     speedA=0.0;
                     speedB=0.0;
                     //Tailbias=0.0;
                     }
                     
         if(Tailbias>0.02)
         {
                     Tailbias=0.02;
                     }
         if(Tailbias<-0.02)
         {
                     Tailbias=-0.02;
                     }
                     
                   
         /*
         if(y>210&&y<400)
         {
             //FA=2000000;
             //FB=200000;
             FA=20;//FA=40;
             FB=5;
             speedB=0.6;
             speedA=0.02;        
             //Tailbias=(y-210)*0.0003;     
             //Tailbias=0.03;
             //AngleTail_stand_bias=0.03;
             //moveE_stand();
             }
             
         else if(y<190&&y>100)
         {
             FB=20;//FB=40;
             FA=5;             
             speedA=0.6;
             speedB=0.02;
             //Tailbias=-0.03;
             //AngleTail_stand_bias=-0.03;
             //moveE_stand();
             }
             else
             {
                 FA=20;
                 FB=20;
                 directionB=!directionA;
                 CountB=CountA;
                 speedA=0.02;
                 speedB=0.02;
                 Tailbias=0;
                 AngleTail_stand_bias=0.0;
                 //moveE_stand();
                 }
                 */
                 /*
             FA=20;//FA=40;
             FB=5;
             speedB=0.6;
             speedA=0.01;             
             //Tailbias=0.03;
             //AngleTail_stand_bias=0.03;
             Tailbias=0;
             AngleTail_stand_bias=0;
             */
         if(CountA>=FA)
         {
             //moveA();
             CountA=0;
             directionA=!directionA;
             /*
             if(directionA==1)
             {
                 CountA=10;
                 }
                 else
                 {
                     CountA=0;
                     }
                     */
             }
             else
             {
                 CountA++;
                 }
         if(CountB>=FB)
         {
             //moveB();
             CountB=0;
             directionB=!directionB;
             /*
             if(directionB==0)
             {
                 CountB=10;
                 }
                 else
                 {
                     CountB=0;
                     }    
                     */       
             }
             else
             {
                 CountB++;
                 }    
                 
        /*
        if(CountE>=FE)
        {
                 moveE();
                 CountE=0;
                 }
                 else
                 {
                     CountE++;
                     }
           */      
         //6/29/2015 Tail
        if(x<300&&x>100) 
        {
            //AngleLeft=-0.0005*x+0.18;
            //AngleRight=0.0005*x-0.12;
            AngleRight=-0.0005*x+0.18;
            AngleLeft=0.0005*x-0.02;
            }
            
        
        /*    
        if(x<300&&x>100)
        {
            if(x<220&&x>210)
            {
                AngleLeft=0.08;
                AngleRight=0.08;
                }
                else if(x>=220)
                {
                     AngleLeft=0.04;
                     AngleRight=0.12;
                     }
                     else if(x<=210)
                     {
                         AngleLeft=0.12;
                         AngleRight=0.04;
                         }               
            }
            else
            {
                AngleLeft=0.08;
                AngleRight=0.08;
                }
                */   
                /*
                AngleLeft=0.08;
                AngleRight=0.08;
                */
        //moveC();   
        //moveD();
        
        moveA();
        moveB();
        
        moveE();
        
       wait(0.03); 
              
    //Serial test
    /*
    if(x>210)
    moveA();
    else if (x<210)
    moveB();
    else if (x==555)
    {
         moveA();
         moveB();
         wait(0.2);
    }
    else 
    {
         moveA();
         moveB();
         wait(0.2);
        }
    */


//Actuator test
//       moveA();
//       moveB();
//       wait(0.2);

//Servo test
         
 //           PWMC.pulsewidth(period_pwm*0.08);
 //           PWMD.pulsewidth(period_pwm*0.08);
 //           PWME.pulsewidth(period_pwm*0.08);
 //            wait(0.05);
 //           PWMC.pulsewidth(period_pwm*0.05);
 //           PWMD.pulsewidth(period_pwm*0.05);
  //          PWME.pulsewidth(period_pwm*0.08);
 //          
 

        

}
            
}