Tk A / Mbed 2 deprecated f3rc3

fdlsj

Dependencies:   mbed

Fork of f3rc2 by Tk A

prime.h@0:df2659fd8031, 2016-08-28 (annotated)

Committer:
choutin
Date:
Sun Aug 28 17:03:34 2016 +0000
Revision:
0:df2659fd8031
Child:
1:a1e592eca305
???

Who changed what in which revision?

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