Bチーム / Mbed 2 deprecated Catchtest

腕のプログラムです.

Dependencies:   mbed TextLCD MultiSerial Pswitch

main.cpp@11:1ee158b83f3f, 2014-10-16 (annotated)

Committer:
lilac0112_1
Date:
Thu Oct 16 23:25:51 2014 +0000
Revision:
11:1ee158b83f3f
Parent:
10:557c74f2bdee 
??????

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 11:1ee158b83f3f 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 11:1ee158b83f3f 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 11:1ee158b83f3f 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 11:1ee158b83f3f 29 //MultiSerial servo_mbed(p9, p10);
lilac0112_1 9:8543cb951a70 30 MultiSerial leg_mbed(p13,p14);
lilac0112_1 11:1ee158b83f3f 31
lilac0112_1 5:c2a278842131 32 DigitalIn OneEighty[2]={p18, p19};//R, L
lilac0112_1 9:8543cb951a70 33 Pswitch HandSW[3]={p15, p16, p17};//R, L, C
lilac0112_1 2:eb1c123c9140 34 DigitalIn TurnSW[2]={p11, p12};//
lilac0112_1 2:eb1c123c9140 35 PwmOut HandSpeed[2]={p23, p24};
lilac0112_1 9:8543cb951a70 36 PwmOut TurnSpeed(p22);
lilac0112_1 9:8543cb951a70 37 DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};
lilac0112_1 0:4baeb4381394 38 //ex)Hand[R][Close]=ON;
lilac0112_1 9:8543cb951a70 39 DigitalOut Turn[2]={p29, p30};
lilac0112_1 2:eb1c123c9140 40 //ex)Turn[CW]=ON;
lilac0112_1 1:006debaed874 41 Serial pc(USBTX, USBRX);
lilac0112_1 0:4baeb4381394 42 Timeout Touch;//腕で掴むときの安全対策
lilac0112_1 11:1ee158b83f3f 43
lilac0112_1 1:006debaed874 44 void StopCatching(void);//literally
lilac0112_1 1:006debaed874 45 void StopLeaving(void);//literally
lilac0112_1 1:006debaed874 46 void StartCatching(void);//literally
lilac0112_1 1:006debaed874 47 void StartLeaving(void);//literally
lilac0112_1 1:006debaed874 48 void RightCatch(void);//右腕のリミットスイッチ
lilac0112_1 1:006debaed874 49 void LeftCatch(void);//左腕のリミットスイッチ
lilac0112_1 1:006debaed874 50 void CenterCatch(void);//中央のリミットスイッチ
lilac0112_1 9:8543cb951a70 51 void display_LED(int kind);//start display LED
lilac0112_1 4:1102ab499430 52 uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one);
lilac0112_1 4:1102ab499430 53 bool GetBit(uint8_t n, uint8_t bit);
lilac0112_1 1:006debaed874 54 void SetUp(void);//literally
lilac0112_1 7:c418a0d929b1 55
lilac0112_1 7:c418a0d929b1 56
lilac0112_1 1:006debaed874 57 int main(void) {
lilac0112_1 1:006debaed874 58
lilac0112_1 11:1ee158b83f3f 59 SetUp();
lilac0112_1 11:1ee158b83f3f 60
lilac0112_1 11:1ee158b83f3f 61 leg_mbed.start_read();
lilac0112_1 11:1ee158b83f3f 62 leg_mbed.read_data(data,KEYCODE);
lilac0112_1 11:1ee158b83f3f 63
lilac0112_1 11:1ee158b83f3f 64 //servo_mbed.start_write();
lilac0112_1 11:1ee158b83f3f 65 //servo_mbed.write_data(INdata,KEYCODE);
lilac0112_1 11:1ee158b83f3f 66
lilac0112_1 11:1ee158b83f3f 67 display_LED(1);
lilac0112_1 9:8543cb951a70 68
lilac0112_1 11:1ee158b83f3f 69 leg_mbed.check_rx_wait();
lilac0112_1 1:006debaed874 70
lilac0112_1 1:006debaed874 71 while(1) {
lilac0112_1 1:006debaed874 72
lilac0112_1 11:1ee158b83f3f 73 INdata[0] = data[0];
lilac0112_1 11:1ee158b83f3f 74 //INdata[0] = ~mt;
lilac0112_1 7:c418a0d929b1 75
lilac0112_1 9:8543cb951a70 76 LED = INdata[0];
lilac0112_1 9:8543cb951a70 77
lilac0112_1 9:8543cb951a70 78 //腕
lilac0112_1 7:c418a0d929b1 79
lilac0112_1 7:c418a0d929b1 80 if((GetBit(INdata[0], 1)==OFF)&&(Interrupt==OFF)) StopCatching();
lilac0112_1 1:006debaed874 81
lilac0112_1 4:1102ab499430 82 if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
lilac0112_1 1:006debaed874 83
lilac0112_1 11:1ee158b83f3f 84 //if(GetBit(INdata[0], 1)&&GetBit(INdata[0], 2)) StopCatching(), StopLeaving(), Touch.detach(), Interrupt=OFF;
lilac0112_1 9:8543cb951a70 85
lilac0112_1 4:1102ab499430 86 if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
lilac0112_1 7:c418a0d929b1 87 if(GetBit(INdata[0], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
lilac0112_1 4:1102ab499430 88 if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
lilac0112_1 1:006debaed874 89 StopCatching();//停止
lilac0112_1 1:006debaed874 90 Touch.detach();//ワンショットタイマー停止
lilac0112_1 7:c418a0d929b1 91 Interrupt=OFF;
lilac0112_1 4:1102ab499430 92 Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
lilac0112_1 4:1102ab499430 93 Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
lilac0112_1 1:006debaed874 94 }
lilac0112_1 1:006debaed874 95 }
lilac0112_1 1:006debaed874 96
lilac0112_1 1:006debaed874 97 if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
lilac0112_1 1:006debaed874 98 StopCatching();//停止
lilac0112_1 1:006debaed874 99 Touch.detach();//ワンショットタイマー停止
lilac0112_1 7:c418a0d929b1 100 Interrupt=OFF;
lilac0112_1 1:006debaed874 101 }
lilac0112_1 1:006debaed874 102
lilac0112_1 2:eb1c123c9140 103 if(!OneEighty[R]) Hand[R][Open]=OFF;
lilac0112_1 2:eb1c123c9140 104 if(!OneEighty[L]) Hand[L][Open]=OFF;
lilac0112_1 7:c418a0d929b1 105
lilac0112_1 7:c418a0d929b1 106 if(!HandSW[R]) Hand[R][Close]=OFF;
lilac0112_1 7:c418a0d929b1 107 if(!HandSW[L]) Hand[L][Close]=OFF;
lilac0112_1 2:eb1c123c9140 108
lilac0112_1 7:c418a0d929b1 109 if(HandSW[R].count() > 0){
lilac0112_1 7:c418a0d929b1 110 RightCatch();
lilac0112_1 7:c418a0d929b1 111 }
lilac0112_1 7:c418a0d929b1 112 if(HandSW[L].count() > 0){
lilac0112_1 7:c418a0d929b1 113 LeftCatch();
lilac0112_1 7:c418a0d929b1 114 }
lilac0112_1 7:c418a0d929b1 115 if(HandSW[C].count() > 0){
lilac0112_1 11:1ee158b83f3f 116 CenterCatch();
lilac0112_1 7:c418a0d929b1 117 }
lilac0112_1 2:eb1c123c9140 118
lilac0112_1 2:eb1c123c9140 119 //上体回転
lilac0112_1 2:eb1c123c9140 120
lilac0112_1 8:4f9c4b791bfe 121 if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
lilac0112_1 8:4f9c4b791bfe 122 if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
lilac0112_1 7:c418a0d929b1 123
lilac0112_1 11:1ee158b83f3f 124 //if(GetBit(INdata[0], 3)&&GetBit(INdata[0], 4)) Turn[CW]=Turn[CCW]=OFF;
lilac0112_1 9:8543cb951a70 125
lilac0112_1 8:4f9c4b791bfe 126 if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4))==1){//同時押し防止
lilac0112_1 4:1102ab499430 127
lilac0112_1 4:1102ab499430 128 Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
lilac0112_1 4:1102ab499430 129 Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
lilac0112_1 2:eb1c123c9140 130
lilac0112_1 2:eb1c123c9140 131 }
lilac0112_1 5:c2a278842131 132
lilac0112_1 9:8543cb951a70 133 if(!TurnSW[N]) Turn[CW]=OFF, FIX=ON;//リミットスイッチによる停止
lilac0112_1 9:8543cb951a70 134 if(!TurnSW[E]) Turn[CCW]=OFF, FIX=ON;
lilac0112_1 4:1102ab499430 135
lilac0112_1 4:1102ab499430 136
lilac0112_1 1:006debaed874 137 }
lilac0112_1 1:006debaed874 138 }
lilac0112_1 0:4baeb4381394 139 void StopCatching(void){//停止
lilac0112_1 0:4baeb4381394 140 Hand[R][Close]=Hand[L][Close]=OFF;
lilac0112_1 7:c418a0d929b1 141 if(Interrupt==ON) Interrupt=OFF;
lilac0112_1 0:4baeb4381394 142 }
lilac0112_1 0:4baeb4381394 143 void StopLeaving(void){//停止
lilac0112_1 0:4baeb4381394 144 Hand[R][Open]=Hand[L][Open]=OFF;
lilac0112_1 0:4baeb4381394 145 }
lilac0112_1 2:eb1c123c9140 146 void StartCatching(void){//掴む（リミットスイッチが押されてなければ）
lilac0112_1 7:c418a0d929b1 147
lilac0112_1 9:8543cb951a70 148 HandSpeed[R] = HandSpeed[L] = ARM_REV;
lilac0112_1 9:8543cb951a70 149
lilac0112_1 7:c418a0d929b1 150 if(GetBit(INdata[0], 1)==true){
lilac0112_1 7:c418a0d929b1 151 Hand[R][Close]=HandSW[R];
lilac0112_1 7:c418a0d929b1 152 Hand[L][Close]=HandSW[L];
lilac0112_1 7:c418a0d929b1 153 }
lilac0112_1 7:c418a0d929b1 154
lilac0112_1 7:c418a0d929b1 155 if((!HandSW[R])&&(!HandSW[L])){
lilac0112_1 2:eb1c123c9140 156 Hand[R][Close] = Hand[L][Close] = OFF;
lilac0112_1 2:eb1c123c9140 157 }
lilac0112_1 2:eb1c123c9140 158 else{
lilac0112_1 2:eb1c123c9140 159
lilac0112_1 2:eb1c123c9140 160 Hand[R][Close]=ON;
lilac0112_1 2:eb1c123c9140 161 Hand[L][Close]=ON;
lilac0112_1 2:eb1c123c9140 162
lilac0112_1 2:eb1c123c9140 163 }
lilac0112_1 2:eb1c123c9140 164
lilac0112_1 0:4baeb4381394 165 }
lilac0112_1 2:eb1c123c9140 166 void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
lilac0112_1 9:8543cb951a70 167
lilac0112_1 9:8543cb951a70 168 HandSpeed[R] = HandSpeed[L] = ARM_LIMIT_REV;
lilac0112_1 9:8543cb951a70 169
lilac0112_1 2:eb1c123c9140 170 Hand[R][Open]=OneEighty[R];
lilac0112_1 2:eb1c123c9140 171 Hand[L][Open]=OneEighty[L];
lilac0112_1 0:4baeb4381394 172 }
lilac0112_1 0:4baeb4381394 173 void RightCatch(void){//右腕のリミットスイッチ
lilac0112_1 7:c418a0d929b1 174
lilac0112_1 7:c418a0d929b1 175 Hand[R][Close] = OFF;
lilac0112_1 7:c418a0d929b1 176
lilac0112_1 0:4baeb4381394 177 if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
lilac0112_1 0:4baeb4381394 178 StopCatching();//停止
lilac0112_1 0:4baeb4381394 179 Touch.detach();//ワンショットタイマー停止
lilac0112_1 7:c418a0d929b1 180 Interrupt=OFF;
lilac0112_1 0:4baeb4381394 181 }
lilac0112_1 0:4baeb4381394 182 }
lilac0112_1 0:4baeb4381394 183 void LeftCatch(void){//左腕のリミットスイッチ
lilac0112_1 7:c418a0d929b1 184
lilac0112_1 7:c418a0d929b1 185 Hand[L][Close] = OFF;
lilac0112_1 7:c418a0d929b1 186
lilac0112_1 0:4baeb4381394 187 if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
lilac0112_1 0:4baeb4381394 188 StopCatching();//停止
lilac0112_1 0:4baeb4381394 189 Touch.detach();//ワンショットタイマー停止
lilac0112_1 7:c418a0d929b1 190 Interrupt=OFF;
lilac0112_1 0:4baeb4381394 191 }
lilac0112_1 0:4baeb4381394 192 }
lilac0112_1 0:4baeb4381394 193 void CenterCatch(void){//中央のリミットスイッチ
lilac0112_1 7:c418a0d929b1 194
lilac0112_1 2:eb1c123c9140 195 if((!OneEighty[R])&&(!OneEighty[L])){
lilac0112_1 9:8543cb951a70 196 if(GetBit(INdata[0], 1)==false){
lilac0112_1 9:8543cb951a70 197 Interrupt=ON;
lilac0112_1 9:8543cb951a70 198 Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
lilac0112_1 9:8543cb951a70 199 StartCatching();
lilac0112_1 9:8543cb951a70 200 }
lilac0112_1 2:eb1c123c9140 201 }
lilac0112_1 7:c418a0d929b1 202 if(GetBit(INdata[0], 1)==true){
lilac0112_1 7:c418a0d929b1 203 StartCatching();
lilac0112_1 7:c418a0d929b1 204 }
lilac0112_1 0:4baeb4381394 205 }
lilac0112_1 11:1ee158b83f3f 206
lilac0112_1 9:8543cb951a70 207 void display_LED(int kind){/*start display LED*/
lilac0112_1 11:1ee158b83f3f 208
lilac0112_1 9:8543cb951a70 209 switch(kind) {
lilac0112_1 11:1ee158b83f3f 210
lilac0112_1 9:8543cb951a70 211 case 0: //err check
lilac0112_1 9:8543cb951a70 212 LED = 0xA;
lilac0112_1 9:8543cb951a70 213 wait(0.5);
lilac0112_1 9:8543cb951a70 214 LED = 0x5;
lilac0112_1 9:8543cb951a70 215 wait(0.5);
lilac0112_1 11:1ee158b83f3f 216
lilac0112_1 9:8543cb951a70 217 break;
lilac0112_1 11:1ee158b83f3f 218
lilac0112_1 9:8543cb951a70 219 case 1:
lilac0112_1 9:8543cb951a70 220 //
lilac0112_1 9:8543cb951a70 221 LED = 0xF;
lilac0112_1 9:8543cb951a70 222 wait(0.5);
lilac0112_1 9:8543cb951a70 223 LED = 0;
lilac0112_1 9:8543cb951a70 224 wait(0.5);
lilac0112_1 9:8543cb951a70 225 break;
lilac0112_1 9:8543cb951a70 226 }
lilac0112_1 9:8543cb951a70 227 }
lilac0112_1 4:1102ab499430 228 uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){
lilac0112_1 4:1102ab499430 229 return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one;
lilac0112_1 4:1102ab499430 230 }
lilac0112_1 4:1102ab499430 231 bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
lilac0112_1 4:1102ab499430 232 return (n>>(bit-1))%2;
lilac0112_1 4:1102ab499430 233 }
lilac0112_1 1:006debaed874 234 void SetUp(void){//literally
lilac0112_1 1:006debaed874 235
lilac0112_1 5:c2a278842131 236 mt.mode(PullUp);
lilac0112_1 1:006debaed874 237
lilac0112_1 1:006debaed874 238 HandSpeed[R] = HandSpeed[L] = ARM_REV;
lilac0112_1 8:4f9c4b791bfe 239 TurnSpeed = TURN_REV;
lilac0112_1 1:006debaed874 240
lilac0112_1 2:eb1c123c9140 241 OneEighty[R].mode(PullUp);
lilac0112_1 2:eb1c123c9140 242 OneEighty[L].mode(PullUp);
lilac0112_1 2:eb1c123c9140 243
lilac0112_1 2:eb1c123c9140 244 TurnSW[N].mode(PullUp);
lilac0112_1 2:eb1c123c9140 245 TurnSW[E].mode(PullUp);
lilac0112_1 2:eb1c123c9140 246
lilac0112_1 11:1ee158b83f3f 247 }