Revision 17:d96afd9d2692, committed 2016-05-25

Comitter:

roger5641

Date:

Wed May 25 13:04:35 2016 +0000

Parent:

16:a102929b2228

Child:

18:2db6c97a4145

Commit message:

motion control;

Changed in this revision

--- a/AI_Friday.cpp	Wed May 25 11:25:00 2016 +0000
+++ b/AI_Friday.cpp	Wed May 25 13:04:35 2016 +0000
@@ -7,6 +7,8 @@
 #define Kp 0.003f
 #define Ki 0.01f
 
+
+PwmOut servo(A0);
 PwmOut pwm1(D7);
 PwmOut pwm1n(D11);
 PwmOut pwm2(D8);
@@ -28,7 +30,7 @@
 Ticker timer1;
 void timer1_interrupt(void);
 void CN_interrupt(void);
-void _ISR_U2RXInterrupt(void);
+//void _ISR_U2RXInterrupt(void);
 
 void init_TIMER(void);
 void init_PWM(void);
@@ -40,10 +42,12 @@
 int v1Count = 0;
 int v2Count = 0;
 
-float v1 = 0.0, v1_ref = 60.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 = -60.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 = 90;
 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
 
 int Command_Flag = 0, Receive_Flag = 0, Receive_Counter = 0, 
@@ -57,20 +61,20 @@
 
 //****  receive and return by bluetooth ************** // bluetooth.getc()
 float xC, yC; // car's position
-
 float angleC;// car's angle 
-
-float xP,yP; //position that car need to reach
-
+double xP,yP,distance; //position that car need to reach
 float angleR; //  angleR =  car-nextSpot direction  --->  car direction
-
+int xGate = 800, yGate = 300;
+double angleGate, distanceGate;
 int aI_State = 0;
+double pi = 3.1415926;
 
 int main() {
     
     init_TIMER();
     init_PWM();
     init_CN();
+    servo.write(0.079 + (0.084/180)*angle);
     
     bluetooth.baud(115200); //設定鮑率
     pc.baud(57600);
@@ -87,7 +91,7 @@
             pc.putc(bluetooth.getc());
         }
         
-        if(Command_Flag == 1)
+        /*if(Command_Flag == 1)
         {
             //read data from matlab
             //distance_target
@@ -143,7 +147,7 @@
             else if(Receive_Data[33] == 'k')pwm_duty = 100;
             
             Command_Flag = 0;
-        }
+        }*/
     }
 }
 
@@ -158,13 +162,10 @@
     v1_ierr = 0.01f*v1_err + v1_ierr;
     PIout_1 = Kp*v1_err + Ki*v1_ierr; 
     
-    /////////////////////////
-    
     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;
-    
+    TIM1->CCER |= 0x4;   
     
     //Motor 2
     v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f;   //unit: rpm
@@ -174,29 +175,27 @@
     v2_err = v2_ref - v2;
     v2_ierr = 0.01f*v2_err + v2_ierr;
     PIout_2 = Kp*v2_err + Ki*v2_ierr; 
-
-    /////////////////////////
     
     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 = 0; 
-    yC = 0; // car's position
-
-    angleC = 0; // car's angle 
+    xC = X_Position_1; 
+    yC = Y_Position_1; // car's position
 
-    xP = 0;
-    yP = 0;//position that car need to reach
+    angleC = Angle_1; // car's angle 
+    
+    distance = Distance_Target;
+    xP = Distance_Target*cos(Angle_Target);
+    yP = Distance_Target*sin(Angle_Target);//position that car need to reach
 
-    angleR = 0; //  angleR =  car-nextSpot direction  --->  car direction
+    angleR = Angle_Target; //  angleR =  car-nextSpot direction  --->  car direction
     
     //****   AI_State   ********************
     
@@ -205,70 +204,239 @@
         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 2:   ///   border condition
+        case 1:   ///   border condition
             // check if car fit border conditions
             //****funcBorder();
-           //****to case3
+           //****to case2
             break;
