Revision 4:1102ab499430, committed 2014-09-30

Comitter:

lilac0112_1

Date:

Tue Sep 30 12:28:33 2014 +0000

Parent:

3:a1489a94c436

Child:

5:c2a278842131

Commit message:

?;

Changed in this revision

--- a/main.cpp	Wed Sep 03 07:44:20 2014 +0000
+++ b/main.cpp	Tue Sep 30 12:28:33 2014 +0000
@@ -1,19 +1,26 @@
 #include "mbed.h"
 #include "MultiSerial.h"
 
-#define DATA_NUM 10 //DATA_NUM[byte]通信
+#define DATA_NUM 1 //DATA_NUM[byte]通信
 #define MY_KEYCODE 0xAA//keycode
 #define EN_KEYCODE 0xAA//keycode
 #define KEY 0//number of first data
 #define CHECK DATA_NUM-1//number of last data
 #define TIME_LIMIT 1.0//腕が閉まっても動けるのはTIME_LIMIT[s]まで
-#define ARM_REV 1.0
-#define TURN_REV 0.5
+#define ARM_REV 1.0//腕のPWM
+#define TURN_REV 0.0//回転の初期PWM
+#define TURN_RATIO 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 KEYCODE 0xAA
 
-volatile uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
-volatile uint8_t count=0;
+uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
+uint8_t count=0;
+uint8_t TURN, IO,SIG;
+uint8_t data[DATA_NUM];
 
 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
 enum Arm{R=0, L, C};//右腕と左腕
@@ -21,10 +28,6 @@
 enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
 enum Compass{N=0, E, W, S};//マシンの回転
 
-bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
-    return (n>>(bit-1))%2;
-}
-
 BusOut LED(LED1, LED2, LED3, LED4);//確認用
 
 /* 
@@ -33,8 +36,7 @@
 
 MultiSerial leg_mbed(p13,p14,read);
 
-uint8_t data[1];
-
+//BusIn mt(p5, p6, p7, p8);
 //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
@@ -47,8 +49,10 @@
 //ex)Turn[CW]=ON;
 Serial pc(USBTX, USBRX);
 Ticker flick;
+Ticker Accelerator;
 Timeout Touch;//腕で掴むときの安全対策
 
+void Acceleration(void);
 void StopCatching(void);//literally
 void StopLeaving(void);//literally
 void StartCatching(void);//literally
@@ -57,27 +61,32 @@
 void LeftCatch(void);//左腕のリミットスイッチ
 void CenterCatch(void);//中央のリミットスイッチ
 void illumination(void);//literally
+uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one);
+bool GetBit(uint8_t n, uint8_t bit);
 void SetUp(void);//literally
 
+
 int main(void) {
     
+    //mt.mode(PullUp);
+    
     SetUp();
     
     while(1) {
         
         leg_mbed.read_data(data,KEYCODE);
         
-        INdata[1]= data[0];
+        INdata[0] = data[0];// = ~mt;
         
-        if(GetBit(INdata[1], 2)==OFF) StopLeaving();//停止
+        if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
         
-        if((GetBit(INdata[1], 1)+GetBit(INdata[1], 2))==1){//同時押し防止
-            if(GetBit(INdata[1], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
-            if(GetBit(INdata[1], 2)==true){//腕を広げる動作はマニュアル操作
+        if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
+            if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
+            if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
                 StopCatching();//停止
                 Touch.detach();//ワンショットタイマー停止
-                Hand[R][Open] = GetBit(INdata[1], 2)&&OneEighty[R];
-                Hand[L][Open] = GetBit(INdata[1], 2)&&OneEighty[L];
+                Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
+                Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
             }
         }
         
@@ -96,22 +105,60 @@
         //    Touch.detach();//ワンショットタイマー停止
         //}
         
-        LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
+        //LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
+        LED = INdata[0];
         
         //上体回転
         
-        if(GetBit(INdata[1], 3)==OFF) Turn[CW] = OFF;//停止
-        if(GetBit(INdata[1], 4)==OFF) Turn[CCW] = OFF;//停止
+        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;//停止
         
-        if((GetBit(INdata[1], 3)+GetBit(INdata[1], 4))==1){//同時押し防止
-            
-            Turn[CW] = GetBit(INdata[1], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
-            Turn[CCW] = GetBit(INdata[1], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
+        if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4)==1)&&(TurnSpeed==0)){//同時押し防止
+        
+            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;
+        
+        
     }
 }
 
+void Acceleration(void){
+    
+    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)&&(((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;
+        
+    }
+    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;
+
+    }
+    i = TurnSpeed;
+    pc.printf("%f, %d\n", i, IO);
+}
 void StopCatching(void){//停止
     Hand[R][Close]=Hand[L][Close]=OFF;
 }
@@ -162,14 +209,21 @@
     count++;
     LED = count;
 }
+uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){
+    return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one;
+}
+bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
+    return (n>>(bit-1))%2;
+}
 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);
     
     HandSW[R].mode(PullUp);
     HandSW[L].mode(PullUp);
@@ -186,4 +240,5 @@
     TurnSW[N].mode(PullUp);
     TurnSW[E].mode(PullUp);
     
+    wait(0.5);
 }
\ No newline at end of file