Tk A / Mbed 2 deprecated f3rc3

fdlsj

Dependencies:   mbed

Fork of f3rc2 by Tk A

prime.h@1:a1e592eca305, 2016-08-30 (annotated)

Committer:
choutin
Date:
Tue Aug 30 06:51:41 2016 +0000
Revision:
1:a1e592eca305
Parent:
0:df2659fd8031
Child:
2:1a2984dfac3e
abosu

Who changed what in which revision?

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