腕のプログラムです.
Dependencies: mbed TextLCD MultiSerial Pswitch
main.cpp@0:4baeb4381394, 2014-07-12 (annotated)
- Committer:
- lilac0112_1
- Date:
- Sat Jul 12 03:43:45 2014 +0000
- Revision:
- 0:4baeb4381394
- Child:
- 1:006debaed874
???????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lilac0112_1 | 0:4baeb4381394 | 1 | #include "mbed.h" |
lilac0112_1 | 0:4baeb4381394 | 2 | //NC = 調整中なので無機能にしておく |
lilac0112_1 | 0:4baeb4381394 | 3 | bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る) |
lilac0112_1 | 0:4baeb4381394 | 4 | return (n>>(bit-1))%2; |
lilac0112_1 | 0:4baeb4381394 | 5 | } |
lilac0112_1 | 0:4baeb4381394 | 6 | BusOut LED(LED1, LED2, LED3, LED4);//確認用 |
lilac0112_1 | 0:4baeb4381394 | 7 | Ticker flick; |
lilac0112_1 | 0:4baeb4381394 | 8 | enum Logic{OFF=0, ON};//殆どはモーターのON,OFFに使う |
lilac0112_1 | 0:4baeb4381394 | 9 | //パワーウィンドウ(腕)関連 |
lilac0112_1 | 0:4baeb4381394 | 10 | #define TimeLimit 1.0//腕が閉まっても動けるのはTimeLimit[s]まで |
lilac0112_1 | 0:4baeb4381394 | 11 | #define ArmRev .25 |
lilac0112_1 | 0:4baeb4381394 | 12 | enum Arm{R=0, L, C};//右腕と左腕 |
lilac0112_1 | 0:4baeb4381394 | 13 | enum Clip{Open=0, Close};//開ける動作と閉める動作3 |
lilac0112_1 | 0:4baeb4381394 | 14 | InterruptIn HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C |
lilac0112_1 | 0:4baeb4381394 | 15 | PwmOut HandSpeed[2]={p25, p26}; |
lilac0112_1 | 0:4baeb4381394 | 16 | DigitalOut Hand[2][2]={{p30, p29}, {p28, p27}};//{p17, p18}, {p19, p20} |
lilac0112_1 | 0:4baeb4381394 | 17 | //ex)Hand[R][Close]=ON; |
lilac0112_1 | 0:4baeb4381394 | 18 | Timeout Touch;//腕で掴むときの安全対策 |
lilac0112_1 | 0:4baeb4381394 | 19 | void StopCatching(void){//停止 |
lilac0112_1 | 0:4baeb4381394 | 20 | Hand[R][Close]=Hand[L][Close]=OFF; |
lilac0112_1 | 0:4baeb4381394 | 21 | } |
lilac0112_1 | 0:4baeb4381394 | 22 | void StopLeaving(void){//停止 |
lilac0112_1 | 0:4baeb4381394 | 23 | Hand[R][Open]=Hand[L][Open]=OFF; |
lilac0112_1 | 0:4baeb4381394 | 24 | } |
lilac0112_1 | 0:4baeb4381394 | 25 | void StartCatching(void){//掴む |
lilac0112_1 | 0:4baeb4381394 | 26 | Hand[R][Close]=Hand[L][Close]=ON; |
lilac0112_1 | 0:4baeb4381394 | 27 | } |
lilac0112_1 | 0:4baeb4381394 | 28 | void StartLeaving(void){//離す |
lilac0112_1 | 0:4baeb4381394 | 29 | Hand[R][Open]=Hand[L][Open]=ON; |
lilac0112_1 | 0:4baeb4381394 | 30 | } |
lilac0112_1 | 0:4baeb4381394 | 31 | void RightCatch(void){//右腕のリミットスイッチ |
lilac0112_1 | 0:4baeb4381394 | 32 | if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止 |
lilac0112_1 | 0:4baeb4381394 | 33 | StopCatching();//停止 |
lilac0112_1 | 0:4baeb4381394 | 34 | Touch.detach();//ワンショットタイマー停止 |
lilac0112_1 | 0:4baeb4381394 | 35 | } |
lilac0112_1 | 0:4baeb4381394 | 36 | } |
lilac0112_1 | 0:4baeb4381394 | 37 | void LeftCatch(void){//左腕のリミットスイッチ |
lilac0112_1 | 0:4baeb4381394 | 38 | if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止 |
lilac0112_1 | 0:4baeb4381394 | 39 | StopCatching();//停止 |
lilac0112_1 | 0:4baeb4381394 | 40 | Touch.detach();//ワンショットタイマー停止 |
lilac0112_1 | 0:4baeb4381394 | 41 | } |
lilac0112_1 | 0:4baeb4381394 | 42 | } |
lilac0112_1 | 0:4baeb4381394 | 43 | void CenterCatch(void){//中央のリミットスイッチ |
lilac0112_1 | 0:4baeb4381394 | 44 | Touch.attach(StopCatching, TimeLimit);//TimeLimit[s]で停止 |
lilac0112_1 | 0:4baeb4381394 | 45 | StartCatching(); |
lilac0112_1 | 0:4baeb4381394 | 46 | } |
lilac0112_1 | 0:4baeb4381394 | 47 | //通信関連 |
lilac0112_1 | 0:4baeb4381394 | 48 | #define DATA_NUM 10 //DATA_NUM[byte]通信 |
lilac0112_1 | 0:4baeb4381394 | 49 | #define MY_KEYCODE 0xAA//keycode |
lilac0112_1 | 0:4baeb4381394 | 50 | #define EN_KEYCODE 0xAA//keycode |
lilac0112_1 | 0:4baeb4381394 | 51 | #define KEY 0//number of first data |
lilac0112_1 | 0:4baeb4381394 | 52 | #define CHECK DATA_NUM-1//number of last data |
lilac0112_1 | 0:4baeb4381394 | 53 | |
lilac0112_1 | 0:4baeb4381394 | 54 | Serial pc(USBTX, USBRX); |
lilac0112_1 | 0:4baeb4381394 | 55 | volatile uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0}; |
lilac0112_1 | 0:4baeb4381394 | 56 | volatile uint8_t count=0; |
lilac0112_1 | 0:4baeb4381394 | 57 | void illumination(){ |
lilac0112_1 | 0:4baeb4381394 | 58 | count++; |
lilac0112_1 | 0:4baeb4381394 | 59 | LED = count; |
lilac0112_1 | 0:4baeb4381394 | 60 | } |
lilac0112_1 | 0:4baeb4381394 | 61 | |
lilac0112_1 | 0:4baeb4381394 | 62 | int main(void) { |
lilac0112_1 | 0:4baeb4381394 | 63 | //簡単そうなコードの説明は省く |
lilac0112_1 | 0:4baeb4381394 | 64 | BusIn mt(p5, p6, p7, p8, p9, p10);//調整用スイッチ |
lilac0112_1 | 0:4baeb4381394 | 65 | mt.mode(PullUp); |
lilac0112_1 | 0:4baeb4381394 | 66 | HandSpeed[R] = HandSpeed[L] = ArmRev; |
lilac0112_1 | 0:4baeb4381394 | 67 | flick.attach(illumination, .5); |
lilac0112_1 | 0:4baeb4381394 | 68 | //パワーウィンドウ(腕)関連 |
lilac0112_1 | 0:4baeb4381394 | 69 | HandSW[R].mode(PullUp); |
lilac0112_1 | 0:4baeb4381394 | 70 | HandSW[L].mode(PullUp); |
lilac0112_1 | 0:4baeb4381394 | 71 | HandSW[C].mode(PullUp); |
lilac0112_1 | 0:4baeb4381394 | 72 | HandSW[R].fall(RightCatch);//立ち上がり |
lilac0112_1 | 0:4baeb4381394 | 73 | HandSW[L].fall(LeftCatch);//立ち上がり |
lilac0112_1 | 0:4baeb4381394 | 74 | HandSW[C].fall(CenterCatch);//立ち上がり |
lilac0112_1 | 0:4baeb4381394 | 75 | while(1) { |
lilac0112_1 | 0:4baeb4381394 | 76 | //腕のプログラム |
lilac0112_1 | 0:4baeb4381394 | 77 | INdata[1]= ~mt;//調整用スイッチ |
lilac0112_1 | 0:4baeb4381394 | 78 | if(GetBit(INdata[1], 2)==OFF) StopLeaving();//停止 |
lilac0112_1 | 0:4baeb4381394 | 79 | if((GetBit(INdata[1], 1)+GetBit(INdata[1], 2))==1){//同時押し防止 |
lilac0112_1 | 0:4baeb4381394 | 80 | if(GetBit(INdata[1], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険 |
lilac0112_1 | 0:4baeb4381394 | 81 | if(GetBit(INdata[1], 2)==true){//腕を広げる動作はマニュアル操作 |
lilac0112_1 | 0:4baeb4381394 | 82 | StopCatching();//停止 |
lilac0112_1 | 0:4baeb4381394 | 83 | Touch.detach();//ワンショットタイマー停止 |
lilac0112_1 | 0:4baeb4381394 | 84 | Hand[R][Open] = Hand[L][Open] = GetBit(INdata[1], 2); |
lilac0112_1 | 0:4baeb4381394 | 85 | } |
lilac0112_1 | 0:4baeb4381394 | 86 | } |
lilac0112_1 | 0:4baeb4381394 | 87 | if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止 |
lilac0112_1 | 0:4baeb4381394 | 88 | StopCatching();//停止 |
lilac0112_1 | 0:4baeb4381394 | 89 | Touch.detach();//ワンショットタイマー停止 |
lilac0112_1 | 0:4baeb4381394 | 90 | } |
lilac0112_1 | 0:4baeb4381394 | 91 | } |
lilac0112_1 | 0:4baeb4381394 | 92 | } |