腕のプログラムです.
Dependencies: mbed TextLCD MultiSerial Pswitch
Diff: main.cpp
- Revision:
- 9:8543cb951a70
- Parent:
- 8:4f9c4b791bfe
- Child:
- 10:557c74f2bdee
--- 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; }