Revision 9:8543cb951a70, committed 2014-10-16

Comitter:

lilac0112_1

Date:

Thu Oct 16 11:02:39 2014 +0000

Parent:

8:4f9c4b791bfe

Child:

10:557c74f2bdee

Commit message:

More Safely

Changed in this revision

--- a/MultiSerial.lib	Sun Oct 05 08:21:56 2014 +0000
+++ b/MultiSerial.lib	Thu Oct 16 11:02:39 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/B/code/MultiSerial/#26262b75f40b
+http://developer.mbed.org/teams/B/code/MultiSerial/#9e14b87bfdac

--- a/main.cpp	Sun Oct 05 08:21:56 2014 +0000
+++ b/main.cpp	Thu Oct 16 11:02:39 2014 +0000
@@ -7,13 +7,16 @@
 
 #define TIME_LIMIT 1.25//腕が閉まっても動けるのはTIME_LIMIT[s]まで
 #define ARM_REV 1.0//腕のPWM
+#define ARM_LIMIT_REV 0.5//
 
-#define TURN_REV 0.2//回転の初期PWM値
-
+#define TURN_REV 0.5//回転の初期PWM値
+#define FIX_REV 0.2
  
 uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
-uint8_t Interrupt=0;
 uint8_t data[DATA_NUM];
+bool Interrupt=0;
+bool IO[2]={0};
+bool FIX=0;
 
 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
 enum Arm{R=0, L, C};//右腕と左腕
@@ -22,21 +25,19 @@
 enum Compass{N=0, E, W, S};//マシンの回転
  
 BusOut LED(LED1, LED2, LED3, LED4);//確認用
- 
-/* 
-    BusIn mt(p19, p20, p17, p18);//Switch
-*/
- 
- 
-BusIn mt(p5, p6, p7, p8);//mt(p7, p8);
+BusIn mt(p5, p6, p7, p8);//確認用
+//DigitalOut servo_mbed[2] = {p9, p10};
+MultiSerial servo_mbed(p9, p10);
+MultiSerial leg_mbed(p13,p14);
+
 DigitalIn OneEighty[2]={p18, p19};//R, L
-Pswitch HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C
+Pswitch HandSW[3]={p15, p16, p17};//R, L, C
 DigitalIn TurnSW[2]={p11, p12};//
 PwmOut HandSpeed[2]={p23, p24};
-PwmOut TurnSpeed(p22);//p22
-DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};//{p17, p18}, {p19, p20}
+PwmOut TurnSpeed(p22);
+DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};
 //ex)Hand[R][Close]=ON;
-DigitalOut Turn[2]={p29, p30};//p29, p30
+DigitalOut Turn[2]={p29, p30};
 //ex)Turn[CW]=ON;
 Serial pc(USBTX, USBRX);
 Timeout Touch;//腕で掴むときの安全対策
@@ -48,6 +49,8 @@
 void RightCatch(void);//右腕のリミットスイッチ
 void LeftCatch(void);//左腕のリミットスイッチ
 void CenterCatch(void);//中央のリミットスイッチ
+void FixToNinety(void);
+void display_LED(int kind);//start display LED
 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
