Diff: main.cpp

Revision:

5:c2a278842131

Parent:

4:1102ab499430

Child:

6:606507fc8852

--- a/main.cpp	Tue Sep 30 12:28:33 2014 +0000
+++ b/main.cpp	Fri Oct 03 10:33:37 2014 +0000
@@ -8,18 +8,18 @@
 #define CHECK DATA_NUM-1//number of last data
 #define TIME_LIMIT 1.0//腕が閉まっても動けるのはTIME_LIMIT[s]まで
 #define ARM_REV 1.0//腕のPWM
-#define TURN_REV 0.0//回転の初期PWM
-#define TURN_RATIO 1000000000//なるべく大きい数
+#define TURN_REV 0.25//回転の初期PWM
+#define TURN_RATIO 100//1000000000//なるべく大きい数
 #define TURN_RATE 1//加速度の変更...1, 2, 4, 8, 16...
 #define TURN_REV_LIMIT 0.5//PWMの最大数値
-#define TURN_ACCELERATION 0.015625//なるべく小さい数
-#define TURN_SLOPE TURN_RATE*0.015625//なるべく小さい数1/2, 4, 8,...256
+#define TURN_ACCELERATION 0.1//0.015625//なるべく小さい数
+#define TURN_SLOPE TURN_RATE*0.1//0.015625//なるべく小さい数1/2, 4, 8,...256
 
 #define KEYCODE 0xAA
 
 uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
 uint8_t count=0;
-uint8_t TURN, IO,SIG;
+uint8_t TURN=0, IO=0,SIG=0;
 uint8_t data[DATA_NUM];
 
 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
@@ -34,12 +34,11 @@
     BusIn mt(p19, p20, p17, p18);//Switch
 */
 
-MultiSerial leg_mbed(p13,p14,read);
 
-//BusIn mt(p5, p6, p7, p8);
+BusIn mt(p7, p8, p9, p10);
 //DigitalIn Ninety[2]={p5, p6};//R, L
-DigitalIn OneEighty[2]={p5, p6};//R, L
-InterruptIn HandSW[3]={p7, p8, p9};//p14, p15, p16//R, L, C
+DigitalIn OneEighty[2]={p18, p19};//R, L
+InterruptIn HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C
 DigitalIn TurnSW[2]={p11, p12};//
 PwmOut HandSpeed[2]={p23, p24};
 PwmOut TurnSpeed(p22);//p22
@@ -49,9 +48,11 @@
 //ex)Turn[CW]=ON;
 Serial pc(USBTX, USBRX);
 Ticker flick;
+Ticker Save;
 Ticker Accelerator;
 Timeout Touch;//腕で掴むときの安全対策
 
+void SW_saving(void);
 void Acceleration(void);
 void StopCatching(void);//literally
 void StopLeaving(void);//literally
@@ -68,7 +69,9 @@
 
 int main(void) {
     
-    //mt.mode(PullUp);
+    wait(0.5);
+    
+    MultiSerial leg_mbed(p13,p14,read);
     
     SetUp();
     
@@ -76,7 +79,7 @@
         
         leg_mbed.read_data(data,KEYCODE);
         
-        INdata[0] = data[0];// = ~mt;
+        INdata[0] = data[0] | ~mt;
         
         if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
         
@@ -110,7 +113,7 @@
         
         //上体回転
         
-        IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
+        //IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
         
         //if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
         //if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
@@ -132,6 +135,27 @@
             Turn[CW] = OFF;
             Turn[CCW] = OFF;
         }
+        
+        //Acceleration();
+        
+        //float i;
+
+        /*SIG = LinkBit(0, 0, 0, 0, 0, 0, GetBit(INdata[0], 3), GetBit(INdata[0], 4)); 
+        IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
+        
+        if((IO==1)&&(0<=TurnSpeed)&&(TurnSpeed<TURN_REV_LIMIT)&&(TURN==SIG)){
+            
+            TurnSpeed = TurnSpeed + TURN_SLOPE;
+            wait(TURN_ACCELERATION);
+        }
+        if(((IO==0)||(IO==2)||(TURN!=SIG))&&(0<TurnSpeed)&&(TurnSpeed<=TURN_REV_LIMIT)){
+            
+            TurnSpeed = TurnSpeed - TURN_SLOPE;
+            wait(TURN_ACCELERATION);
+        }*/
+        //i = TurnSpeed;
+        //pc.printf("%f, %d\n", i, IO);
+        
         if(!TurnSW[N]) Turn[CW]=OFF;//リミットスイッチによる停止
         if(!TurnSW[E]) Turn[CCW]=OFF;
         
@@ -139,6 +163,10 @@
     }
 }
 
+void SW_saving(void){
+    IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
+    SIG = LinkBit(0, 0, 0, 0, 0, 0, GetBit(INdata[0], 3), GetBit(INdata[0], 4));
+}
 void Acceleration(void){
     
     float i;
@@ -149,12 +177,12 @@
     if((IO==1)&&(((unsigned long)(TurnSpeed*TURN_RATIO)==0)||(0<(unsigned long)(TurnSpeed*TURN_RATIO)))&&((unsigned long)(TurnSpeed*TURN_RATIO)<(unsigned long)(TURN_REV_LIMIT*TURN_RATIO))&&(TURN==SIG)){
         
         TurnSpeed = TurnSpeed + TURN_SLOPE;
-        
+        wait(TURN_ACCELERATION);
     }
     if(((IO==0)||(IO==2)||(TURN!=SIG))&&(0<(unsigned long)(TurnSpeed*TURN_RATIO))&&((unsigned long)(TurnSpeed*TURN_RATIO)<=(unsigned long)(TURN_REV_LIMIT*TURN_RATIO))){
         
         TurnSpeed = TurnSpeed - TURN_SLOPE;
-
+        wait(TURN_ACCELERATION);
     }
     i = TurnSpeed;
     pc.printf("%f, %d\n", i, IO);
@@ -202,6 +230,7 @@
     if((!OneEighty[R])&&(!OneEighty[L])){
         Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
         StartCatching();
+        LED=15;
     }
     
 }
@@ -217,13 +246,14 @@
 }
 void SetUp(void){//literally
     
-    //mt.mode(PullUp);
+    mt.mode(PullUp);
     
     HandSpeed[R] = HandSpeed[L] = ARM_REV;
     TurnSpeed = TURN_REV;
     
     //flick.attach(illumination, .5);
-    Accelerator.attach(Acceleration, TURN_ACCELERATION);
+    //Accelerator.attach(Acceleration, TURN_ACCELERATION);
+    //Save.attach(SW_saving, 0.015625);
     
     HandSW[R].mode(PullUp);
     HandSW[L].mode(PullUp);
@@ -240,5 +270,4 @@
     TurnSW[N].mode(PullUp);
     TurnSW[E].mode(PullUp);
     
-    wait(0.5);
 }
\ No newline at end of file