Revision 4:c93e1ecd3359, committed 2015-07-15

Comitter:

RenjianHao

Date:

Wed Jul 15 20:13:09 2015 +0000

Parent:

3:caa0d4fd1d1d

Child:

5:a5f0395d2fa4

Commit message:

Renjian Hao 7/15/2015

Changed in this revision

--- a/main.cpp	Wed Jul 01 18:45:22 2015 +0000
+++ b/main.cpp	Wed Jul 15 20:13:09 2015 +0000
@@ -36,6 +36,7 @@
 float speedA;
 float speedB;
 float speedC;
+float Tail_increasement=0.0;
 
 float AngleLeft=0.08;
 float AngleRight=0.04;
@@ -43,7 +44,7 @@
 float Tailbias=0.0;
 float AngleTail_stand_bias=0.0;
 
-long int FA=1000000,FB=1000000,FE=10,CountA=0,CountB=0,CountE=0;;
+long int FA=1000000,FB=1000000,FE=4,CountA=0,CountB=0,CountE=0,CountT=0;
 
 int flag;
 int x,y; // Image coordinate
@@ -79,7 +80,7 @@
     }
     AIN1=inPin1;
     AIN2=inPin2;
-    directionA=!directionA;
+    //directionA=!directionA;
 
     //pc.printf("dirA = %d\n\r",directionA);
 
@@ -88,11 +89,12 @@
     {
         //speedA=0.4;
         PWMA.pulsewidth(period_pwm*(speedA*1.5));
+        //PWMA.pulsewidth(period_pwm*(0.6));
         }
         else
         {
             //speedA=0.2;
-            PWMA.pulsewidth(period_pwm*speedA);
+            PWMA.pulsewidth(period_pwm*speedA*1.5);
             }
     //PWMA.pulsewidth(period_pwm*speedA);
 
@@ -110,17 +112,18 @@
     //pc.printf("dirB = %d\n\r",directionB);
     BIN1=inPin1;
     BIN2=inPin2;
-    directionB=!directionB;
+    //directionB=!directionB;
 
     if(!directionB)
     {
         //speedB=0.4;
         PWMB.pulsewidth(period_pwm*(speedB*1.5));
+        //PWMB.pulsewidth(period_pwm*(0.6));
         }
         else
         {
             //speedB=0.2;
-            PWMB.pulsewidth(period_pwm*speedB);
+            PWMB.pulsewidth(period_pwm*speedB*1.5);
             }
     //PWMB.pulsewidth(period_pwm*speedB);
 
@@ -142,19 +145,27 @@
 
 void moveE()
 {
+ /*
  if(directionE)
  {
-     AngleTail=0.08+Tailbias;
+     AngleTail=0.082+Tailbias;
      directionE=!directionE;
  }
  else
  {
-     AngleTail=0.07+Tailbias;
+     AngleTail=0.068+Tailbias;
      directionE=!directionE;
  }
- 
+ */
+ /*
+ if(Tailbias==0)
+ {
+   AngleTail=0.075;
+   }
+   */
+    AngleTail=0.075+Tailbias;
    //AngleTail=0.12;
-    //AngleTail=0.08;
+    //AngleTail=0.075;
     STBY=1; //disable standby
     PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
 }
@@ -230,34 +241,100 @@
     
                 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("%d \r \n",y);
+                //pc.printf("\n");
               //pc.printf(str);
             }
          }
