Diff: main.cpp

Revision:

1:9a7e97e643bc

Parent:

0:8f37781c0054

Child:

2:d5dc0db74d84

--- a/main.cpp	Fri Apr 17 21:13:44 2015 +0000
+++ b/main.cpp	Fri Jun 26 17:31:28 2015 +0000
@@ -36,6 +36,11 @@
 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
 
@@ -75,8 +80,17 @@
     //pc.printf("dirA = %d\n\r",directionA);
 
 
-
-    PWMA.pulsewidth(period_pwm*speedA);
+    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);
 
 }
 
@@ -94,10 +108,33 @@
     BIN2=inPin2;
     directionB=!directionB;
 
-    PWMB.pulsewidth(period_pwm*speedB);
+    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
@@ -133,7 +170,15 @@
         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) 
          {
@@ -147,12 +192,103 @@
     
                 token = strtok(NULL, ",");
                 y = atoi(token);                //100-340(height_240); No target:555 
-               // pc.printf(" x=%d y=%d", x,y);
+               //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)
@@ -163,6 +299,13 @@
          moveB();
          wait(0.2);
     }
+    else 
+    {
+         moveA();
+         moveB();
+         wait(0.2);
+        }
+    */
 
 
 //Actuator test