腕のプログラムです.
Dependencies: mbed TextLCD MultiSerial Pswitch
Diff: main.cpp
- Revision:
- 4:1102ab499430
- Parent:
- 3:a1489a94c436
- Child:
- 5:c2a278842131
diff -r a1489a94c436 -r 1102ab499430 main.cpp --- 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