Diff: main.cpp

Revision:

2:d5dc0db74d84

Parent:

1:9a7e97e643bc

Child:

3:caa0d4fd1d1d

--- a/main.cpp	Fri Jun 26 17:31:28 2015 +0000
+++ b/main.cpp	Mon Jun 29 17:30:28 2015 +0000
@@ -31,15 +31,19 @@
 bool directionA;
 bool directionB;
 bool directionC;
+bool directionE;
 
 float speedA;
 float speedB;
 float speedC;
 
 float AngleLeft=0.08;
-float AngleRight=0.08;
+float AngleRight=0.04;
+float AngleTail=0.08;
+float Tailbias=0.0;
+float AngleTail_stand_bias=0.0;
 
-long int FA=1000000,FB=1000000,CountA=0,CountB=0;
+long int FA=1000000,FB=1000000,FE=40,CountA=0,CountB=0,CountE=0;;
 
 int flag;
 int x,y; // Image coordinate
@@ -135,6 +139,37 @@
     PWMD.pulsewidth(period_pwm*AngleRight);//0.08 mid 0.04 forward
 
 }
+/*
+void moveE()
+{
+ if(directionE)
+ {
+     AngleTail=0.09+Tailbias;
+     directionE=!directionE;
+ }
+ else
+ {
+     AngleTail=0.06+Tailbias;
+     directionE=!directionE;
+ }
+ 
+   //AngleTail=0.12;
+    //AngleTail=0.08;
+    STBY=1; //disable standby
+    PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
+}
+*/
+
+void moveE_stand()
+{
+
+     AngleTail=0.075+AngleTail_stand_bias;
+ 
+   //AngleTail=0.12;
+    //AngleTail=0.08;
+    STBY=1; //disable standby
+    PWME.pulsewidth(period_pwm*AngleTail);//0.08 mid 0.04 forward
+}
 void stop()
 {
 //enable standby
@@ -163,6 +198,7 @@
     //initial direction & current    
         directionA=1;
         directionB=0;
+        directionE=1;
         flag=1; 
         speedA=0.2;
         speedB=0.2; //Actuator speed control
@@ -207,6 +243,9 @@
              FB=20;
              speedB=0.3;
              speedA=0.02;             
+             Tailbias=0.015;
+             AngleTail_stand_bias=0.015;
+             moveE_stand();
              }
              
          else if(y<190&&y>100)
@@ -215,6 +254,9 @@
              FA=20;             
              speedA=0.3;
              speedB=0.02;
+             Tailbias=-0.015;
+             AngleTail_stand_bias=-0.015;
+             moveE_stand();
              }
              else
              {
@@ -224,6 +266,9 @@
                  CountB=CountA;
                  speedA=0.2;
                  speedB=0.2;
+                 Tailbias=0;
+                 AngleTail_stand_bias=0.0;
+                 moveE_stand();
                  }
          if(CountA>=FA)
          {
@@ -237,6 +282,7 @@
          if(CountB>=FB)
          {
              moveB();
+             //moveE();
              CountB=0;
              }
              else
@@ -244,7 +290,18 @@
                  CountB++;
                  }    
                  
-        
+        /*
+        if(CountE>=FE)
+        {
+                 moveE();
+                 CountE=0;
+                 }
+                 else
+                 {
+                     CountE++;
+                     }
+                 
+        */ //6/29/2015 Tail
         if(x<300&&x>100) 
         {
             //AngleLeft=-0.0005*x+0.18;