腕のプログラムです.
Dependencies: mbed TextLCD MultiSerial Pswitch
Revision 11:1ee158b83f3f, committed 2014-10-16
- Comitter:
- lilac0112_1
- Date:
- Thu Oct 16 23:25:51 2014 +0000
- Parent:
- 10:557c74f2bdee
- Commit message:
- ??????
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 16 11:25:17 2014 +0000 +++ b/main.cpp Thu Oct 16 23:25:51 2014 +0000 @@ -4,11 +4,11 @@ #define DATA_NUM 1 //DATA_NUM[byte]通信 #define KEYCODE 0xAA - + #define TIME_LIMIT 1.25//腕が閉まっても動けるのはTIME_LIMIT[s]まで #define ARM_REV 1.0//腕のPWM #define ARM_LIMIT_REV 0.5// - + #define TURN_REV 0.5//回転の初期PWM値 #define FIX_REV 0.2 @@ -17,7 +17,7 @@ bool Interrupt=0; bool IO[2]={0}; bool FIX=0; - + enum Logic{OFF=0, ON};//殆どはモーターのON,OFFに使う enum Arm{R=0, L, C};//右腕と左腕 enum Clip{Open=0, Close};//開ける動作と閉める動作 @@ -26,10 +26,9 @@ BusOut LED(LED1, LED2, LED3, LED4);//確認用 BusIn mt(p5, p6, p7, p8);//確認用 -//DigitalOut servo_mbed[2] = {p9, p10}; -MultiSerial servo_mbed(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};//R, L, C DigitalIn TurnSW[2]={p11, p12};// @@ -41,7 +40,7 @@ //ex)Turn[CW]=ON; Serial pc(USBTX, USBRX); Timeout Touch;//腕で掴むときの安全対策 - + void StopCatching(void);//literally void StopLeaving(void);//literally void StartCatching(void);//literally @@ -49,7 +48,6 @@ 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); @@ -58,39 +56,22 @@ int main(void) { - //leg_mbed.start_read(); - //leg_mbed.read_data(data,KEYCODE); - - servo_mbed.start_write(); - servo_mbed.write_data(INdata,KEYCODE); - - //display_LED(1); + SetUp(); + + 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(); - - //wait(0.5); - - //MultiSerial leg_mbed(p13,p14); - - SetUp(); + leg_mbed.check_rx_wait(); while(1) { - - //NVIC_DisableIRQ(UART0_IRQn);//通信割り込み禁止 - //NVIC_DisableIRQ(UART1_IRQn);//通信割り込み禁止 - //NVIC_DisableIRQ(UART2_IRQn);//通信割り込み禁止 - //NVIC_DisableIRQ(UART3_IRQn);//通信割り込み禁止 - //leg_mbed.read_data(data,KEYCODE); - - //INdata[0] = data[0]; - INdata[0] = ~mt; - - //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]); + INdata[0] = data[0]; + //INdata[0] = ~mt; LED = INdata[0]; @@ -100,7 +81,7 @@ 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)) StopCatching(), StopLeaving(), Touch.detach(), Interrupt=OFF; if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止 if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険 @@ -132,22 +113,15 @@ LeftCatch(); } if(HandSW[C].count() > 0){ - if(GetBit(INdata[0], 5)) CenterCatch(); + CenterCatch(); } - //LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) )); - //上体回転 - //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)) Turn[CW]=Turn[CCW]=OFF; if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止 @@ -229,31 +203,19 @@ 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; @@ -276,18 +238,10 @@ HandSpeed[R] = HandSpeed[L] = ARM_REV; TurnSpeed = TURN_REV; - //HandSW[R].mode(PullUp); - //HandSW[L].mode(PullUp); - //HandSW[C].mode(PullUp); - //HandSW[R].fall(RightCatch); - //HandSW[L].fall(LeftCatch); - //HandSW[C].fall(CenterCatch); - OneEighty[R].mode(PullUp); OneEighty[L].mode(PullUp); TurnSW[N].mode(PullUp); TurnSW[E].mode(PullUp); -} - \ No newline at end of file +} \ No newline at end of file