Bチーム / Mbed 2 deprecated Catchtest

腕のプログラムです.

Dependencies:   mbed TextLCD MultiSerial Pswitch

main.cpp@9:8543cb951a70, 2014-10-16 (annotated)

Committer:
lilac0112_1
Date:
Thu Oct 16 11:02:39 2014 +0000
Revision:
9:8543cb951a70
Parent:
8:4f9c4b791bfe
Child:
10:557c74f2bdee
More Safely

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:4baeb4381394 1 #include "mbed.h"
Hatter 3:a1489a94c436 2 #include "MultiSerial.h"
lilac0112_1 7:c418a0d929b1 3 #include "Pswitch.h"
lilac0112_1 7:c418a0d929b1 4
lilac0112_1 4:1102ab499430 5 #define DATA_NUM 1 //DATA_NUM[byte]通信
lilac0112_1 7:c418a0d929b1 6 #define KEYCODE 0xAA
lilac0112_1 7:c418a0d929b1 7
lilac0112_1 7:c418a0d929b1 8 #define TIME_LIMIT 1.25//腕が閉まっても動けるのはTIME_LIMIT[s]まで
lilac0112_1 4:1102ab499430 9 #define ARM_REV 1.0//腕のPWM
lilac0112_1 9:8543cb951a70 10 #define ARM_LIMIT_REV 0.5//
lilac0112_1 7:c418a0d929b1 11
lilac0112_1 9:8543cb951a70 12 #define TURN_REV 0.5//回転の初期PWM値
lilac0112_1 9:8543cb951a70 13 #define FIX_REV 0.2
lilac0112_1 7:c418a0d929b1 14
lilac0112_1 4:1102ab499430 15 uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
lilac0112_1 4:1102ab499430 16 uint8_t data[DATA_NUM];
lilac0112_1 9:8543cb951a70 17 bool Interrupt=0;
lilac0112_1 9:8543cb951a70 18 bool IO[2]={0};
lilac0112_1 9:8543cb951a70 19 bool FIX=0;
lilac0112_1 1:006debaed874 20
lilac0112_1 1:006debaed874 21 enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
lilac0112_1 1:006debaed874 22 enum Arm{R=0, L, C};//右腕と左腕
lilac0112_1 1:006debaed874 23 enum Clip{Open=0, Close};//開ける動作と閉める動作
lilac0112_1 2:eb1c123c9140 24 enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
lilac0112_1 2:eb1c123c9140 25 enum Compass{N=0, E, W, S};//マシンの回転
lilac0112_1 7:c418a0d929b1 26
lilac0112_1 0:4baeb4381394 27 BusOut LED(LED1, LED2, LED3, LED4);//確認用
lilac0112_1 9:8543cb951a70 28 BusIn mt(p5, p6, p7, p8);//確認用
lilac0112_1 9:8543cb951a70 29 //DigitalOut servo_mbed[2] = {p9, p10};
lilac0112_1 9:8543cb951a70 30 MultiSerial servo_mbed(p9, p10);
lilac0112_1 9:8543cb951a70 31 MultiSerial leg_mbed(p13,p14);
lilac0112_1 9:8543cb951a70 32
lilac0112_1 5:c2a278842131 33 DigitalIn OneEighty[2]={p18, p19};//R, L
lilac0112_1 9:8543cb951a70 34 Pswitch HandSW[3]={p15, p16, p17};//R, L, C
lilac0112_1 2:eb1c123c9140 35 DigitalIn TurnSW[2]={p11, p12};//
lilac0112_1 2:eb1c123c9140 36 PwmOut HandSpeed[2]={p23, p24};
lilac0112_1 9:8543cb951a70 37 PwmOut TurnSpeed(p22);
lilac0112_1 9:8543cb951a70 38 DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};
lilac0112_1 0:4baeb4381394 39 //ex)Hand[R][Close]=ON;
lilac0112_1 9:8543cb951a70 40 DigitalOut Turn[2]={p29, p30};
lilac0112_1 2:eb1c123c9140 41 //ex)Turn[CW]=ON;
lilac0112_1 1:006debaed874 42 Serial pc(USBTX, USBRX);
lilac0112_1 0:4baeb4381394 43 Timeout Touch;//腕で掴むときの安全対策
lilac0112_1 1:006debaed874 44
lilac0112_1 1:006debaed874 45 void StopCatching(void);//literally
lilac0112_1 1:006debaed874 46 void StopLeaving(void);//literally
lilac0112_1 1:006debaed874 47 void StartCatching(void);//literally
lilac0112_1 1:006debaed874 48 void StartLeaving(void);//literally
lilac0112_1 1:006debaed874 49 void RightCatch(void);//右腕のリミットスイッチ
lilac0112_1 1:006debaed874 50 void LeftCatch(void);//左腕のリミットスイッチ
lilac0112_1 1:006debaed874 51 void CenterCatch(void);//中央のリミットスイッチ
lilac0112_1 9:8543cb951a70 52 void FixToNinety(void);
lilac0112_1 9:8543cb951a70 53 void display_LED(int kind);//start display LED
lilac0112_1 4:1102ab499430 54 uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one);
lilac0112_1 4:1102ab499430 55 bool GetBit(uint8_t n, uint8_t bit);
lilac0112_1 1:006debaed874 56 void SetUp(void);//literally
lilac0112_1 7:c418a0d929b1 57
lilac0112_1 7:c418a0d929b1 58
lilac0112_1 1:006debaed874 59 int main(void) {
lilac0112_1 1:006debaed874 60
lilac0112_1 9:8543cb951a70 61 leg_mbed.start_read();
lilac0112_1 9:8543cb951a70 62 leg_mbed.read_data(data,KEYCODE);
lilac0112_1 9:8543cb951a70 63
lilac0112_1 9:8543cb951a70 64 servo_mbed.start_write();
lilac0112_1 9:8543cb951a70 65 servo_mbed.write_data(INdata,KEYCODE);
lilac0112_1 9:8543cb951a70 66
lilac0112_1 9:8543cb951a70 67 display_LED(1);
lilac0112_1 9:8543cb951a70 68
lilac0112_1 9:8543cb951a70 69 leg_mbed.check_rx_wait();
lilac0112_1 5:c2a278842131 70
lilac0112_1 9:8543cb951a70 71 //wait(0.5);
lilac0112_1 9:8543cb951a70 72
lilac0112_1 9:8543cb951a70 73 //MultiSerial leg_mbed(p13,p14);
lilac0112_1 4:1102ab499430 74
lilac0112_1 1:006debaed874 75 SetUp();
lilac0112_1 1:006debaed874 76
lilac0112_1 1:006debaed874 77 while(1) {
lilac0112_1 1:006debaed874 78
lilac0112_1 9:8543cb951a70 79 //leg_mbed.read_data(data,KEYCODE);
Hatter 3:a1489a94c436 80
lilac0112_1 9:8543cb951a70 81 INdata[0] = data[0];
lilac0112_1 7:c418a0d929b1 82 //INdata[0] = ~mt;
lilac0112_1 7:c418a0d929b1 83
lilac0112_1 9:8543cb951a70 84 //if(!TurnSW[N]) IO[0]=ON;
lilac0112_1 9:8543cb951a70 85 //if(!TurnSW[E]) IO[1]=ON;
lilac0112_1 9:8543cb951a70 86 //servo_mbed[0] = IO[0]&&IO[1];
lilac0112_1 9:8543cb951a70 87 //servo_mbed[1] = IO[0]&&IO[1];
lilac0112_1 9:8543cb951a70 88 //servo_mbed.putc(IO[0]&&IO[1]);
lilac0112_1 7:c418a0d929b1 89
lilac0112_1 9:8543cb951a70 90 LED = INdata[0];
lilac0112_1 9:8543cb951a70 91
lilac0112_1 9:8543cb951a70 92 //腕
lilac0112_1 7:c418a0d929b1 93
lilac0112_1 7:c418a0d929b1 94 if((GetBit(INdata[0], 1)==OFF)&&(Interrupt==OFF)) StopCatching();
lilac0112_1 1:006debaed874 95
lilac0112_1 4:1102ab499430 96 if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
lilac0112_1 1:006debaed874 97
lilac0112_1 9:8543cb951a70 98 if(GetBit(INdata[0], 1)&&GetBit(INdata[0], 2)) StopCatching(), StopLeaving(), Touch.detach(), Interrupt=OFF;
lilac0112_1 9:8543cb951a70 99
lilac0112_1 4:1102ab499430 100 if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
lilac0112_1 7:c418a0d929b1 101 if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
lilac0112_1 4:1102ab499430 102 if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
lilac0112_1 1:006debaed874 103 StopCatching();//停止
lilac0112_1 1:006debaed874 104 Touch.detach();//ワンショットタイマー停止
lilac0112_1 7:c418a0d929b1 105 Interrupt=OFF;
lilac0112_1 4:1102ab499430 106 Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
lilac0112_1 4:1102ab499430 107 Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
lilac0112_1 1:006debaed874 108 }
lilac0112_1 1:006debaed874 109 }
lilac0112_1 1:006debaed874 110
lilac0112_1 1:006debaed874 111 if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
lilac0112_1 1:006debaed874 112 StopCatching();//停止
lilac0112_1 1:006debaed874 113 Touch.detach();//ワンショットタイマー停止
lilac0112_1 7:c418a0d929b1 114 Interrupt=OFF;
lilac0112_1 1:006debaed874 115 }
lilac0112_1 1:006debaed874 116
lilac0112_1 2:eb1c123c9140 117 if(!OneEighty[R]) Hand[R][Open]=OFF;
lilac0112_1 2:eb1c123c9140 118 if(!OneEighty[L]) Hand[L][Open]=OFF;
lilac0112_1 7:c418a0d929b1 119
lilac0112_1 7:c418a0d929b1 120 if(!HandSW[R]) Hand[R][Close]=OFF;
lilac0112_1 7:c418a0d929b1 121 if(!HandSW[L]) Hand[L][Close]=OFF;
lilac0112_1 2:eb1c123c9140 122
lilac0112_1 7:c418a0d929b1 123 if(HandSW[R].count() > 0){
lilac0112_1 7:c418a0d929b1 124 RightCatch();
lilac0112_1 7:c418a0d929b1 125 }
lilac0112_1 7:c418a0d929b1 126 if(HandSW[L].count() > 0){
lilac0112_1 7:c418a0d929b1 127 LeftCatch();
lilac0112_1 7:c418a0d929b1 128 }
lilac0112_1 7:c418a0d929b1 129 if(HandSW[C].count() > 0){
lilac0112_1 9:8543cb951a70 130 if(GetBit(INdata[0], 5)) CenterCatch();
lilac0112_1 7:c418a0d929b1 131 }
lilac0112_1 2:eb1c123c9140 132
lilac0112_1 4:1102ab499430 133 //LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
lilac0112_1 2:eb1c123c9140 134
lilac0112_1 2:eb1c123c9140 135 //上体回転
lilac0112_1 2:eb1c123c9140 136
lilac0112_1 9:8543cb951a70 137 //if(FIX&&(!((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1))){
lilac0112_1 9:8543cb951a70 138 // FixToNinety();
lilac0112_1 9:8543cb951a70 139 // continue;
lilac0112_1 9:8543cb951a70 140 //}
lilac0112_1 9:8543cb951a70 141
lilac0112_1 8:4f9c4b791bfe 142 if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
lilac0112_1 8:4f9c4b791bfe 143 if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
lilac0112_1 7:c418a0d929b1 144
lilac0112_1 9:8543cb951a70 145 if(GetBit(INdata[0], 3)&&GetBit(INdata[0], 4)) Turn[CW]=Turn[CCW]=OFF;
lilac0112_1 9:8543cb951a70 146
lilac0112_1 8:4f9c4b791bfe 147 if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止
lilac0112_1 4:1102ab499430 148
lilac0112_1 4:1102ab499430 149 Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
lilac0112_1 4:1102ab499430 150 Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
lilac0112_1 2:eb1c123c9140 151
lilac0112_1 2:eb1c123c9140 152 }
lilac0112_1 5:c2a278842131 153
lilac0112_1 9:8543cb951a70 154 if(!TurnSW[N]) Turn[CW]=OFF, FIX=ON;//リミットスイッチによる停止
lilac0112_1 9:8543cb951a70 155 if(!TurnSW[E]) Turn[CCW]=OFF, FIX=ON;
lilac0112_1 4:1102ab499430 156
lilac0112_1 4:1102ab499430 157
lilac0112_1 1:006debaed874 158 }
lilac0112_1 1:006debaed874 159 }
lilac0112_1 0:4baeb4381394 160 void StopCatching(void){//停止
lilac0112_1 0:4baeb4381394 161 Hand[R][Close]=Hand[L][Close]=OFF;
lilac0112_1 7:c418a0d929b1 162 if(Interrupt==ON) Interrupt=OFF;
lilac0112_1 0:4baeb4381394 163 }
lilac0112_1 0:4baeb4381394 164 void StopLeaving(void){//停止
lilac0112_1 0:4baeb4381394 165 Hand[R][Open]=Hand[L][Open]=OFF;
lilac0112_1 0:4baeb4381394 166 }
lilac0112_1 2:eb1c123c9140 167 void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
lilac0112_1 7:c418a0d929b1 168
lilac0112_1 9:8543cb951a70 169 HandSpeed[R] = HandSpeed[L] = ARM_REV;
lilac0112_1 9:8543cb951a70 170
lilac0112_1 7:c418a0d929b1 171 if(GetBit(INdata[0], 1)==true){
lilac0112_1 7:c418a0d929b1 172 Hand[R][Close]=HandSW[R];
lilac0112_1 7:c418a0d929b1 173 Hand[L][Close]=HandSW[L];
lilac0112_1 7:c418a0d929b1 174 }
lilac0112_1 7:c418a0d929b1 175
lilac0112_1 7:c418a0d929b1 176 if((!HandSW[R])&&(!HandSW[L])){
lilac0112_1 2:eb1c123c9140 177 Hand[R][Close] = Hand[L][Close] = OFF;
lilac0112_1 2:eb1c123c9140 178 }
lilac0112_1 2:eb1c123c9140 179 else{
lilac0112_1 2:eb1c123c9140 180
lilac0112_1 2:eb1c123c9140 181 Hand[R][Close]=ON;
lilac0112_1 2:eb1c123c9140 182 Hand[L][Close]=ON;
lilac0112_1 2:eb1c123c9140 183
lilac0112_1 2:eb1c123c9140 184 }
lilac0112_1 2:eb1c123c9140 185
lilac0112_1 0:4baeb4381394 186 }
lilac0112_1 2:eb1c123c9140 187 void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
lilac0112_1 9:8543cb951a70 188
lilac0112_1 9:8543cb951a70 189 HandSpeed[R] = HandSpeed[L] = ARM_LIMIT_REV;
lilac0112_1 9:8543cb951a70 190
lilac0112_1 2:eb1c123c9140 191 Hand[R][Open]=OneEighty[R];
lilac0112_1 2:eb1c123c9140 192 Hand[L][Open]=OneEighty[L];
lilac0112_1 0:4baeb4381394 193 }
lilac0112_1 0:4baeb4381394 194 void RightCatch(void){//右腕のリミットスイッチ
lilac0112_1 7:c418a0d929b1 195
lilac0112_1 7:c418a0d929b1 196 Hand[R][Close] = OFF;
lilac0112_1 7:c418a0d929b1 197
lilac0112_1 0:4baeb4381394 198 if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
lilac0112_1 0:4baeb4381394 199 StopCatching();//停止
lilac0112_1 0:4baeb4381394 200 Touch.detach();//ワンショットタイマー停止
lilac0112_1 7:c418a0d929b1 201 Interrupt=OFF;
lilac0112_1 0:4baeb4381394 202 }
lilac0112_1 0:4baeb4381394 203 }
lilac0112_1 0:4baeb4381394 204 void LeftCatch(void){//左腕のリミットスイッチ
lilac0112_1 7:c418a0d929b1 205
lilac0112_1 7:c418a0d929b1 206 Hand[L][Close] = OFF;
lilac0112_1 7:c418a0d929b1 207
lilac0112_1 0:4baeb4381394 208 if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
lilac0112_1 0:4baeb4381394 209 StopCatching();//停止
lilac0112_1 0:4baeb4381394 210 Touch.detach();//ワンショットタイマー停止
lilac0112_1 7:c418a0d929b1 211 Interrupt=OFF;
lilac0112_1 0:4baeb4381394 212 }
lilac0112_1 0:4baeb4381394 213 }
lilac0112_1 0:4baeb4381394 214 void CenterCatch(void){//中央のリミットスイッチ
lilac0112_1 7:c418a0d929b1 215
lilac0112_1 2:eb1c123c9140 216 if((!OneEighty[R])&&(!OneEighty[L])){
lilac0112_1 9:8543cb951a70 217 if(GetBit(INdata[0], 1)==false){
lilac0112_1 9:8543cb951a70 218 Interrupt=ON;
lilac0112_1 9:8543cb951a70 219 Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
lilac0112_1 9:8543cb951a70 220 StartCatching();
lilac0112_1 9:8543cb951a70 221 }
lilac0112_1 2:eb1c123c9140 222 }
lilac0112_1 7:c418a0d929b1 223 if(GetBit(INdata[0], 1)==true){
lilac0112_1 7:c418a0d929b1 224 StartCatching();
lilac0112_1 7:c418a0d929b1 225 }
lilac0112_1 0:4baeb4381394 226 }
lilac0112_1 9:8543cb951a70 227 void FixToNinety(void){
lilac0112_1 9:8543cb951a70 228
lilac0112_1 9:8543cb951a70 229 if((FIX)&&(TurnSpeed != FIX_REV)) TurnSpeed = FIX_REV;
lilac0112_1 9:8543cb951a70 230
lilac0112_1 9:8543cb951a70 231 if((!TurnSW[N])&&(TurnSW[E])) Turn[CCW]=ON;
lilac0112_1 9:8543cb951a70 232
lilac0112_1 9:8543cb951a70 233 if((TurnSW[N])&&(!TurnSW[E])) Turn[CW]=ON;
lilac0112_1 9:8543cb951a70 234
lilac0112_1 9:8543cb951a70 235 if((TurnSW[N])&&(TurnSW[E])) Turn[CW]=Turn[CCW]=OFF, FIX=OFF;
lilac0112_1 9:8543cb951a70 236
lilac0112_1 9:8543cb951a70 237 if((!FIX)&&(TurnSpeed != TURN_REV)) TurnSpeed = TURN_REV;
lilac0112_1 9:8543cb951a70 238
lilac0112_1 9:8543cb951a70 239 }
lilac0112_1 9:8543cb951a70 240 void display_LED(int kind){/*start display LED*/
lilac0112_1 9:8543cb951a70 241
lilac0112_1 9:8543cb951a70 242 switch(kind) {
lilac0112_1 9:8543cb951a70 243
lilac0112_1 9:8543cb951a70 244 case 0: //err check
lilac0112_1 9:8543cb951a70 245 LED = 0xA;
lilac0112_1 9:8543cb951a70 246 wait(0.5);
lilac0112_1 9:8543cb951a70 247 LED = 0x5;
lilac0112_1 9:8543cb951a70 248 wait(0.5);
lilac0112_1 9:8543cb951a70 249
lilac0112_1 9:8543cb951a70 250 break;
lilac0112_1 9:8543cb951a70 251
lilac0112_1 9:8543cb951a70 252 case 1:
lilac0112_1 9:8543cb951a70 253 //
lilac0112_1 9:8543cb951a70 254 LED = 0xF;
lilac0112_1 9:8543cb951a70 255 wait(0.5);
lilac0112_1 9:8543cb951a70 256 LED = 0;
lilac0112_1 9:8543cb951a70 257 wait(0.5);
lilac0112_1 9:8543cb951a70 258 break;
lilac0112_1 9:8543cb951a70 259 }
lilac0112_1 9:8543cb951a70 260 }
lilac0112_1 4:1102ab499430 261 uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){
lilac0112_1 4:1102ab499430 262 return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one;
lilac0112_1 4:1102ab499430 263 }
lilac0112_1 4:1102ab499430 264 bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
lilac0112_1 4:1102ab499430 265 return (n>>(bit-1))%2;
lilac0112_1 4:1102ab499430 266 }
lilac0112_1 1:006debaed874 267 void SetUp(void){//literally
lilac0112_1 1:006debaed874 268
lilac0112_1 5:c2a278842131 269 mt.mode(PullUp);
lilac0112_1 1:006debaed874 270
lilac0112_1 1:006debaed874 271 HandSpeed[R] = HandSpeed[L] = ARM_REV;
lilac0112_1 8:4f9c4b791bfe 272 TurnSpeed = TURN_REV;
lilac0112_1 1:006debaed874 273
lilac0112_1 7:c418a0d929b1 274 //HandSW[R].mode(PullUp);
lilac0112_1 7:c418a0d929b1 275 //HandSW[L].mode(PullUp);
lilac0112_1 7:c418a0d929b1 276 //HandSW[C].mode(PullUp);
lilac0112_1 7:c418a0d929b1 277 //HandSW[R].fall(RightCatch);
lilac0112_1 7:c418a0d929b1 278 //HandSW[L].fall(LeftCatch);
lilac0112_1 7:c418a0d929b1 279 //HandSW[C].fall(CenterCatch);
lilac0112_1 2:eb1c123c9140 280
lilac0112_1 2:eb1c123c9140 281 OneEighty[R].mode(PullUp);
lilac0112_1 2:eb1c123c9140 282 OneEighty[L].mode(PullUp);
lilac0112_1 2:eb1c123c9140 283
lilac0112_1 2:eb1c123c9140 284 TurnSW[N].mode(PullUp);
lilac0112_1 2:eb1c123c9140 285 TurnSW[E].mode(PullUp);
lilac0112_1 2:eb1c123c9140 286
lilac0112_1 7:c418a0d929b1 287 }
lilac0112_1 7:c418a0d929b1 288