Diff: main.cpp

Revision:

8:4f9c4b791bfe

Parent:

7:c418a0d929b1

Child:

9:8543cb951a70

--- a/main.cpp	Sun Oct 05 06:50:36 2014 +0000
+++ b/main.cpp	Sun Oct 05 08:21:56 2014 +0000
@@ -8,19 +8,12 @@
 #define TIME_LIMIT 1.25//腕が閉まっても動けるのはTIME_LIMIT[s]まで
 #define ARM_REV 1.0//腕のPWM
 
-#define TURN_REV 0.2//回転の初期PWM
-#define TURN_RATE 1//加速度の変更...1, 2, 4, 8, 16...
-#define TURN_REV_LIMIT 0.5//PWMの最大数値
-#define TURN_ACCELERATION (1e-4)/1.5//[s]
-#define TURN_RATIO 1e4//Holder/TURN_RATIO
+#define TURN_REV 0.2//回転の初期PWM値
 
  
 uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
 uint8_t Interrupt=0;
-uint8_t TURN=0, IO=0,SIG=0;
 uint8_t data[DATA_NUM];
-uint16_t Holder=0;
-#define COUNT_LIMIT 10e3
 
 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
 enum Arm{R=0, L, C};//右腕と左腕
@@ -46,10 +39,8 @@
 DigitalOut Turn[2]={p29, p30};//p29, p30
 //ex)Turn[CW]=ON;
 Serial pc(USBTX, USBRX);
-Ticker Accelerator;
 Timeout Touch;//腕で掴むときの安全対策
 
-void Acceleration(void);
 void StopCatching(void);//literally
 void StopLeaving(void);//literally
 void StartCatching(void);//literally
@@ -128,29 +119,15 @@
         
         //上体回転
         
-        wait(TURN_ACCELERATION);
-        
-        Acceleration();
+        if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
+        if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
         
-        TurnSpeed.write(   (float)   (   (int)(100  *  (TURN_REV_LIMIT*((float)(Holder)/TURN_RATIO))   )  /100.0  )  ); 
-        
-        if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4)==1)&&(TurnSpeed==0)){//同時押し防止
+        if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止
         
             Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
             Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
-            TURN = LinkBit(0, 0, 0, 0, 0, 0, Turn[CW], Turn[CCW]);
             
         }
-        if(TurnSpeed!=0){
-            
-            Turn[CW] = GetBit(TURN, 2)&&TurnSW[N];
-            Turn[CCW] = GetBit(TURN, 1)&&TurnSW[E];
-            
-        }
-        if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4)!=1)&&(TurnSpeed==0)){
-            Turn[CW] = OFF;
-            Turn[CCW] = OFF;
-        }
         
         if(!TurnSW[N]) Turn[CW]=OFF;//リミットスイッチによる停止
         if(!TurnSW[E]) Turn[CCW]=OFF;
@@ -158,16 +135,6 @@
         
     }
 }
-
-void Acceleration(void){
-    
-    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)&&(Holder< 1e4)&&(TURN==SIG)) Holder++;
-    if(((IO!=1)||(TURN!=SIG))&&(0 <Holder)) Holder--;
-    
-}
 void StopCatching(void){//停止
     Hand[R][Close]=Hand[L][Close]=OFF;
     if(Interrupt==ON) Interrupt=OFF;
@@ -239,9 +206,7 @@
     mt.mode(PullUp);
     
     HandSpeed[R] = HandSpeed[L] = ARM_REV;
-    //TurnSpeed = TURN_REV;
-    
-    //Accelerator.attach(Acceleration, TURN_ACCELERATION);
+    TurnSpeed = TURN_REV;
     
     //HandSW[R].mode(PullUp);
     //HandSW[L].mode(PullUp);