涼太郎 中村 / Mbed 2 deprecated f3rc2

めいん

Dependencies:   mbed

prime.h@5:1c95260515c1, 2016-09-03 (annotated)

Committer:
choutin
Date:
Sat Sep 03 06:51:48 2016 +0000
Revision:
5:1c95260515c1
Parent:
2:b204cf2f9b60 
test for you

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 2:b204cf2f9b60 39 PwmOut M2ccw(PB_14);
choutin 2:b204cf2f9b60 40 PwmOut M2cw(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 2:b204cf2f9b60 47
choutin 1:a1e592eca305 48 QEI right (PA_7, PA_5, NC, 100, QEI::X2_ENCODING);
choutin 2:b204cf2f9b60 49 QEI left (PA_13, PA_15, NC, 100, QEI::X2_ENCODING);
choutin 1:a1e592eca305 50
choutin 2:b204cf2f9b60 51 DigitalOut encordervcc1(PA_6);
choutin 2:b204cf2f9b60 52 DigitalOut encordervcc2(PA_14);
choutin 2:b204cf2f9b60 53
choutin 2:b204cf2f9b60 54 const int PERIOD=20000;
choutin 2:b204cf2f9b60 55
choutin 1:a1e592eca305 56
choutin 1:a1e592eca305 57 void initencorder(void){
choutin 1:a1e592eca305 58 encordervcc1=1;
choutin 1:a1e592eca305 59 encordervcc2=1;
choutin 1:a1e592eca305 60 }
choutin 1:a1e592eca305 61
choutin 0:df2659fd8031 62
choutin 0:df2659fd8031 63
choutin 0:df2659fd8031 64 void close_hand(void) {
choutin 2:b204cf2f9b60 65 int i,degree;
choutin 0:df2659fd8031 66
choutin 2:b204cf2f9b60 67 pwmhand.period_ms(20); //20ms
choutin 2:b204cf2f9b60 68
choutin 2:b204cf2f9b60 69 degree=175;
choutin 0:df2659fd8031 70
choutin 2:b204cf2f9b60 71 i=500+degree*1900/180;
choutin 2:b204cf2f9b60 72 pwmhand.write(i/PERIOD);
choutin 2:b204cf2f9b60 73
choutin 2:b204cf2f9b60 74
choutin 0:df2659fd8031 75 }
choutin 0:df2659fd8031 76
choutin 0:df2659fd8031 77 void close_arm(void) {
choutin 2:b204cf2f9b60 78 PwmOut mypwm(PB_3);
choutin 2:b204cf2f9b60 79 int i,degree;
choutin 0:df2659fd8031 80
choutin 2:b204cf2f9b60 81 mypwm.period_ms(20); //20ms
choutin 2:b204cf2f9b60 82
choutin 2:b204cf2f9b60 83 degree=160;
choutin 0:df2659fd8031 84
choutin 2:b204cf2f9b60 85 i=500+degree*1900/180;
choutin 2:b204cf2f9b60 86 pwmarm.write(i/PERIOD);
choutin 2:b204cf2f9b60 87
choutin 2:b204cf2f9b60 88
choutin 0:df2659fd8031 89 }
choutin 0:df2659fd8031 90
choutin 0:df2659fd8031 91
choutin 0:df2659fd8031 92
choutin 0:df2659fd8031 93 void open_hand(void) {
choutin 2:b204cf2f9b60 94 PwmOut mypwm(PWM_OUT);
choutin 2:b204cf2f9b60 95 int i,degree;
choutin 0:df2659fd8031 96
choutin 2:b204cf2f9b60 97 pwmhand.period_ms(20); //20ms
choutin 2:b204cf2f9b60 98
choutin 2:b204cf2f9b60 99 degree=90;
choutin 0:df2659fd8031 100
choutin 2:b204cf2f9b60 101 i=500+degree*1900/180;
choutin 2:b204cf2f9b60 102 mypwm.write(i/PERIOD);
choutin 2:b204cf2f9b60 103
choutin 2:b204cf2f9b60 104
choutin 0:df2659fd8031 105 }
choutin 0:df2659fd8031 106
choutin 0:df2659fd8031 107
choutin 0:df2659fd8031 108
choutin 0:df2659fd8031 109 void open_arm(void) {
choutin 2:b204cf2f9b60 110 PwmOut mypwm(PWM_OUT);
choutin 2:b204cf2f9b60 111 int i,degree;
choutin 0:df2659fd8031 112
choutin 2:b204cf2f9b60 113 mypwm.period_ms(20); //20ms
choutin 2:b204cf2f9b60 114
choutin 2:b204cf2f9b60 115 degree=10;
choutin 0:df2659fd8031 116
choutin 2:b204cf2f9b60 117 i=500+degree*1900/180;
choutin 2:b204cf2f9b60 118 pwmarm.write(i/PERIOD);
choutin 2:b204cf2f9b60 119
choutin 2:b204cf2f9b60 120
choutin 0:df2659fd8031 121 }
choutin 0:df2659fd8031 122
choutin 0:df2659fd8031 123 void step(int degree){
choutin 0:df2659fd8031 124
choutin 0:df2659fd8031 125
choutin 1:a1e592eca305 126
choutin 0:df2659fd8031 127 int puls_times=0;
choutin 0:df2659fd8031 128
choutin 0:df2659fd8031 129 if(degree>0){
choutin 0:df2659fd8031 130
choutin 0:df2659fd8031 131 puls_times=1+(int)(degree/(3.75));
choutin 0:df2659fd8031 132
choutin 0:df2659fd8031 133 while(1){
choutin 0:df2659fd8031 134 PINW=1;
choutin 0:df2659fd8031 135 PINX=1;
choutin 0:df2659fd8031 136 PINY=0;
choutin 0:df2659fd8031 137 PINZ=0;
choutin 0:df2659fd8031 138 wait_ms(20);
choutin 0:df2659fd8031 139 puls_times--;
choutin 0:df2659fd8031 140
choutin 0:df2659fd8031 141 if(puls_times<0){break;}
choutin 0:df2659fd8031 142
choutin 0:df2659fd8031 143 PINW=0;
choutin 0:df2659fd8031 144 PINX=1;
choutin 0:df2659fd8031 145 PINY=1;
choutin 0:df2659fd8031 146 PINZ=0;
choutin 0:df2659fd8031 147 wait_ms(20);
choutin 0:df2659fd8031 148 puls_times--;
choutin 0:df2659fd8031 149
choutin 0:df2659fd8031 150 if(puls_times<0){break;}
choutin 0:df2659fd8031 151
choutin 0:df2659fd8031 152 PINW=0;
choutin 0:df2659fd8031 153 PINX=0;
choutin 0:df2659fd8031 154 PINY=1;
choutin 0:df2659fd8031 155 PINZ=1;
choutin 0:df2659fd8031 156 wait_ms(20);
choutin 0:df2659fd8031 157 puls_times--;
choutin 0:df2659fd8031 158
choutin 0:df2659fd8031 159 if(puls_times<0){break;}
choutin 0:df2659fd8031 160
choutin 0:df2659fd8031 161 PINW=1;
choutin 0:df2659fd8031 162 PINX=1;
choutin 0:df2659fd8031 163 PINY=0;
choutin 0:df2659fd8031 164 PINZ=0;
choutin 0:df2659fd8031 165 wait_ms(20);
choutin 0:df2659fd8031 166 puls_times--;
choutin 0:df2659fd8031 167
choutin 0:df2659fd8031 168 if(puls_times<0){break;}
choutin 0:df2659fd8031 169 }
choutin 0:df2659fd8031 170
choutin 0:df2659fd8031 171 }
choutin 0:df2659fd8031 172
choutin 0:df2659fd8031 173 if(degree<0){
choutin 0:df2659fd8031 174
choutin 0:df2659fd8031 175 puls_times=(-1)*(1+(int)(degree/(3.75)));
choutin 0:df2659fd8031 176
choutin 0:df2659fd8031 177 while(1){
choutin 0:df2659fd8031 178 PINW=1;
choutin 0:df2659fd8031 179 PINX=1;
choutin 0:df2659fd8031 180 PINY=0;
choutin 0:df2659fd8031 181 PINZ=0;
choutin 0:df2659fd8031 182 wait_ms(20);
choutin 0:df2659fd8031 183 puls_times--;
choutin 0:df2659fd8031 184
choutin 0:df2659fd8031 185 if(puls_times<0){break;}
choutin 0:df2659fd8031 186
choutin 0:df2659fd8031 187 PINW=1;
choutin 0:df2659fd8031 188 PINX=0;
choutin 0:df2659fd8031 189 PINY=0;
choutin 0:df2659fd8031 190 PINZ=1;
choutin 0:df2659fd8031 191 wait_ms(20);
choutin 0:df2659fd8031 192 puls_times--;
choutin 0:df2659fd8031 193
choutin 0:df2659fd8031 194 if(puls_times<0){break;}
choutin 0:df2659fd8031 195
choutin 0:df2659fd8031 196 PINW=0;
choutin 0:df2659fd8031 197 PINX=0;
choutin 0:df2659fd8031 198 PINY=1;
choutin 0:df2659fd8031 199 PINZ=1;
choutin 0:df2659fd8031 200 wait_ms(20);
choutin 0:df2659fd8031 201 puls_times--;
choutin 0:df2659fd8031 202
choutin 0:df2659fd8031 203 if(puls_times<0){break;}
choutin 0:df2659fd8031 204
choutin 0:df2659fd8031 205 PINW=0;
choutin 0:df2659fd8031 206 PINX=1;
choutin 0:df2659fd8031 207 PINY=1;
choutin 0:df2659fd8031 208 PINZ=0;
choutin 0:df2659fd8031 209 wait_ms(20);
choutin 0:df2659fd8031 210 puls_times--;
choutin 0:df2659fd8031 211
choutin 0:df2659fd8031 212 if(puls_times<0){break;}
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
choutin 0:df2659fd8031 221 int sensor(void){
choutin 0:df2659fd8031 222 //センサー読
choutin 0:df2659fd8031 223 int right ,left,x,y;
choutin 0:df2659fd8031 224
choutin 0:df2659fd8031 225 right=rightsensor.read();
choutin 0:df2659fd8031 226 left=leftsensor.read();
choutin 0:df2659fd8031 227
choutin 0:df2659fd8031 228 if(right>0.5){x=1;}
choutin 0:df2659fd8031 229 else{right=0;}
choutin 0:df2659fd8031 230
choutin 0:df2659fd8031 231 if(left>0.5){y=1;}
choutin 0:df2659fd8031 232 else{left=0;}
choutin 0:df2659fd8031 233
choutin 0:df2659fd8031 234 return x+2*(y);
choutin 0:df2659fd8031 235 //(right,left)=(off,off),(on,off),(off,on),(on,on)
choutin 0:df2659fd8031 236 // 0 1 2 3
choutin 0:df2659fd8031 237 }
choutin 0:df2659fd8031 238
choutin 0:df2659fd8031 239
choutin 0:df2659fd8031 240
choutin 1:a1e592eca305 241 void initmotor(){
choutin 1:a1e592eca305 242
choutin 1:a1e592eca305 243
choutin 1:a1e592eca305 244 M1cw.period_us(256);
choutin 1:a1e592eca305 245 M1ccw.period_us(256);
choutin 1:a1e592eca305 246 M2cw.period_us(256);
choutin 1:a1e592eca305 247 M2ccw.period_us(256);
choutin 1:a1e592eca305 248
choutin 1:a1e592eca305 249 }
choutin 1:a1e592eca305 250
choutin 1:a1e592eca305 251 void move(int right,int left){
choutin 1:a1e592eca305 252
choutin 2:b204cf2f9b60 253 float rightduty,leftduty;
choutin 2:b204cf2f9b60 254
choutin 1:a1e592eca305 255 if(right>256){right=256;}
choutin 1:a1e592eca305 256 if(left>256){left=256;}
choutin 1:a1e592eca305 257 if(right<-256){right=-256;}
choutin 1:a1e592eca305 258 if(left<-256){left=-256;}
choutin 1:a1e592eca305 259
choutin 2:b204cf2f9b60 260 rightduty=right/256.0;
choutin 2:b204cf2f9b60 261 leftduty=left/256.0;
choutin 1:a1e592eca305 262 if(right>0){
choutin 2:b204cf2f9b60 263 M1cw.write(1-rightduty);
choutin 2:b204cf2f9b60 264 M1ccw.write(1);
choutin 1:a1e592eca305 265 }else{
choutin 2:b204cf2f9b60 266 M1cw.write(1);
choutin 2:b204cf2f9b60 267 M1ccw.write(1+rightduty);
choutin 0:df2659fd8031 268 }
choutin 2:b204cf2f9b60 269
choutin 1:a1e592eca305 270 if(left>0){
choutin 2:b204cf2f9b60 271 M2cw.write(1-leftduty);
choutin 2:b204cf2f9b60 272 M2ccw.write(1);
choutin 1:a1e592eca305 273 }else{
choutin 2:b204cf2f9b60 274 M2cw.write(1);
choutin 2:b204cf2f9b60 275 M2ccw.write(1+leftduty);
choutin 1:a1e592eca305 276 }
choutin 1:a1e592eca305 277 }
choutin 0:df2659fd8031 278
choutin 0:df2659fd8031 279
choutin 0:df2659fd8031 280 int phase(void){
choutin 0:df2659fd8031 281
choutin 0:df2659fd8031 282 phase1.mode(PullUp);
choutin 0:df2659fd8031 283 phase2.mode(PullUp);
choutin 0:df2659fd8031 284 phase4.mode(PullUp);
choutin 0:df2659fd8031 285 phase8.mode(PullUp);
choutin 0:df2659fd8031 286
choutin 0:df2659fd8031 287 int r=0;
choutin 0:df2659fd8031 288
choutin 0:df2659fd8031 289 r=phase1+2*phase2+4*phase4+8*phase8;
choutin 0:df2659fd8031 290
choutin 0:df2659fd8031 291 return r;
choutin 0:df2659fd8031 292 }
choutin 0:df2659fd8031 293