Diff: main.cpp

Revision:

12:8ff721b67e49

Parent:

11:748134b48d24

--- a/main.cpp	Sat Sep 27 12:07:31 2014 +0000
+++ b/main.cpp	Mon Oct 06 08:58:28 2014 +0000
@@ -13,6 +13,7 @@
  ***関数の内容
  *void WaitStop()   :use  wait
  *void MotorStop()  :erase motor pulses
+ *+void MotorStart():lifting wheel pulses make 
  *void TempraStop() :stop motor of crawler
  *void Photo()      :main processing ,wheel(lift or lower)motor move start
  *void Reset()      :wheels lower 
@@ -24,6 +25,10 @@
  ***********************
  ***追記
  *
+ *前足もセンサが反応した後，少し経過してからliftする仕様にしました．
+ *
+ *
+ *確認用LEDを追加します．ピンはPWMの部分と置換予定．(出力100％だから関係ないため．)
  *
  *
  */
@@ -32,8 +37,10 @@
 
 
 #define MOTOR_NUM 8             //motor & sensor num
-#define MOVE_TIME 0.10         //lift a wheel for 0.090 seconds
-#define MOVE_DOWN_TIME 0.115     //lower a wheel for 0.10 seconds
+#define START_TIME_B 0.237      //motor start wait time of Back  //a lot
+#define START_TIME_F 0.04       //motor start wait time of Front //a few
+#define MOVE_TIME 0.10          //lift a wheel for 0.090 seconds
+#define MOVE_DOWN_TIME 0.115    //lower a wheel for 0.10 seconds
 #define TEMPRA_TIME 4.0         //move a crawler for 4.0 seconds
 #define WAIT_TIME 0.010         //make wait time 
 #define PWM_LEVEL 1.0           //motors pwm setting level
@@ -49,6 +56,7 @@
 BusOut Tempra(p25, p26, p27, p28 );
 PwmOut Pwm[4] = {p21, p22, p23, p24 };
 
+Timeout StartTimer[4];
 Timeout StopTimer[8];
 Timeout StopTempra;
 Timeout StopWait[4];
@@ -88,6 +96,7 @@
 }
 
 
+
 void MotorStop0()
 {
     Motor = Motor & 0x7F;
@@ -126,6 +135,38 @@
 }
 
 
+
+
+void MotorStart0()
+{
+    StopTimer[0].attach(&MotorStop0,MOVE_TIME);
+    Motor = Motor & 0xBF;
+    Motor = Motor | 0x80;//10000000
+}
+void MotorStart1()
+{
+    StopTimer[2].attach(&MotorStop2,MOVE_TIME);
+    Motor = Motor & 0xFB;
+    Motor = Motor | 0x08;//00001000
+}
+
+void MotorStart2()
+{
+    StopTimer[4].attach(&MotorStop4,MOVE_TIME);
+    Motor = Motor & 0xEF;
+    Motor = Motor | 0x20;//00100000
+
+}
+void MotorStart3()
+{
+    StopTimer[6].attach(&MotorStop6,MOVE_TIME);
+    Motor = Motor & 0xFE;
+    Motor = Motor | 0x02;//00000010
+}
+
+
+
+
 void TempraStop()
 {
     Tempra = 0x00;
@@ -138,9 +179,7 @@
     }
     if((Way == 1)&&!Wait[0]) {
         if(!MotorState[0]) {
-            StopTimer[0].attach(&MotorStop0,MOVE_TIME);
-            Motor = Motor & 0xBF;
-            Motor = Motor | 0x80;//10000000
+            StartTimer[0].attach(&MotorStart0,START_TIME_F);
             MotorState[0] = 1;
             Led[0]= 1;
         } else {
@@ -166,13 +205,11 @@
         }
     }
 }
-void Photo2() //two Up
+void Photo2() //two Up //後ろあし
 {
     if((Way == 1)&&!Wait[1]) {
         if(!MotorState[1]) {
-            StopTimer[2].attach(&MotorStop2,MOVE_TIME);
-            Motor = Motor & 0xFB;
-            Motor = Motor | 0x08;//00001000
+            StartTimer[1].attach(&MotorStart1,START_TIME_B);
             MotorState[1] = 1;
             Led[1] = 1;
         } else {
@@ -205,9 +242,7 @@
     }
     if((Way == 1)&&!Wait[2]) {
         if(!MotorState[2]) {
-            StopTimer[4].attach(&MotorStop4,MOVE_TIME);
-            Motor = Motor & 0xEF;
-            Motor = Motor | 0x20;//00100000
+            StartTimer[2].attach(&MotorStart2,START_TIME_F);
             MotorState[2] = 1;
             Led[2] = 1;
         } else {
@@ -237,9 +272,7 @@
 {
     if((Way == 1)&&!Wait[3]) {
         if(!MotorState[3]) {
-            StopTimer[6].attach(&MotorStop6,MOVE_TIME);
-            Motor = Motor & 0xFE;
-            Motor = Motor | 0x02;//00000010
+            StartTimer[3].attach(&MotorStart3,START_TIME_B);
             MotorState[3] = 1;
             Led[3] = 1;
         } else {
@@ -254,7 +287,7 @@
             FlagAdd[3] = 1;
         } else if(MotorState[3]) {
             StopTimer[7].attach(&MotorStop7,MOVE_DOWN_TIME);
-            Motor = Motor & 0xFD;
+            Motor = Motor & 0xFD;//11111101
             Motor = Motor | 0x01;//00000001
             MotorState[3] = 0;
             Flag[3] = 0;