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