Bチーム / Mbed 2 deprecated Catchtest

腕のプログラムです.

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?

UserRevisionLine numberNew 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 }