File content as of revision 0:8f37781c0054:

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

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



    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;

    PWMB.pulsewidth(period_pwm*speedB);

}

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)
{
            
    //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(str);
            }
         }
         
    //Serial test
    if(x>210)
    moveA();
    else if (x<210)
    moveB();
    else if (x==555)
    {
         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);
 //          
 

        

}
            
}