腕のプログラムです.
Dependencies: mbed TextLCD MultiSerial Pswitch
main.cpp
- Committer:
- lilac0112_1
- Date:
- 2014-10-16
- Revision:
- 10:557c74f2bdee
- Parent:
- 9:8543cb951a70
- Child:
- 11:1ee158b83f3f
File content as of revision 10:557c74f2bdee:
#include "mbed.h" #include "MultiSerial.h" #include "Pswitch.h" #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 uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={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};//右腕と左腕 enum Clip{Open=0, Close};//開ける動作と閉める動作 enum Revolution{CW=0, CCW};//正回転,逆回転(上体回転) enum Compass{N=0, E, W, S};//マシンの回転 BusOut LED(LED1, LED2, LED3, LED4);//確認用 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};//R, L, C DigitalIn TurnSW[2]={p11, p12};// PwmOut HandSpeed[2]={p23, p24}; PwmOut TurnSpeed(p22); DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}}; //ex)Hand[R][Close]=ON; DigitalOut Turn[2]={p29, p30}; //ex)Turn[CW]=ON; Serial pc(USBTX, USBRX); Timeout Touch;//腕で掴むときの安全対策 void StopCatching(void);//literally void StopLeaving(void);//literally void StartCatching(void);//literally void StartLeaving(void);//literally 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 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); //leg_mbed.check_rx_wait(); //wait(0.5); //MultiSerial leg_mbed(p13,p14); SetUp(); 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]); 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){//腕を広げる動作はマニュアル操作 StopCatching();//停止 Touch.detach();//ワンショットタイマー停止 Interrupt=OFF; Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R]; Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L]; } } if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止 StopCatching();//停止 Touch.detach();//ワンショットタイマー停止 Interrupt=OFF; } if(!OneEighty[R]) Hand[R][Open]=OFF; if(!OneEighty[L]) Hand[L][Open]=OFF; if(!HandSW[R]) Hand[R][Close]=OFF; if(!HandSW[L]) Hand[L][Close]=OFF; if(HandSW[R].count() > 0){ RightCatch(); } if(HandSW[L].count() > 0){ LeftCatch(); } if(HandSW[C].count() > 0){ if(GetBit(INdata[0], 5)) 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))==1){//同時押し防止 Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて,かつリミットスイッチが押されていなかったら右回転 Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて,かつリミットスイッチが押されていなかったら左回転 } if(!TurnSW[N]) Turn[CW]=OFF, FIX=ON;//リミットスイッチによる停止 if(!TurnSW[E]) Turn[CCW]=OFF, FIX=ON; } } void StopCatching(void){//停止 Hand[R][Close]=Hand[L][Close]=OFF; if(Interrupt==ON) Interrupt=OFF; } void StopLeaving(void){//停止 Hand[R][Open]=Hand[L][Open]=OFF; } 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]; } if((!HandSW[R])&&(!HandSW[L])){ Hand[R][Close] = Hand[L][Close] = OFF; } else{ Hand[R][Close]=ON; Hand[L][Close]=ON; } } void StartLeaving(void){//離す(リミットスイッチが押されてなければ) HandSpeed[R] = HandSpeed[L] = ARM_LIMIT_REV; Hand[R][Open]=OneEighty[R]; Hand[L][Open]=OneEighty[L]; } void RightCatch(void){//右腕のリミットスイッチ Hand[R][Close] = OFF; if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止 StopCatching();//停止 Touch.detach();//ワンショットタイマー停止 Interrupt=OFF; } } void LeftCatch(void){//左腕のリミットスイッチ Hand[L][Close] = OFF; if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止 StopCatching();//停止 Touch.detach();//ワンショットタイマー停止 Interrupt=OFF; } } void CenterCatch(void){//中央のリミットスイッチ if((!OneEighty[R])&&(!OneEighty[L])){ 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; } 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); 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); }