Tk A / Mbed 2 deprecated f3rc3

fdlsj

Dependencies:   mbed

Fork of f3rc2 by Tk A

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

Committer:
sakanakuuun
Date:
Tue Aug 30 07:05:09 2016 +0000
Revision:
2:1a2984dfac3e
Parent:
1:a1e592eca305 
fe;

Who changed what in which revision?

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