Tk A / Mbed 2 deprecated hello_enc_test

ew

Dependencies:   Nucleo_Hello_Encoder mbed

prime.h@1:026846d05bb7, 2016-09-01 (annotated)

Committer:
sakanakuuun
Date:
Thu Sep 01 08:41:49 2016 +0000
Revision:
1:026846d05bb7
Parent:
0:1c8034b7f565 
f; ; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:1c8034b7f565 1
sakanakuuun 0:1c8034b7f565 2 /*
sakanakuuun 0:1c8034b7f565 3 primeではPINを制御する関数を扱う。
sakanakuuun 0:1c8034b7f565 4
sakanakuuun 0:1c8034b7f565 5 以下一覧
sakanakuuun 0:1c8034b7f565 6 open,close hand
sakanakuuun 0:1c8034b7f565 7 open close arm
sakanakuuun 0:1c8034b7f565 8
sakanakuuun 0:1c8034b7f565 9 step
sakanakuuun 0:1c8034b7f565 10 move
sakanakuuun 0:1c8034b7f565 11 blueorred
sakanakuuun 0:1c8034b7f565 12 lrsensor
sakanakuuun 0:1c8034b7f565 13
sakanakuuun 0:1c8034b7f565 14 全関数共通で意図しない動作が起きたら基板上のLEDを点灯
sakanakuuun 0:1c8034b7f565 15 */
sakanakuuun 0:1c8034b7f565 16 PwmOut pwmarm(PC_6);
sakanakuuun 0:1c8034b7f565 17 PwmOut pwmhand(PC_8);
sakanakuuun 0:1c8034b7f565 18
sakanakuuun 0:1c8034b7f565 19 AnalogIn rightsensor(A0);
sakanakuuun 0:1c8034b7f565 20 AnalogIn leftsensor(A1);
sakanakuuun 0:1c8034b7f565 21
sakanakuuun 0:1c8034b7f565 22 AnalogIn armadj(A2);
sakanakuuun 0:1c8034b7f565 23 AnalogIn handadj(A3);
sakanakuuun 0:1c8034b7f565 24
sakanakuuun 0:1c8034b7f565 25 DigitalIn teamSW(PC_11);
sakanakuuun 0:1c8034b7f565 26 DigitalOut teamledblue(PC_10);
sakanakuuun 0:1c8034b7f565 27 DigitalOut teamledred(PC_12);
sakanakuuun 0:1c8034b7f565 28
sakanakuuun 0:1c8034b7f565 29 DigitalOut errorled(LED1);
sakanakuuun 0:1c8034b7f565 30
sakanakuuun 0:1c8034b7f565 31
sakanakuuun 0:1c8034b7f565 32 DigitalIn phase1(PB_8);
sakanakuuun 0:1c8034b7f565 33 DigitalIn phase2(PC_9);
sakanakuuun 0:1c8034b7f565 34 DigitalIn phase4(PB_9);
sakanakuuun 0:1c8034b7f565 35 DigitalIn phase8(PD_2);
sakanakuuun 0:1c8034b7f565 36
sakanakuuun 0:1c8034b7f565 37 PwmOut M1cw(PA_11);
sakanakuuun 0:1c8034b7f565 38 PwmOut M1ccw(PB_15);
sakanakuuun 0:1c8034b7f565 39 PwmOut M2ccw(PB_14);
sakanakuuun 0:1c8034b7f565 40 PwmOut M2cw(PB_13);
sakanakuuun 0:1c8034b7f565 41
sakanakuuun 0:1c8034b7f565 42 DigitalOut PINW(PA_3);
sakanakuuun 0:1c8034b7f565 43 DigitalOut PINX(PA_2);
sakanakuuun 0:1c8034b7f565 44 DigitalOut PINY(PA_10);
sakanakuuun 0:1c8034b7f565 45 DigitalOut PINZ(PB_3);
sakanakuuun 0:1c8034b7f565 46
sakanakuuun 0:1c8034b7f565 47
sakanakuuun 0:1c8034b7f565 48
sakanakuuun 0:1c8034b7f565 49
sakanakuuun 0:1c8034b7f565 50 DigitalOut encordervcc1(PA_6);
sakanakuuun 0:1c8034b7f565 51 DigitalOut encordervcc2(PA_14);
sakanakuuun 0:1c8034b7f565 52
sakanakuuun 0:1c8034b7f565 53 const int PERIOD=20000;
sakanakuuun 0:1c8034b7f565 54
sakanakuuun 0:1c8034b7f565 55
sakanakuuun 0:1c8034b7f565 56 void initencorder(void){
sakanakuuun 0:1c8034b7f565 57 encordervcc1=1;
sakanakuuun 0:1c8034b7f565 58 encordervcc2=1;
sakanakuuun 0:1c8034b7f565 59 }
sakanakuuun 0:1c8034b7f565 60
sakanakuuun 0:1c8034b7f565 61
sakanakuuun 0:1c8034b7f565 62
sakanakuuun 0:1c8034b7f565 63 void close_hand(void) {
sakanakuuun 0:1c8034b7f565 64 int i,degree;
sakanakuuun 0:1c8034b7f565 65
sakanakuuun 0:1c8034b7f565 66 pwmhand.period_ms(20); //20ms
sakanakuuun 0:1c8034b7f565 67
sakanakuuun 0:1c8034b7f565 68 degree=175;
sakanakuuun 0:1c8034b7f565 69
sakanakuuun 0:1c8034b7f565 70 i=500+degree*1900/180;
sakanakuuun 0:1c8034b7f565 71 pwmhand.write(i/PERIOD);
sakanakuuun 0:1c8034b7f565 72
sakanakuuun 0:1c8034b7f565 73
sakanakuuun 0:1c8034b7f565 74 }
sakanakuuun 0:1c8034b7f565 75
sakanakuuun 0:1c8034b7f565 76 void close_arm(void) {
sakanakuuun 0:1c8034b7f565 77 PwmOut mypwm(PB_3);
sakanakuuun 0:1c8034b7f565 78 int i,degree;
sakanakuuun 0:1c8034b7f565 79
sakanakuuun 0:1c8034b7f565 80 mypwm.period_ms(20); //20ms
sakanakuuun 0:1c8034b7f565 81
sakanakuuun 0:1c8034b7f565 82 degree=160;
sakanakuuun 0:1c8034b7f565 83
sakanakuuun 0:1c8034b7f565 84 i=500+degree*1900/180;
sakanakuuun 0:1c8034b7f565 85 pwmarm.write(i/PERIOD);
sakanakuuun 0:1c8034b7f565 86
sakanakuuun 0:1c8034b7f565 87
sakanakuuun 0:1c8034b7f565 88 }
sakanakuuun 0:1c8034b7f565 89
sakanakuuun 0:1c8034b7f565 90
sakanakuuun 0:1c8034b7f565 91
sakanakuuun 0:1c8034b7f565 92 void open_hand(void) {
sakanakuuun 0:1c8034b7f565 93 PwmOut mypwm(PWM_OUT);
sakanakuuun 0:1c8034b7f565 94 int i,degree;
sakanakuuun 0:1c8034b7f565 95
sakanakuuun 0:1c8034b7f565 96 pwmhand.period_ms(20); //20ms
sakanakuuun 0:1c8034b7f565 97
sakanakuuun 0:1c8034b7f565 98 degree=90;
sakanakuuun 0:1c8034b7f565 99
sakanakuuun 0:1c8034b7f565 100 i=500+degree*1900/180;
sakanakuuun 0:1c8034b7f565 101 mypwm.write(i/PERIOD);
sakanakuuun 0:1c8034b7f565 102
sakanakuuun 0:1c8034b7f565 103
sakanakuuun 0:1c8034b7f565 104 }
sakanakuuun 0:1c8034b7f565 105
sakanakuuun 0:1c8034b7f565 106
sakanakuuun 0:1c8034b7f565 107
sakanakuuun 0:1c8034b7f565 108 void open_arm(void) {
sakanakuuun 0:1c8034b7f565 109 PwmOut mypwm(PWM_OUT);
sakanakuuun 0:1c8034b7f565 110 int i,degree;
sakanakuuun 0:1c8034b7f565 111
sakanakuuun 0:1c8034b7f565 112 mypwm.period_ms(20); //20ms
sakanakuuun 0:1c8034b7f565 113
sakanakuuun 0:1c8034b7f565 114 degree=10;
sakanakuuun 0:1c8034b7f565 115
sakanakuuun 0:1c8034b7f565 116 i=500+degree*1900/180;
sakanakuuun 0:1c8034b7f565 117 pwmarm.write(i/PERIOD);
sakanakuuun 0:1c8034b7f565 118
sakanakuuun 0:1c8034b7f565 119
sakanakuuun 0:1c8034b7f565 120 }
sakanakuuun 0:1c8034b7f565 121
sakanakuuun 0:1c8034b7f565 122 void step(int degree){
sakanakuuun 0:1c8034b7f565 123
sakanakuuun 0:1c8034b7f565 124
sakanakuuun 0:1c8034b7f565 125
sakanakuuun 0:1c8034b7f565 126 int puls_times=0;
sakanakuuun 0:1c8034b7f565 127
sakanakuuun 0:1c8034b7f565 128 if(degree>0){
sakanakuuun 0:1c8034b7f565 129
sakanakuuun 0:1c8034b7f565 130 puls_times=1+(int)(degree/(3.75));
sakanakuuun 0:1c8034b7f565 131
sakanakuuun 0:1c8034b7f565 132 while(1){
sakanakuuun 0:1c8034b7f565 133 PINW=1;
sakanakuuun 0:1c8034b7f565 134 PINX=1;
sakanakuuun 0:1c8034b7f565 135 PINY=0;
sakanakuuun 0:1c8034b7f565 136 PINZ=0;
sakanakuuun 0:1c8034b7f565 137 wait_ms(20);
sakanakuuun 0:1c8034b7f565 138 puls_times--;
sakanakuuun 0:1c8034b7f565 139
sakanakuuun 0:1c8034b7f565 140 if(puls_times<0){break;}
sakanakuuun 0:1c8034b7f565 141
sakanakuuun 0:1c8034b7f565 142 PINW=0;
sakanakuuun 0:1c8034b7f565 143 PINX=1;
sakanakuuun 0:1c8034b7f565 144 PINY=1;
sakanakuuun 0:1c8034b7f565 145 PINZ=0;
sakanakuuun 0:1c8034b7f565 146 wait_ms(20);
sakanakuuun 0:1c8034b7f565 147 puls_times--;
sakanakuuun 0:1c8034b7f565 148
sakanakuuun 0:1c8034b7f565 149 if(puls_times<0){break;}
sakanakuuun 0:1c8034b7f565 150
sakanakuuun 0:1c8034b7f565 151 PINW=0;
sakanakuuun 0:1c8034b7f565 152 PINX=0;
sakanakuuun 0:1c8034b7f565 153 PINY=1;
sakanakuuun 0:1c8034b7f565 154 PINZ=1;
sakanakuuun 0:1c8034b7f565 155 wait_ms(20);
sakanakuuun 0:1c8034b7f565 156 puls_times--;
sakanakuuun 0:1c8034b7f565 157
sakanakuuun 0:1c8034b7f565 158 if(puls_times<0){break;}
sakanakuuun 0:1c8034b7f565 159
sakanakuuun 0:1c8034b7f565 160 PINW=1;
sakanakuuun 0:1c8034b7f565 161 PINX=1;
sakanakuuun 0:1c8034b7f565 162 PINY=0;
sakanakuuun 0:1c8034b7f565 163 PINZ=0;
sakanakuuun 0:1c8034b7f565 164 wait_ms(20);
sakanakuuun 0:1c8034b7f565 165 puls_times--;
sakanakuuun 0:1c8034b7f565 166
sakanakuuun 0:1c8034b7f565 167 if(puls_times<0){break;}
sakanakuuun 0:1c8034b7f565 168 }
sakanakuuun 0:1c8034b7f565 169
sakanakuuun 0:1c8034b7f565 170 }
sakanakuuun 0:1c8034b7f565 171
sakanakuuun 0:1c8034b7f565 172 if(degree<0){
sakanakuuun 0:1c8034b7f565 173
sakanakuuun 0:1c8034b7f565 174 puls_times=(-1)*(1+(int)(degree/(3.75)));
sakanakuuun 0:1c8034b7f565 175
sakanakuuun 0:1c8034b7f565 176 while(1){
sakanakuuun 0:1c8034b7f565 177 PINW=1;
sakanakuuun 0:1c8034b7f565 178 PINX=1;
sakanakuuun 0:1c8034b7f565 179 PINY=0;
sakanakuuun 0:1c8034b7f565 180 PINZ=0;
sakanakuuun 0:1c8034b7f565 181 wait_ms(20);
sakanakuuun 0:1c8034b7f565 182 puls_times--;
sakanakuuun 0:1c8034b7f565 183
sakanakuuun 0:1c8034b7f565 184 if(puls_times<0){break;}
sakanakuuun 0:1c8034b7f565 185
sakanakuuun 0:1c8034b7f565 186 PINW=1;
sakanakuuun 0:1c8034b7f565 187 PINX=0;
sakanakuuun 0:1c8034b7f565 188 PINY=0;
sakanakuuun 0:1c8034b7f565 189 PINZ=1;
sakanakuuun 0:1c8034b7f565 190 wait_ms(20);
sakanakuuun 0:1c8034b7f565 191 puls_times--;
sakanakuuun 0:1c8034b7f565 192
sakanakuuun 0:1c8034b7f565 193 if(puls_times<0){break;}
sakanakuuun 0:1c8034b7f565 194
sakanakuuun 0:1c8034b7f565 195 PINW=0;
sakanakuuun 0:1c8034b7f565 196 PINX=0;
sakanakuuun 0:1c8034b7f565 197 PINY=1;
sakanakuuun 0:1c8034b7f565 198 PINZ=1;
sakanakuuun 0:1c8034b7f565 199 wait_ms(20);
sakanakuuun 0:1c8034b7f565 200 puls_times--;
sakanakuuun 0:1c8034b7f565 201
sakanakuuun 0:1c8034b7f565 202 if(puls_times<0){break;}
sakanakuuun 0:1c8034b7f565 203
sakanakuuun 0:1c8034b7f565 204 PINW=0;
sakanakuuun 0:1c8034b7f565 205 PINX=1;
sakanakuuun 0:1c8034b7f565 206 PINY=1;
sakanakuuun 0:1c8034b7f565 207 PINZ=0;
sakanakuuun 0:1c8034b7f565 208 wait_ms(20);
sakanakuuun 0:1c8034b7f565 209 puls_times--;
sakanakuuun 0:1c8034b7f565 210
sakanakuuun 0:1c8034b7f565 211 if(puls_times<0){break;}
sakanakuuun 0:1c8034b7f565 212 }
sakanakuuun 0:1c8034b7f565 213
sakanakuuun 0:1c8034b7f565 214 }
sakanakuuun 0:1c8034b7f565 215
sakanakuuun 0:1c8034b7f565 216 }
sakanakuuun 0:1c8034b7f565 217
sakanakuuun 0:1c8034b7f565 218
sakanakuuun 0:1c8034b7f565 219
sakanakuuun 0:1c8034b7f565 220 int sensor(void){
sakanakuuun 0:1c8034b7f565 221 //センサー読
sakanakuuun 0:1c8034b7f565 222 int right ,left,x,y;
sakanakuuun 0:1c8034b7f565 223
sakanakuuun 0:1c8034b7f565 224 right=rightsensor.read();
sakanakuuun 0:1c8034b7f565 225 left=leftsensor.read();
sakanakuuun 0:1c8034b7f565 226
sakanakuuun 0:1c8034b7f565 227 if(right>0.5){x=1;}
sakanakuuun 0:1c8034b7f565 228 else{right=0;}
sakanakuuun 0:1c8034b7f565 229
sakanakuuun 0:1c8034b7f565 230 if(left>0.5){y=1;}
sakanakuuun 0:1c8034b7f565 231 else{left=0;}
sakanakuuun 0:1c8034b7f565 232
sakanakuuun 0:1c8034b7f565 233 return x+2*(y);
sakanakuuun 0:1c8034b7f565 234 //(right,left)=(off,off),(on,off),(off,on),(on,on)
sakanakuuun 0:1c8034b7f565 235 // 0 1 2 3
sakanakuuun 0:1c8034b7f565 236 }
sakanakuuun 0:1c8034b7f565 237
sakanakuuun 0:1c8034b7f565 238
sakanakuuun 0:1c8034b7f565 239
sakanakuuun 0:1c8034b7f565 240 void initmotor(){
sakanakuuun 0:1c8034b7f565 241
sakanakuuun 0:1c8034b7f565 242
sakanakuuun 0:1c8034b7f565 243 M1cw.period_us(256);
sakanakuuun 0:1c8034b7f565 244 M1ccw.period_us(256);
sakanakuuun 0:1c8034b7f565 245 M2cw.period_us(256);
sakanakuuun 0:1c8034b7f565 246 M2ccw.period_us(256);
sakanakuuun 0:1c8034b7f565 247
sakanakuuun 0:1c8034b7f565 248 }
sakanakuuun 0:1c8034b7f565 249
sakanakuuun 0:1c8034b7f565 250 void move(int right,int left){
sakanakuuun 0:1c8034b7f565 251
sakanakuuun 0:1c8034b7f565 252 float rightduty,leftduty;
sakanakuuun 0:1c8034b7f565 253
sakanakuuun 0:1c8034b7f565 254 if(right>256){right=256;}
sakanakuuun 0:1c8034b7f565 255 if(left>256){left=256;}
sakanakuuun 0:1c8034b7f565 256 if(right<-256){right=-256;}
sakanakuuun 0:1c8034b7f565 257 if(left<-256){left=-256;}
sakanakuuun 0:1c8034b7f565 258
sakanakuuun 0:1c8034b7f565 259 rightduty=right/256.0;
sakanakuuun 0:1c8034b7f565 260 leftduty=left/256.0;
sakanakuuun 0:1c8034b7f565 261 if(right>0){
sakanakuuun 0:1c8034b7f565 262 M1cw.write(1-rightduty);
sakanakuuun 0:1c8034b7f565 263 M1ccw.write(1);
sakanakuuun 0:1c8034b7f565 264 }else{
sakanakuuun 0:1c8034b7f565 265 M1cw.write(1);
sakanakuuun 0:1c8034b7f565 266 M1ccw.write(1+rightduty);
sakanakuuun 0:1c8034b7f565 267 }
sakanakuuun 0:1c8034b7f565 268
sakanakuuun 0:1c8034b7f565 269 if(left>0){
sakanakuuun 0:1c8034b7f565 270 M2cw.write(1-leftduty);
sakanakuuun 0:1c8034b7f565 271 M2ccw.write(1);
sakanakuuun 0:1c8034b7f565 272 }else{
sakanakuuun 0:1c8034b7f565 273 M2cw.write(1);
sakanakuuun 0:1c8034b7f565 274 M2ccw.write(1+leftduty);
sakanakuuun 0:1c8034b7f565 275 }
sakanakuuun 0:1c8034b7f565 276 }
sakanakuuun 0:1c8034b7f565 277
sakanakuuun 0:1c8034b7f565 278
sakanakuuun 0:1c8034b7f565 279 int phase(void){
sakanakuuun 0:1c8034b7f565 280
sakanakuuun 0:1c8034b7f565 281 phase1.mode(PullUp);
sakanakuuun 0:1c8034b7f565 282 phase2.mode(PullUp);
sakanakuuun 0:1c8034b7f565 283 phase4.mode(PullUp);
sakanakuuun 0:1c8034b7f565 284 phase8.mode(PullUp);
sakanakuuun 0:1c8034b7f565 285
sakanakuuun 0:1c8034b7f565 286 int r=0;
sakanakuuun 0:1c8034b7f565 287
sakanakuuun 0:1c8034b7f565 288 r=phase1+2*phase2+4*phase4+8*phase8;
sakanakuuun 0:1c8034b7f565 289
sakanakuuun 0:1c8034b7f565 290 return r;
sakanakuuun 0:1c8034b7f565 291 }
sakanakuuun 0:1c8034b7f565 292