腕のプログラムです.

Dependencies:   mbed TextLCD MultiSerial Pswitch

Committer:
lilac0112_1
Date:
Fri Oct 03 12:30:58 2014 +0000
Revision:
6:606507fc8852
Parent:
5:c2a278842131
Child:
7:c418a0d929b1

        

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