File content as of revision 34:b0ae940a2fb5:

/*LAB_DCMotor*/
#include "mbed.h"

//The number will be compiled as type "double" in default
//Add a "f" after the number can make it compiled as type "float"
#define Ts 0.01f    //period of timer1 (s)
#define Kp 0.003f
#define Ki 0.01f


PwmOut servo(A0);
PwmOut pwm1(D7);
PwmOut pwm1n(D11);
PwmOut pwm2(D8);
PwmOut pwm2n(A3);

Serial bluetooth(D10,D2);
Serial pc(D1, D0);

DigitalOut led1(A4);
DigitalOut led2(A5);

//Motor1 sensor
InterruptIn HallA_1(A1);
InterruptIn HallB_1(A2);
//Motor2 sensor
InterruptIn HallA_2(D13);
InterruptIn HallB_2(D12);

Ticker timer1;
void timer1_interrupt(void);
void CN_interrupt(void);

void init_TIMER(void);
void init_PWM(void);
void init_CN(void);
void funcBorder(void);
void smallAngle( float );
void turnCW(float);
void turnCCW(float);

int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;

int v1Count = 0;
int v2Count = 0;

float v1 = 0.0, v1_ref = 0.0;
float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
float v2 = 0.0, v2_ref = 0.0;
float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;

int angle = 0;
float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90

int Receive_Data[33] = {0};
double Distance_Target = 0, Angle_Target = 0;
int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0;
int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0;

float k_forward = 0.05, k_turn = 1;

//****  receive and return by bluetooth ************** // bluetooth.getc()
float xC, yC; // car's position
float angleC;// car's angle 
double xP,yP,distance; //position that car need to reach
float angleR; //  angleR =  car-nextSpot direction  --->  car direction
int xGate = 306, yGate = 124; //左們136 328, 右們874,316   
double angleGate, distanceGate;
int aI_State = 2;
int bor_state = 1;
double pi = 3.1415926;
double borAngle=0;

float ballSize; // ball size

float longC = 32; // car's length  //65
int yB1 = 70;
int yB2 = 232;
int xB3 = 27;
int xB4 = 305; // broder conditions
//65 , 580, 170,860
int ticCheck = 10;

int main() {
    
    init_TIMER();
    init_PWM();
    init_CN();
    servo.write(0.079 + (0.084/180)*angle);
    
    bluetooth.baud(115200); //設定鮑率
    pc.baud(57600);
    
    
    while(1) 
    {
        if(pc.readable())
        {
            bluetooth.putc(pc.getc());
        }
        if(bluetooth.readable())
        {
            pc.putc(bluetooth.getc());
        }
    }
}

