Bチーム / Mbed 2 deprecated Catchtest

腕のプログラムです.

Dependencies:   mbed TextLCD MultiSerial Pswitch

main.cpp@2:eb1c123c9140, 2014-09-03 (annotated)

Committer:
lilac0112_1
Date:
Wed Sep 03 05:22:12 2014 +0000
Revision:
2:eb1c123c9140
Parent:
1:006debaed874
Child:
3:a1489a94c436
?

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 2:eb1c123c9140 9 #define ARM_REV 1.0
lilac0112_1 2:eb1c123c9140 10 #define TURN_REV 0.5
lilac0112_1 1:006debaed874 11
lilac0112_1 1:006debaed874 12 volatile uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
lilac0112_1 1:006debaed874 13 volatile uint8_t count=0;
lilac0112_1 1:006debaed874 14
lilac0112_1 1:006debaed874 15 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
lilac0112_1 1:006debaed874 16 enum Arm{R=0, L, C};//右腕と左腕
lilac0112_1 1:006debaed874 17 enum Clip{Open=0, Close};//開ける動作と閉める動作
lilac0112_1 2:eb1c123c9140 18 enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
lilac0112_1 2:eb1c123c9140 19 enum Compass{N=0, E, W, S};//マシンの回転
lilac0112_1 1:006debaed874 20
lilac0112_1 2:eb1c123c9140 21 bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
lilac0112_1 0:4baeb4381394 22 return (n>>(bit-1))%2;
lilac0112_1 0:4baeb4381394 23 }
lilac0112_1 1:006debaed874 24
lilac0112_1 0:4baeb4381394 25 BusOut LED(LED1, LED2, LED3, LED4);//確認用
lilac0112_1 2:eb1c123c9140 26 BusIn mt(p19, p20, p17, p18);//Switch
lilac0112_1 2:eb1c123c9140 27 //DigitalIn Ninety[2]={p5, p6};//R, L
lilac0112_1 2:eb1c123c9140 28 DigitalIn OneEighty[2]={p5, p6};//R, L
lilac0112_1 2:eb1c123c9140 29 InterruptIn HandSW[3]={p7, p8, p9};//p14, p15, p16//R, L, C
lilac0112_1 2:eb1c123c9140 30 DigitalIn TurnSW[2]={p11, p12};//
lilac0112_1 2:eb1c123c9140 31 PwmOut HandSpeed[2]={p23, p24};
lilac0112_1 2:eb1c123c9140 32 PwmOut TurnSpeed(p22);//p22
lilac0112_1 2:eb1c123c9140 33 DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};//{p17, p18}, {p19, p20}
lilac0112_1 0:4baeb4381394 34 //ex)Hand[R][Close]=ON;
lilac0112_1 2:eb1c123c9140 35 DigitalOut Turn[2]={p29, p30};//p29, p30
lilac0112_1 2:eb1c123c9140 36 //ex)Turn[CW]=ON;
lilac0112_1 1:006debaed874 37 Serial pc(USBTX, USBRX);
lilac0112_1 1:006debaed874 38 Ticker flick;
lilac0112_1 0:4baeb4381394 39 Timeout Touch;//腕で掴むときの安全対策
lilac0112_1 1:006debaed874 40
lilac0112_1 1:006debaed874 41 void StopCatching(void);//literally
lilac0112_1 1:006debaed874 42 void StopLeaving(void);//literally
lilac0112_1 1:006debaed874 43 void StartCatching(void);//literally
lilac0112_1 1:006debaed874 44 void StartLeaving(void);//literally
lilac0112_1 1:006debaed874 45 void RightCatch(void);//右腕のリミットスイッチ
lilac0112_1 1:006debaed874 46 void LeftCatch(void);//左腕のリミットスイッチ
lilac0112_1 1:006debaed874 47 void CenterCatch(void);//中央のリミットスイッチ
lilac0112_1 1:006debaed874 48 void illumination(void);//literally
lilac0112_1 1:006debaed874 49 void SetUp(void);//literally
lilac0112_1 1:006debaed874 50
lilac0112_1 1:006debaed874 51 int main(void) {
lilac0112_1 1:006debaed874 52
lilac0112_1 1:006debaed874 53 SetUp();
lilac0112_1 1:006debaed874 54
lilac0112_1 1:006debaed874 55 while(1) {
lilac0112_1 1:006debaed874 56
lilac0112_1 1:006debaed874 57 INdata[1]= ~mt;
lilac0112_1 1:006debaed874 58
lilac0112_1 1:006debaed874 59 if(GetBit(INdata[1], 2)==OFF) StopLeaving();//停止
lilac0112_1 1:006debaed874 60
lilac0112_1 1:006debaed874 61 if((GetBit(INdata[1], 1)+GetBit(INdata[1], 2))==1){//同時押し防止
lilac0112_1 1:006debaed874 62 if(GetBit(INdata[1], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
lilac0112_1 1:006debaed874 63 if(GetBit(INdata[1], 2)==true){//腕を広げる動作はマニュアル操作
lilac0112_1 1:006debaed874 64 StopCatching();//停止
lilac0112_1 1:006debaed874 65 Touch.detach();//ワンショットタイマー停止
lilac0112_1 2:eb1c123c9140 66 Hand[R][Open] = GetBit(INdata[1], 2)&&OneEighty[R];
lilac0112_1 2:eb1c123c9140 67 Hand[L][Open] = GetBit(INdata[1], 2)&&OneEighty[L];
lilac0112_1 1:006debaed874 68 }
lilac0112_1 1:006debaed874 69 }
lilac0112_1 1:006debaed874 70
lilac0112_1 1:006debaed874 71 if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
lilac0112_1 1:006debaed874 72 StopCatching();//停止
lilac0112_1 1:006debaed874 73 Touch.detach();//ワンショットタイマー停止
lilac0112_1 1:006debaed874 74 }
lilac0112_1 1:006debaed874 75
lilac0112_1 2:eb1c123c9140 76 if(!OneEighty[R]) Hand[R][Open]=OFF;
lilac0112_1 2:eb1c123c9140 77 if(!OneEighty[L]) Hand[L][Open]=OFF;
lilac0112_1 2:eb1c123c9140 78 //if(!Ninety[R]) Hand[R][Close]=OFF;
lilac0112_1 2:eb1c123c9140 79 //if(!Ninety[L]) Hand[L][Close]=OFF;
lilac0112_1 2:eb1c123c9140 80
lilac0112_1 2:eb1c123c9140 81 //if((!Ninety[R])&&(!Ninety[L])){//どちらも押されたら停止
lilac0112_1 2:eb1c123c9140 82 // StopCatching();//停止
lilac0112_1 2:eb1c123c9140 83 // Touch.detach();//ワンショットタイマー停止
lilac0112_1 2:eb1c123c9140 84 //}
lilac0112_1 2:eb1c123c9140 85
lilac0112_1 2:eb1c123c9140 86 LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
lilac0112_1 2:eb1c123c9140 87
lilac0112_1 2:eb1c123c9140 88 //上体回転
lilac0112_1 2:eb1c123c9140 89
lilac0112_1 2:eb1c123c9140 90 if(GetBit(INdata[1], 3)==OFF) Turn[CW] = OFF;//停止
lilac0112_1 2:eb1c123c9140 91 if(GetBit(INdata[1], 4)==OFF) Turn[CCW] = OFF;//停止
lilac0112_1 2:eb1c123c9140 92
lilac0112_1 2:eb1c123c9140 93 if((GetBit(INdata[1], 3)+GetBit(INdata[1], 4))==1){//同時押し防止
lilac0112_1 2:eb1c123c9140 94
lilac0112_1 2:eb1c123c9140 95 Turn[CW] = GetBit(INdata[1], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
lilac0112_1 2:eb1c123c9140 96 Turn[CCW] = GetBit(INdata[1], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
lilac0112_1 2:eb1c123c9140 97
lilac0112_1 2:eb1c123c9140 98 }
lilac0112_1 1:006debaed874 99 }
lilac0112_1 1:006debaed874 100 }
lilac0112_1 1:006debaed874 101
lilac0112_1 0:4baeb4381394 102 void StopCatching(void){//停止
lilac0112_1 0:4baeb4381394 103 Hand[R][Close]=Hand[L][Close]=OFF;
lilac0112_1 0:4baeb4381394 104 }
lilac0112_1 0:4baeb4381394 105 void StopLeaving(void){//停止
lilac0112_1 0:4baeb4381394 106 Hand[R][Open]=Hand[L][Open]=OFF;
lilac0112_1 0:4baeb4381394 107 }
lilac0112_1 2:eb1c123c9140 108 void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
lilac0112_1 2:eb1c123c9140 109
lilac0112_1 2:eb1c123c9140 110 if((!HandSW[R])&&(!HandSW[L])){
lilac0112_1 2:eb1c123c9140 111 Hand[R][Close] = Hand[L][Close] = OFF;
lilac0112_1 2:eb1c123c9140 112 }
lilac0112_1 2:eb1c123c9140 113 else{
lilac0112_1 2:eb1c123c9140 114
lilac0112_1 2:eb1c123c9140 115 //Hand[R][Close]=Ninety[R];
lilac0112_1 2:eb1c123c9140 116 //Hand[L][Close]=Ninety[L];
lilac0112_1 2:eb1c123c9140 117
lilac0112_1 2:eb1c123c9140 118 Hand[R][Close]=ON;
lilac0112_1 2:eb1c123c9140 119 Hand[L][Close]=ON;
lilac0112_1 2:eb1c123c9140 120
lilac0112_1 2:eb1c123c9140 121 }
lilac0112_1 2:eb1c123c9140 122
lilac0112_1 0:4baeb4381394 123 }
lilac0112_1 2:eb1c123c9140 124 void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
lilac0112_1 2:eb1c123c9140 125 Hand[R][Open]=OneEighty[R];
lilac0112_1 2:eb1c123c9140 126 Hand[L][Open]=OneEighty[L];
lilac0112_1 0:4baeb4381394 127 }
lilac0112_1 0:4baeb4381394 128 void RightCatch(void){//右腕のリミットスイッチ
lilac0112_1 0:4baeb4381394 129 if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
lilac0112_1 0:4baeb4381394 130 StopCatching();//停止
lilac0112_1 0:4baeb4381394 131 Touch.detach();//ワンショットタイマー停止
lilac0112_1 0:4baeb4381394 132 }
lilac0112_1 0:4baeb4381394 133 }
lilac0112_1 0:4baeb4381394 134 void LeftCatch(void){//左腕のリミットスイッチ
lilac0112_1 0:4baeb4381394 135 if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
lilac0112_1 0:4baeb4381394 136 StopCatching();//停止
lilac0112_1 0:4baeb4381394 137 Touch.detach();//ワンショットタイマー停止
lilac0112_1 0:4baeb4381394 138 }
lilac0112_1 0:4baeb4381394 139 }
lilac0112_1 0:4baeb4381394 140 void CenterCatch(void){//中央のリミットスイッチ
lilac0112_1 2:eb1c123c9140 141
lilac0112_1 2:eb1c123c9140 142 if((!OneEighty[R])&&(!OneEighty[L])){
lilac0112_1 2:eb1c123c9140 143 Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
lilac0112_1 2:eb1c123c9140 144 StartCatching();
lilac0112_1 2:eb1c123c9140 145 }
lilac0112_1 2:eb1c123c9140 146
lilac0112_1 0:4baeb4381394 147 }
lilac0112_1 1:006debaed874 148 void illumination(void){//literally
lilac0112_1 0:4baeb4381394 149 count++;
lilac0112_1 0:4baeb4381394 150 LED = count;
lilac0112_1 0:4baeb4381394 151 }
lilac0112_1 1:006debaed874 152 void SetUp(void){//literally
lilac0112_1 1:006debaed874 153
lilac0112_1 0:4baeb4381394 154 mt.mode(PullUp);
lilac0112_1 1:006debaed874 155
lilac0112_1 1:006debaed874 156 HandSpeed[R] = HandSpeed[L] = ARM_REV;
lilac0112_1 2:eb1c123c9140 157 TurnSpeed = TURN_REV;
lilac0112_1 1:006debaed874 158
lilac0112_1 2:eb1c123c9140 159 //flick.attach(illumination, .5);
lilac0112_1 1:006debaed874 160
lilac0112_1 0:4baeb4381394 161 HandSW[R].mode(PullUp);
lilac0112_1 0:4baeb4381394 162 HandSW[L].mode(PullUp);
lilac0112_1 0:4baeb4381394 163 HandSW[C].mode(PullUp);
lilac0112_1 1:006debaed874 164 HandSW[R].fall(RightCatch);
lilac0112_1 1:006debaed874 165 HandSW[L].fall(LeftCatch);
lilac0112_1 1:006debaed874 166 HandSW[C].fall(CenterCatch);
lilac0112_1 2:eb1c123c9140 167
lilac0112_1 2:eb1c123c9140 168 OneEighty[R].mode(PullUp);
lilac0112_1 2:eb1c123c9140 169 OneEighty[L].mode(PullUp);
lilac0112_1 2:eb1c123c9140 170 //Ninety[R].mode(PullUp);
lilac0112_1 2:eb1c123c9140 171 //Ninety[L].mode(PullUp);
lilac0112_1 2:eb1c123c9140 172
lilac0112_1 2:eb1c123c9140 173 TurnSW[N].mode(PullUp);
lilac0112_1 2:eb1c123c9140 174 TurnSW[E].mode(PullUp);
lilac0112_1 2:eb1c123c9140 175
lilac0112_1 0:4baeb4381394 176 }