@@ -55,32 +58,45 @@
  
 int main(void) {
     
-    wait(0.5);
+    leg_mbed.start_read();
+    leg_mbed.read_data(data,KEYCODE);
+
+    servo_mbed.start_write();
+    servo_mbed.write_data(INdata,KEYCODE);
+
+    display_LED(1);
+        
+    leg_mbed.check_rx_wait();
     
-    MultiSerial leg_mbed(p13,p14,read);
-    //Serial servo_mbed(p9, p10);
+    //wait(0.5);
+    
+    //MultiSerial leg_mbed(p13,p14);
     
     SetUp();
     
     while(1) {
         
-        leg_mbed.read_data(data,KEYCODE);
+        //leg_mbed.read_data(data,KEYCODE);
         
-        INdata[0] = data[0];// | ~mt;
-        
+        INdata[0] = data[0];
         //INdata[0] = ~mt;
         
-        //EXdata[0] = LinkBit(0, 0, 0, 0, 0, 0, !OneEighty[R], !OneEighty[L]);
-        //EXdata[0] = 3;//~mt;
-        LED =   INdata[0];//~mt;//~mt;//EXdata[0]| servo_mbed.getc()*4;
+        //if(!TurnSW[N]) IO[0]=ON;
+        //if(!TurnSW[E]) IO[1]=ON;
+        //servo_mbed[0] = IO[0]&&IO[1];
+        //servo_mbed[1] = IO[0]&&IO[1];
+        //servo_mbed.putc(IO[0]&&IO[1]);
         
-        //servo_mbed.putc(~mt);
-        //pc.printf("%5d, %f\n", Holder, TurnSpeed.read());
+        LED = INdata[0];
+        
+        //腕
         
         if((GetBit(INdata[0], 1)==OFF)&&(Interrupt==OFF)) StopCatching();
         
         if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
         
+        if(GetBit(INdata[0], 1)&&GetBit(INdata[0], 2)) StopCatching(), StopLeaving(), Touch.detach(), Interrupt=OFF;
+        
         if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
             if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
             if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
@@ -111,17 +127,23 @@
             LeftCatch();
         }
         if(HandSW[C].count() > 0){
-            CenterCatch();
+            if(GetBit(INdata[0], 5)) CenterCatch();
         }
         
         //LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
-        LED = INdata[0];
         
         //上体回転
         
+        //if(FIX&&(!((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1))){
+        //     FixToNinety();
+        //     continue;
+        //}
+        
         if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
         if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
         
+        if(GetBit(INdata[0], 3)&&GetBit(INdata[0], 4)) Turn[CW]=Turn[CCW]=OFF;
+        
         if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止
         
             Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
@@ -129,8 +151,8 @@
             
         }
         
-        if(!TurnSW[N]) Turn[CW]=OFF;//リミットスイッチによる停止
-        if(!TurnSW[E]) Turn[CCW]=OFF;
+        if(!TurnSW[N]) Turn[CW]=OFF, FIX=ON;//リミットスイッチによる停止
+        if(!TurnSW[E]) Turn[CCW]=OFF, FIX=ON;
         
         
     }
@@ -144,6 +166,8 @@
 }
 void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
     
+    HandSpeed[R] = HandSpeed[L] = ARM_REV;
+    
     if(GetBit(INdata[0], 1)==true){
         Hand[R][Close]=HandSW[R];
         Hand[L][Close]=HandSW[L];
@@ -161,6 +185,9 @@
     
 }
 void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
+    
+    HandSpeed[R] = HandSpeed[L] = ARM_LIMIT_REV;
+    
     Hand[R][Open]=OneEighty[R];
     Hand[L][Open]=OneEighty[L];
 }
@@ -187,14 +214,50 @@
 void CenterCatch(void){//中央のリミットスイッチ
  
     if((!OneEighty[R])&&(!OneEighty[L])){
-        Interrupt=ON;
-        Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
-        StartCatching();
+        if(GetBit(INdata[0], 1)==false){
+            Interrupt=ON;
+            Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
+            StartCatching();
+        }
     }
     if(GetBit(INdata[0], 1)==true){
         StartCatching();
     }
 }
+void FixToNinety(void){
+    
+    if((FIX)&&(TurnSpeed != FIX_REV)) TurnSpeed = FIX_REV;
+    
+    if((!TurnSW[N])&&(TurnSW[E])) Turn[CCW]=ON;
+    
+    if((TurnSW[N])&&(!TurnSW[E])) Turn[CW]=ON;
+    
+    if((TurnSW[N])&&(TurnSW[E])) Turn[CW]=Turn[CCW]=OFF, FIX=OFF;
+    
+    if((!FIX)&&(TurnSpeed != TURN_REV)) TurnSpeed = TURN_REV;
+    
+}
+void display_LED(int kind){/*start display LED*/
+
+    switch(kind) {
+
+        case 0: //err check
+            LED = 0xA;
+            wait(0.5);
+            LED = 0x5;
+            wait(0.5);
+
+            break;
+
+        case 1:
+            //
+            LED = 0xF;
+            wait(0.5);
+            LED = 0;
+            wait(0.5);
+            break;
+    }
+}
 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;
 }