void timer1_interrupt(void)
{
    for(int i=0; i<32; i++)
    {
        Receive_Data[i] =  bluetooth.getc();  
    }
    //read data from matlab
    //distance_target
    Distance_Target = (Receive_Data[1]-0x30)*100 + (Receive_Data[2]-0x30)*10 + (Receive_Data[3]-0x30);
            
    if(Receive_Data[0] == '-')Distance_Target = -1* Distance_Target;
    else Distance_Target = Distance_Target;
            
    //ang_rel_target
    Angle_Target = (Receive_Data[5]-0x30)*100 + (Receive_Data[6]-0x30)*10 + (Receive_Data[7]-0x30);
            
    if(Receive_Data[4] == '-')Distance_Target = -1* Distance_Target;
    else Distance_Target = Distance_Target;
            
    //x_position_car_1
    X_Position_1 = (Receive_Data[9]-0x30)*100 + (Receive_Data[10]-0x30)*10 + (Receive_Data[11]-0x30);
            
    if(Receive_Data[8] == '-')Distance_Target = -1* Distance_Target;
    else Distance_Target = Distance_Target;
            
    //y_position_car_1
    Y_Position_1 = (Receive_Data[13]-0x30)*100 + (Receive_Data[14]-0x30)*10 + (Receive_Data[15]-0x30);
            
    if(Receive_Data[12] == '-')Distance_Target = -1* Distance_Target;
    else Distance_Target = Distance_Target;
            
    //angle_car_1
    Angle_1 = (Receive_Data[17]-0x30)*100 + (Receive_Data[18]-0x30)*10 + (Receive_Data[19]-0x30);
            
    if(Receive_Data[16] == '-')Distance_Target = -1* Distance_Target;
    else Distance_Target = Distance_Target;
            
    //x_position_car_2
    X_Position_2 = (Receive_Data[21]-0x30)*100 + (Receive_Data[22]-0x30)*10 + (Receive_Data[23]-0x30);
            
    if(Receive_Data[20] == '-')Distance_Target = -1* Distance_Target;
    else Distance_Target = Distance_Target;
            
    //y_position_car_2
    Y_Position_2 = (Receive_Data[25]-0x30)*100 + (Receive_Data[26]-0x30)*10 + (Receive_Data[27]-0x30);
            
    if(Receive_Data[24] == '-')Distance_Target = -1* Distance_Target;
    else Distance_Target = Distance_Target;
            
    //angle_car_1
    Angle_2 = (Receive_Data[29]-0x30)*100 + (Receive_Data[30]-0x30)*10 + (Receive_Data[31]-0x30);
            
    if(Receive_Data[28] == '-')Distance_Target = -1* Distance_Target;
    else Distance_Target = Distance_Target;
 
    
    
    //Motor 1
    v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    v1Count = 0;
    
    ///code for PI control///    
    v1_err = v1_ref - v1;
    v1_ierr = 0.01f*v1_err + v1_ierr;
    //PIout_1 = Kp*v1_err + Ki*v1_ierr; 
    PIout_1 = v1_ref;
    if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
    else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
    pwm1.write(PIout_1 + 0.5f);
    TIM1->CCER |= 0x4;   
    
    //Motor 2
    v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
    v2Count = 0;
    
    ///code for PI control///
    v2_err = v2_ref - v2;
    v2_ierr = 0.01f*v2_err + v2_ierr;
    //PIout_2 = Kp*v2_err + Ki*v2_ierr; 
    PIout_2 = v2_ref;
    
    if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
    else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
    pwm2.write(PIout_2 + 0.5f);
    TIM1->CCER |= 0x40;
    
      
    //*****  main AI  **************************
    
    
    //***** get position information from the bluetooth
    xC = X_Position_1; 
    yC = Y_Position_1; // car's position

    angleC = Angle_1; // car's angle 
    
    distance = Distance_Target;
    xP = Distance_Target*cos(Angle_Target*pi/180);
    yP = Distance_Target*sin(Angle_Target*pi/180);//position that car need to reach

    angleR = Angle_Target; //  angleR =  car-nextSpot direction  --->  car direction
    
    yB1 += longC; 
    yB2 -= longC ;
    xB3 += longC ;
    xB4 -= longC;
    
    //****   AI_State   ********************
    
    switch(aI_State)
    {
        case 0:   // IDLE
            // IDLE check if stop color appear 
            //check purple color appear or not 
           if(xP==0 && yP==0 && angleR==0)
           {
               v1_ref = 0;
               v2_ref = 0;
           }
           else
           aI_State = 1;
            //****setSpecs();
            break;
      
        case 1:   ///   border condition
            // check if car fit border conditions
            switch(bor_state){
                case 1:
                    funcBorder();
                    break;
                case 2: // 前進 再判別   v1+  v2-
                    if(ticCheck >0) //誤差容忍正負五度
                    {
                        v1_ref =  50;
                        v2_ref = -50;    
                        ticCheck -- ;
                    }
                    else
                    {
                        bor_state = 1;
                        ticCheck = 10;
                    }
                    break;
                case 3: // 後退 再判別   v1-  v2+
                    
                    if(ticCheck >0) //誤差容忍正負五度
                    {
                        v1_ref = -50;
                        v2_ref = 50;    
                        ticCheck -- ;
                    }
                    else
                    {
                        bor_state = 1;
                        ticCheck = 10;
                    }
                    break;
                case 4: // 順時轉到特定角 結束  v1-- v2 --
                    
                    if((borAngle-angleC)> 5 || (borAngle-angleC)<-5)//誤差容忍正負五度
                    {
                        v1_ref = -50;
                        v2_ref = -50;    
                    }
                    else
                    {
                        borAngle = 0;
                        aI_State = 2;
                        bor_state = 1;
                    }
                    break;
                case 5: // 逆時轉到特定角 結束  v1 ++   v2++
                     if((borAngle-angleC)> 5 || (borAngle-angleC)<-5)//誤差容忍正負五度
                    {
                        v1_ref = 50;
                        v2_ref = 50;    
                    }
                    else
                    {
                        borAngle = 0;
                        aI_State = 2;
                        bor_state = 1;
                    }
                    break;
                
                }
            
           //****to case2
            break;
        case 2://move to get ball
            
            if((40<=distance <= 50) && (-5<=angleR<=5))
            {
                aI_State = 3;
            }
            else
            {
                v1_ref = k_forward*(distance - 40) + k_turn*angleR;
                v2_ref = -k_forward*(distance - 40) + k_turn*angleR;
            }
            break;
        case 3: //****getBall();
             if((40<=distance <= 50) && (-5<=angleR<=5))
             {
                 angle = -10;
                 servo_duty = 0.079 + (0.084/180)*angle;
                 servo.write(servo_duty);
                 servo = 1;
                 wait(0.1);       
                 servo = 0;
             }
             else
             aI_State = 2;
             
            break;
        case 4: /// move to the gate and release ball
        // move to the point in front of the gate first
        // then head to the gate
            distanceGate = sqrt(((xGate-xP)*(xGate-xP))+((yGate-yP)*(yGate-yP)));
            angleGate = atan2((yGate-yP),(xGate-xP))*180/pi;
            if(-3<=angleGate<=3) // direct to the gate
           {
              if(distanceGate<=80) // check distance between car cemtroid and gate 
              {
                  v1_ref = 0;
                  v2_ref = 0;
                  aI_State = 5;
              }
              else if(80<distanceGate<150)
              {
                  v1_ref = 50;
                  v2_ref = -50;
              }
              else
              {
                  v1_ref = 50;
                  v2_ref = -50;
              }
           }          
           else if(3<angleGate<=15) // small angle right
           {
              if(distanceGate<50)
              {
                  v1_ref = 50;
                  v2_ref = -(v1_ref-angleGate*k_turn);
              }
              else
              {
                  v1_ref = 50;
                  v2_ref = -(v1_ref-angleGate*k_turn);
              }  
           }
           else if(-15<=angleGate<-3) // small angle left
           {
              if(distanceGate<150)
              {
                  v1_ref = -(v2_ref+angleGate*k_turn);
                  v2_ref = 50;
              }
              else
              {
                  v1_ref = -(v2_ref+angleGate*k_turn);
                  v2_ref = 300;
              }  
           }
           else if(15<angleGate<=180) // big angle right
           {
              v1_ref = 50;
              v2_ref = 50;
              wait(0.005); //need to count the relation between angle and time in v = 200 
              if(0<=distanceGate<150)
              {
                  v1_ref = 50;
                  v2_ref = -50;
              }
              else
              {
                  v1_ref = 50;
                  v2_ref = -50;
              }  
           }
           else if(-180<=angleGate<-15) // big angle left
           {
              v1_ref = -50;
              v2_ref = -50;
              wait(0.005); //need to count the relation between angle and time in v = 200 
              if(0<distanceGate<150)
              {
                  v1_ref = 50;
                  v2_ref = -50;
              }
              else
              {
                  v1_ref = 50;
                  v2_ref = -50;
              }   
           }
            
            break;
         case 5: // release ball
            if((-3<=angleGate<=3) && (distanceGate<=10)) // direct to the gate
              {
                 angle = 90; // Fixture up
                 servo_duty = 0.079 + (0.084/180)*angle;
                 servo.write(servo_duty);
                 servo = 1;
                 //wait(0.1);       
                 servo = 0;
                 
                 pwm1.write(0.1f + 0.5f); // push
                 pwm2.write(-0.1f + 0.5f);
                 //wait(0.5);
                 
                 angle = -10; // Fixture down
                 servo_duty = 0.079 + (0.084/180)*angle;
                 servo.write(servo_duty);
                 servo = 1;
                 //wait(0.1);       
                 servo = 0;
                 
                 pwm1.write(-0.3f + 0.5f); // back and leave gate
                 pwm2.write(0.3f + 0.5f);
                 //wait(1);
                 
                 aI_State = 0;
                  
              }
         
          break;
    
    }
       
}