-         
-       
+         /*
+         if(y<300&&y>100)
+         {
+           Tailbias=(y-200)*0.0006;
+           }
+           
+           else
+           {
+               Tailbias=0;
+               }
+               */
+         FA=20;//5;
+         FB=20;//5;  
+         /*
+         if(CountT>300&&CountT<=600)
+         {
+         speedA=0.6;
+         Tailbias=0;//(160-200)*0.0006;
+         speedB=0.0;
+         CountT++;
+         }
+         else if(CountT<=300)
+         {
+           speedB=0.6;
+           Tailbias=0;//(200-160)*0.0006;
+           speedA=0.0;
+           CountT++;
+             }
+             else
+             {
+                 CountT=0;
+                 }
+                 */
+          
+         if(y<350&&y>200)
+         {
+           
+           speedA=(y-200)*0.006; 
+           Tail_increasement=speedA*0.0012;
+           Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006;
+           speedB=0.0; 
+             }
+             else if (y>50&&y<=200)
+             {
+                 speedB=(200-y)*0.006;
+                 Tail_increasement=speedB*0.0012;
+                 Tailbias=Tailbias+Tail_increasement;//0.0;//(y-200)*0.0006;
+                 speedA=0.0;
+                 }
+                 else
+                 {
+                     speedA=0.0;
+                     speedB=0.0;
+                     //Tailbias=0.0;
+                     }
+                     
+         if(Tailbias>0.02)
+         {
+                     Tailbias=0.02;
+                     }
+         if(Tailbias<-0.02)
+         {
+                     Tailbias=-0.02;
+                     }
+                     
+                   
+         /*
          if(y>210&&y<400)
          {
              //FA=2000000;
              //FB=200000;
              FA=20;//FA=40;
-             FB=20;
-             speedB=0.3;
-             speedA=0.02;             
-             Tailbias=0.015;
-             AngleTail_stand_bias=0.015;
+             FB=5;
+             speedB=0.6;
+             speedA=0.02;        
+             //Tailbias=(y-210)*0.0003;     
+             //Tailbias=0.03;
+             //AngleTail_stand_bias=0.03;
              //moveE_stand();
              }
              
          else if(y<190&&y>100)
          {
              FB=20;//FB=40;
-             FA=20;             
-             speedA=0.3;
+             FA=5;             
+             speedA=0.6;
              speedB=0.02;
-             Tailbias=-0.015;
-             AngleTail_stand_bias=-0.015;
+             //Tailbias=-0.03;
+             //AngleTail_stand_bias=-0.03;
              //moveE_stand();
              }
              else
@@ -266,16 +343,38 @@
                  FB=20;
                  directionB=!directionA;
                  CountB=CountA;
-                 speedA=0.2;
-                 speedB=0.2;
+                 speedA=0.02;
+                 speedB=0.02;
                  Tailbias=0;
                  AngleTail_stand_bias=0.0;
                  //moveE_stand();
                  }
+                 */
+                 /*
+             FA=20;//FA=40;
+             FB=5;
+             speedB=0.6;
+             speedA=0.01;             
+             //Tailbias=0.03;
+             //AngleTail_stand_bias=0.03;
+             Tailbias=0;
+             AngleTail_stand_bias=0;
+             */
          if(CountA>=FA)
          {
-             moveA();
+             //moveA();
              CountA=0;
+             directionA=!directionA;
+             /*
+             if(directionA==1)
+             {
+                 CountA=10;
+                 }
+                 else
+                 {
+                     CountA=0;
+                     }
+                     */
              }
              else
              {
@@ -283,16 +382,26 @@
                  }
          if(CountB>=FB)
          {
-             moveB();
-             //moveE();
+             //moveB();
              CountB=0;
+             directionB=!directionB;
+             /*
+             if(directionB==0)
+             {
+                 CountB=10;
+                 }
+                 else
+                 {
+                     CountB=0;
+                     }    
+                     */       
              }
              else
              {
                  CountB++;
                  }    
                  
-        
+        /*
         if(CountE>=FE)
         {
                  moveE();
@@ -302,7 +411,7 @@
                  {
                      CountE++;
                      }
-                 
+           */      
          //6/29/2015 Tail
         if(x<300&&x>100) 
         {
@@ -344,6 +453,12 @@
                 */
         //moveC();   
         //moveD();
+        
+        moveA();
+        moveB();
+        
+        moveE();
+        
        wait(0.03); 
               
     //Serial test