File content as of revision 1:9a7e97e643bc:

#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;

float speedA;
float speedB;
float speedC;

float AngleLeft=0.08;
float AngleRight=0.08;

long int FA=1000000,FB=1000000,CountA=0,CountB=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));
        }
        else
        {
            //speedA=0.2;
            PWMA.pulsewidth(period_pwm*speedA);
            }
    //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));
        }
        else
        {
            //speedB=0.2;
            PWMB.pulsewidth(period_pwm*speedB);
            }
    //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 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;
        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(" x=%d y=%d", x,y);
               //pc.printf("\n");
              //pc.printf(str);
            }
         }
         
       
         if(y>210&&y<400)
         {
             //FA=2000000;
             //FB=200000;
             FA=20;//FA=40;
             FB=20;
             speedB=0.3;
             speedA=0.02;             
             }
             
         else if(y<190&&y>100)
         {
             FB=20;//FB=40;
             FA=20;             
             speedA=0.3;
             speedB=0.02;
             }
             else
             {
                 FA=20;
                 FB=20;
                 directionB=!directionA;
                 CountB=CountA;
                 speedA=0.2;
                 speedB=0.2;
                 }
         if(CountA>=FA)
         {
             moveA();
             CountA=0;
             }
             else
             {
                 CountA++;
                 }
         if(CountB>=FB)
         {
             moveB();
             CountB=0;
             }
             else
             {
                 CountB++;
                 }    
                 
        
        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();
       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);
 //          
 

        

}
            
}