Bチーム / Mbed 2 deprecated Catchtest

腕のプログラムです.

Dependencies:   mbed TextLCD MultiSerial Pswitch

main.cpp@3:a1489a94c436, 2014-09-03 (annotated)

Committer:
Hatter
Date:
Wed Sep 03 07:44:20 2014 +0000
Revision:
3:a1489a94c436
Parent:
2:eb1c123c9140
Child:
4:1102ab499430
????????

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