-        case 3://move to get ball
-          
-            //****getBall();
+        case 2://move to get ball
+           if(-3<=angleR<=3) // direct to the ball
+           {
+              if(distance<=10) // check夾具距離
+              {
+                  v1_ref = 0;
+                  v2_ref = 0;
+                  aI_State = 3;
+              }
+              else if(10<distance<50)
+              {
+                  v1_ref = 100;
+                  v2_ref = -100;
+              }
+              else
+              {
+                  v1_ref = 300;
+                  v2_ref = -300;
+              }
+           }
+           
+           else if(3<angleR<=15) // small angle right
+           {
+              if(distance<50)
+              {
+                  v1_ref = 200;
+                  v2_ref = -(v1_ref-5);
+              }
+              else
+              {
+                  v1_ref = 300;
+                  v2_ref = -(v1_ref-5);
+              }
+   
+           }
+           else if(-15<=angleR<-3) // small angle left
+           {
+              if(distance<50)
+              {
+                  v1_ref = -(v2_ref-5);
+                  v2_ref = 200;
+              }
+              else
+              {
+                  v1_ref = -(v2_ref-5);
+                  v2_ref = 300;
+              }
+   
+           }
+           else if(15<angleR<=180) // big angle right
+           {
+              v1_ref = 200;
+              v2_ref = 200;
+              wait(10); //need to count the relation between angle and time in v = 200 
+              if(distance<50)
+              {
+                  v1_ref = 100;
+                  v2_ref = -100;
+              }
+              else
+              {
+                  v1_ref = 300;
+                  v2_ref = -300;
+              }
+   
+           }
+           else if(-180<=angleR<-15) // big angle left
+           {
+              v1_ref = -200;
+              v2_ref = -200;
+              wait(10); //need to count the relation between angle and time in v = 200 
+              if(0<distance<50)
+              {
+                  v1_ref = 100;
+                  v2_ref = -100;
+              }
+              else
+              {
+                  v1_ref = 300;
+                  v2_ref = -300;
+              }
+   
+           }
+            
+            break;
+        case 3: //****getBall();
+             if((distance == 10) && (-3<=angleR<=3))
+             {
+                 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)^2+(yGate-yP)^2);
+            angleGate = atan2((yGate-yP),(xGate-xP))*180/pi;
+            if(-3<=angleGate<=3) // direct to the gate
+           {
+              if(distanceGate<=10) // check distance between car cemtroid and gate 
+              {
+                  v1_ref = 0;
+                  v2_ref = 0;
+                  aI_State = 5;
+              }
+              else if(10<distanceGate<50)
+              {
+                  v1_ref = 100;
+                  v2_ref = -100;
+              }
+              else
+              {
+                  v1_ref = 300;
+                  v2_ref = -300;
+              }
+           }          
+           else if(3<angleGate<=15) // small angle right
+           {
+              if(distanceGate<50)
+              {
+                  v1_ref = 200;
+                  v2_ref = -(v1_ref-5);
+              }
+              else
+              {
+                  v1_ref = 300;
+                  v2_ref = -(v1_ref-5);
+              }  
+           }
+           else if(-15<=angleGate<-3) // small angle left
+           {
+              if(distanceGate<50)
+              {
+                  v1_ref = -(v2_ref-5);
+                  v2_ref = 200;
+              }
+              else
+              {
+                  v1_ref = -(v2_ref-5);
+                  v2_ref = 300;
+              }  
+           }
+           else if(15<angleGate<=180) // big angle right
+           {
+              v1_ref = 200;
+              v2_ref = 200;
+              wait(10); //need to count the relation between angle and time in v = 200 
+              if(distanceGate<50)
+              {
+                  v1_ref = 100;
+                  v2_ref = -100;
+              }
+              else
+              {
+                  v1_ref = 300;
+                  v2_ref = -300;
+              }  
+           }
+           else if(-180<=angleGate<-15) // big angle left
+           {
+              v1_ref = -200;
+              v2_ref = -200;
+              wait(10); //need to count the relation between angle and time in v = 200 
+              if(0<distanceGate<50)
+              {
+                  v1_ref = 100;
+                  v2_ref = -100;
+              }
+              else
+              {
+                  v1_ref = 300;
+                  v2_ref = -300;
+              }   
+           }
+            
             break;
-        case 5: 
-            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; // return to idle
+                  
+              }
+         
+          break;
+            
         
     }
     
     
-    
-    
-    
-    
-    switch(bluetooth.getc())
-    {
-    case '1':
-        v1_ref = 30;
-        v2_ref = -30;
-        break;
-    case '2':
-        v1_ref = 40;
-        v2_ref = -40;
-        break;
-    case '3':
-        v1_ref = 50;
-        v2_ref = -50;
-        break;
-    case '4':
-        v1_ref = 60;
-        v2_ref = -60;
-        break;
-    case '5':
-        v1_ref = -70;
-        v2_ref = 70;
-        break;
-    case '6':
-        v1_ref = -80;
-        v2_ref = 80;
-        break;
-    case '7':
-        v1_ref = -100;
-        v2_ref = 100;
-        break;
-    case '8':
-        v1_ref = 0;
-        v2_ref = 0;
-        break;            
-    }
-    
-    
 }
 
 void CN_interrupt(void)
@@ -378,7 +546,7 @@
     //v2Count -1
 }
 
-void _ISR_U2RXInterrupt(void)
+/*void _ISR_U2RXInterrupt(void)
 {
 /////////// Receive ////////////
     static char Temp;
@@ -414,7 +582,7 @@
     
     IFS1bits.U2RXIF = 0;
 }
-
+*/
 
 void init_TIMER(void)
 {