void CN_interrupt(void)
{
    //Motor 1
    stateA_1 = HallA_1.read();
    stateB_1 = HallB_1.read();
    
    ///code for state determination///
    if(stateA_1==0&&stateB_1==0){
    state_1 = 1;}
    else if(stateA_1==0&&stateB_1==1){
    state_1 = 2;}
    else if(stateA_1==1&&stateB_1==1){
    state_1 = 3;}
    else if(stateA_1==1&&stateB_1==0){
    state_1 = 4;}
       
    if(state_1 == 1)
    {
        if(state_1-state_1_old == -3)
        v1Count=v1Count+1;
        else if(state_1-state_1_old == -1)
        v1Count=v1Count-1;     
    }
    else if(state_1 == 2)
    {
        if(state_1-state_1_old == 1)
        v1Count=v1Count+1;
        else if(state_1-state_1_old == -1)
        v1Count=v1Count-1;     
    }
    else if(state_1 == 3)
    {
        if(state_1-state_1_old == 1)
        v1Count=v1Count+1;
        else if(state_1-state_1_old == -1)
        v1Count=v1Count-1;     
    }
    else if(state_1 == 4)
    {
        if(state_1-state_1_old == 1)
        v1Count=v1Count+1;
        else if(state_1-state_1_old == 3)
        v1Count=v1Count-1;     
    } 
        state_1_old = state_1;
    
    
    //////////////////////////////////
    
    //Forward
    //v1Count +1
    //Inverse
    //v1Count -1
    
    
    //Motor 2
    stateA_2 = HallA_2.read();
    stateB_2 = HallB_2.read();
    
    ///code for state determination///
    if(stateA_2==0&&stateB_2==0){
    state_2 = 1;}
    else if(stateA_2==0&&stateB_2==1){
    state_2 = 2;}
    else if(stateA_2==1&&stateB_2==1){
    state_2 = 3;}
    else if(stateA_2==1&&stateB_2==0){
    state_2 = 4;}
    
    if(state_2 == 1)
    {
        if(state_2-state_2_old == -3)
        v2Count=v2Count+1;
        else if(state_2-state_2_old == -1)
        v2Count=v2Count-1;     
    }
    else if(state_2 == 2)
    {
        if(state_2-state_2_old == 1)
        v2Count=v2Count+1;
        else if(state_2-state_2_old == -1)
        v2Count=v2Count-1;     
    }
    else if(state_2 == 3)
    {
        if(state_2-state_2_old == 1)
        v2Count=v2Count+1;
        else if(state_2-state_2_old == -1)
        v2Count=v2Count-1;     
    }
    else if(state_2 == 4)
    {
        if(state_2-state_2_old == 1)
        v2Count=v2Count+1;
        else if(state_2-state_2_old == 3)
        v2Count=v2Count-1;     
    }
        state_2_old = state_2;
    
    //////////////////////////////////
    
    //Forward
    //v2Count +1
    //Inverse
    //v2Count -1
    
    
}

void init_TIMER(void)
{
    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
}         
   
void init_PWM(void)
{
    pwm1.period_us(50);
    pwm1.write(0.5);
    TIM1->CCER |= 0x4;
    
    pwm2.period_us(50);
    pwm2.write(0.5);
    TIM1->CCER |= 0x40;
}

void init_CN(void)
{
    HallA_1.rise(&CN_interrupt);
    HallA_1.fall(&CN_interrupt);
    HallB_1.rise(&CN_interrupt);
    HallB_1.fall(&CN_interrupt);
    
    HallA_2.rise(&CN_interrupt);
    HallA_2.fall(&CN_interrupt);
    HallB_2.rise(&CN_interrupt);
    HallB_2.fall(&CN_interrupt);
    
    stateA_1 = HallA_1.read();
    stateB_1 = HallB_1.read();
    stateA_2 = HallA_2.read();
    stateB_2 = HallB_2.read();
}

//****************  funcBorder()  *****************************************

void funcBorder(void) // check if car is too close to the border  //  miss the ball restriction part
{
  if(yC > yB1 || yC < yB2 || xC < xB3 || xC > xB4){

    if(yC > yB1) // 靠近上邊界          *****************************
    {
        if(xC < xB3) //左上角  
        {
           if(angleC <0 && angleC >=-90)         //   I
            {
                bor_state = 4;  //clockwise, to angleC = 45
                borAngle = 45;
            }
            
           else if(angleC > -180 && angleC <-90)  //   II
            bor_state = 3;  //back 0.5*longC, judge again
            
           else if(angleC <=180 && angleC >=90)  //   III
             {
                 bor_state = 5;//counter clockwise, to angleC = 45
                 borAngle = 45;
            }

           else if(angleC < 90 && angleC >=0)  //  IV
             bor_state = 2;//forward 0.5*longC, judge again

        }
        else if(xC > xB4) //右上角  
        {
           if(angleC <0 && angleC >=-90)         //   I
             bor_state = 3;//back 0.5*longC, judge again
            
           else if(angleC > -180 && angleC <-90)  //   II
             {
                 bor_state = 5;//counter clockwise, to angleC = 135
                 borAngle = 135;
            }
            
           else if(angleC <=180 && angleC >=90)  //   III
             bor_state = 2;//forward 0.5*longC, judge again

           else if(angleC < 90 && angleC >=0)  //  IV
             {
                bor_state = 4;//clockwise, to angleC = 135
                borAngle = 135;
         }
            
        }
        else //上邊界 
        {
          if(angleC == -90 || angleC == 90)
            aI_State = 2; // do nothing, turn to next point
          else
          {
            if(0 > borAngle >= -90 || 90< borAngle <=0) //CW
            {
                bor_state = 4;
                borAngle = 90;
            }
            else
            {
                bor_state = 5;
                borAngle = 90;
            }
          } // turn to 90, then turn to next point
        }
      
        
    }
    else if(yC < yB2) // 靠近下邊界    *E****************************************************88
    {
        if(xC < xB3) //左下角 
        {
           if(angleC <0 && angleC >=-90)         //   I
            bor_state = 2;  //forward 0.5*longC, judge again
 
           else if(angleC > -180 && angleC <-90)  //   II
             {
                 bor_state = 4;   //clockwise, to angleC = -45
                 borAngle = -45;
            }
            
           else if(angleC <=180 && angleC >=90)  //   III
             bor_state = 3;//back 0.5*longC, judge again

           else if(angleC < 90 && angleC >=0)  //  IV
             {
                 bor_state = 5;//counter clockwise, to angleC = -45
                 borAngle = -45;
            }
          
        }
        else if(xC > xB4) //右下角
        {
           if(angleC <0 && angleC >=-90)         //   I
             {
                 bor_state = 5;//counter clockwise, to angleC = -135
                 borAngle = -135;
            }
            
           else if(angleC > -180 && angleC <-90)  //   II
             bor_state = 2;//forward 0.5*longC, judge again
            
           else if(angleC <=180 && angleC >=90)  //   III
             {
                 bor_state = 4;//clockwise, to angleC = -135
                 borAngle = -135;
            }

           else if(angleC < 90 && angleC >=0)  //  IV
             bor_state = 3;//back 0.5*longC, judge again
          
        }
        else //下邊界  
        {       
          if(angleC == -90 || angleC == 90)
            aI_State = 2; // do nothing, turn to next point
          else
          {
            if(0 > borAngle >= -90 || 90< borAngle <=0) //CCW
            {
                bor_state = 5;
                borAngle = -90;
            }
            else
            {
                bor_state = 4;
                borAngle = -90;
            }
          } // turn to -90, then turn to next point
        }
        
       
    }
    else if(xC < xB3) //靠近左邊界****************************************************************
    {
          if(angleC == 0 || angleC == 180)
            aI_State = 2; // do nothing, turn to next point

          else if(-180 < angleC < 0) // CW
          {
             bor_state = 4;
             borAngle = 0;
          }
          else
          {
              bor_state = 5;
              borAngle = 0;
          }
                 // turn to 0, then turn to next point
    }

    else if(xC > xB4) //靠近右邊界     *********************************************88
    {
          if(angleC == 0 || angleC == 180)
            aI_State = 2; // do nothing, turn to next point
            
          else if(-180 < angleC < 0) // CCW
          {
             bor_state = 5;
             borAngle = 180;
          } // turn to 90, then turn to next point
          else
          {
              bor_state = 4;
              borAngle = 180;
          }
    }
  }
  else
  {aI_State = 2;}
} // funcBorder() 

//****************  smallAngle()  *****************************************

void smallAngle( float goodAngle ) // use the smallest turn to right angle  ccw / cw,  based on 0~360 degree system
{
    if(angleR > 0) // CW
    {
        // turn clockwise to goodAngle;
    }
    else if(angleR < 0) //CCW
    {
        // turn counter clockwise to goodAngle;
    }

}
//***************   turnCW() ***********************************

void turnCW(float goodAngle)
{
  // turn clockwise to goodAngle;
}

//***************   turnCCW() ***********************************

void turnCCW(float goodAngle)
{
  // turn counter clockwise to goodAngle;
}