F3RC1班
/
f3rc_auto
自動機
main.cpp@0:1f542f8756d6, 2017-08-22 (annotated)
- Committer:
- eri
- Date:
- Tue Aug 22 02:39:13 2017 +0000
- Revision:
- 0:1f542f8756d6
- Child:
- 1:651bd0514eb9
???
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eri | 0:1f542f8756d6 | 1 | #include "mbed.h" |
eri | 0:1f542f8756d6 | 2 | #include "EC.h" |
eri | 0:1f542f8756d6 | 3 | |
eri | 0:1f542f8756d6 | 4 | PwmOut servo(PB_7); |
eri | 0:1f542f8756d6 | 5 | DigitalIn sw(PA_15); |
eri | 0:1f542f8756d6 | 6 | SPISlave spi(PB_15,PB_14,PB_13,PB_12); |
eri | 0:1f542f8756d6 | 7 | |
eri | 0:1f542f8756d6 | 8 | Ec Ec1(PB_6,PC_7,NC,1048,0.05); |
eri | 0:1f542f8756d6 | 9 | Ticker ticker; |
eri | 0:1f542f8756d6 | 10 | Ticker ticker2; |
eri | 0:1f542f8756d6 | 11 | DigitalIn button(USER_BUTTON); |
eri | 0:1f542f8756d6 | 12 | Serial pc(USBTX,USBRX); |
eri | 0:1f542f8756d6 | 13 | |
eri | 0:1f542f8756d6 | 14 | SpeedControl motor1(PA_10,PB_3,NC,1048,0.05,PA_1,PA_0); //left |
eri | 0:1f542f8756d6 | 15 | SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5); //right |
eri | 0:1f542f8756d6 | 16 | |
eri | 0:1f542f8756d6 | 17 | |
eri | 0:1f542f8756d6 | 18 | |
eri | 0:1f542f8756d6 | 19 | int n=1; |
eri | 0:1f542f8756d6 | 20 | |
eri | 0:1f542f8756d6 | 21 | |
eri | 0:1f542f8756d6 | 22 | void calOmega() //角速度計算関数 |
eri | 0:1f542f8756d6 | 23 | { |
eri | 0:1f542f8756d6 | 24 | motor1.CalOmega(); |
eri | 0:1f542f8756d6 | 25 | motor2.CalOmega(); |
eri | 0:1f542f8756d6 | 26 | Ec1.CalOmega(); |
eri | 0:1f542f8756d6 | 27 | } |
eri | 0:1f542f8756d6 | 28 | |
eri | 0:1f542f8756d6 | 29 | void st() |
eri | 0:1f542f8756d6 | 30 | { |
eri | 0:1f542f8756d6 | 31 | int kai=0; |
eri | 0:1f542f8756d6 | 32 | while(kai<50000){ |
eri | 0:1f542f8756d6 | 33 | motor1.stop(); |
eri | 0:1f542f8756d6 | 34 | motor2.stop(); |
eri | 0:1f542f8756d6 | 35 | kai=kai + 1; |
eri | 0:1f542f8756d6 | 36 | } |
eri | 0:1f542f8756d6 | 37 | } |
eri | 0:1f542f8756d6 | 38 | |
eri | 0:1f542f8756d6 | 39 | /*void print() |
eri | 0:1f542f8756d6 | 40 | { |
eri | 0:1f542f8756d6 | 41 | // pc.printf("n=%d",n); |
eri | 0:1f542f8756d6 | 42 | // pc.printf("count1=%d",Ec1.getCount()); |
eri | 0:1f542f8756d6 | 43 | // pc.printf("gyro=%d",spi.read()); |
eri | 0:1f542f8756d6 | 44 | pc.printf("count1=%f ",motor1.getOmega()); |
eri | 0:1f542f8756d6 | 45 | pc.printf("count2=%f ",motor2.getOmega()); |
eri | 0:1f542f8756d6 | 46 | pc.printf("duty1=%f ",motor1.duty); |
eri | 0:1f542f8756d6 | 47 | pc.printf("duty2=%f\r\n",motor2.duty); |
eri | 0:1f542f8756d6 | 48 | }*/ |
eri | 0:1f542f8756d6 | 49 | |
eri | 0:1f542f8756d6 | 50 | void str(int a) // |
eri | 0:1f542f8756d6 | 51 | { |
eri | 0:1f542f8756d6 | 52 | int kai=0; |
eri | 0:1f542f8756d6 | 53 | |
eri | 0:1f542f8756d6 | 54 | while(n==1){ |
eri | 0:1f542f8756d6 | 55 | int dis=0.301*Ec1.getCount(); |
eri | 0:1f542f8756d6 | 56 | if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示 |
eri | 0:1f542f8756d6 | 57 | // if(spi.receive()){ |
eri | 0:1f542f8756d6 | 58 | pc.printf("count1=%f ",motor1.getOmega()); |
eri | 0:1f542f8756d6 | 59 | pc.printf("count2=%f ",motor2.getOmega()); |
eri | 0:1f542f8756d6 | 60 | pc.printf("duty1=%f ",motor1.duty); |
eri | 0:1f542f8756d6 | 61 | pc.printf("duty2=%f\r\n",motor2.duty); |
eri | 0:1f542f8756d6 | 62 | kai=0; |
eri | 0:1f542f8756d6 | 63 | // } |
eri | 0:1f542f8756d6 | 64 | } |
eri | 0:1f542f8756d6 | 65 | if(dis < a){ //距離 |
eri | 0:1f542f8756d6 | 66 | motor1.Sc(5); |
eri | 0:1f542f8756d6 | 67 | motor2.Sc(5); |
eri | 0:1f542f8756d6 | 68 | }else{ |
eri | 0:1f542f8756d6 | 69 | n=2; |
eri | 0:1f542f8756d6 | 70 | } |
eri | 0:1f542f8756d6 | 71 | kai++; |
eri | 0:1f542f8756d6 | 72 | //pc.printf("kai=%d\r\n",kai); |
eri | 0:1f542f8756d6 | 73 | |
eri | 0:1f542f8756d6 | 74 | } |
eri | 0:1f542f8756d6 | 75 | st(); |
eri | 0:1f542f8756d6 | 76 | } |
eri | 0:1f542f8756d6 | 77 | void back(int a) |
eri | 0:1f542f8756d6 | 78 | { |
eri | 0:1f542f8756d6 | 79 | int kai=0; |
eri | 0:1f542f8756d6 | 80 | while(n==2){ |
eri | 0:1f542f8756d6 | 81 | int dis=0.301*Ec1.getCount(); |
eri | 0:1f542f8756d6 | 82 | kai++; |
eri | 0:1f542f8756d6 | 83 | if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示 |
eri | 0:1f542f8756d6 | 84 | // if(spi.receive()){ |
eri | 0:1f542f8756d6 | 85 | pc.printf("count1=%f ",motor1.getOmega()); |
eri | 0:1f542f8756d6 | 86 | pc.printf("count2=%f ",motor2.getOmega()); |
eri | 0:1f542f8756d6 | 87 | pc.printf("duty1=%f ",motor1.duty); |
eri | 0:1f542f8756d6 | 88 | pc.printf("duty2=%f\r\n",motor2.duty); |
eri | 0:1f542f8756d6 | 89 | kai=0; |
eri | 0:1f542f8756d6 | 90 | // } |
eri | 0:1f542f8756d6 | 91 | } |
eri | 0:1f542f8756d6 | 92 | if(dis > a){ |
eri | 0:1f542f8756d6 | 93 | motor1.Sc(-5); |
eri | 0:1f542f8756d6 | 94 | motor2.Sc(-5); |
eri | 0:1f542f8756d6 | 95 | }else{ |
eri | 0:1f542f8756d6 | 96 | motor1.Sc(0); |
eri | 0:1f542f8756d6 | 97 | motor2.Sc(0); |
eri | 0:1f542f8756d6 | 98 | n=1; |
eri | 0:1f542f8756d6 | 99 | } |
eri | 0:1f542f8756d6 | 100 | } |
eri | 0:1f542f8756d6 | 101 | st(); |
eri | 0:1f542f8756d6 | 102 | } |
eri | 0:1f542f8756d6 | 103 | |
eri | 0:1f542f8756d6 | 104 | void turnR(int b,int c) |
eri | 0:1f542f8756d6 | 105 | { |
eri | 0:1f542f8756d6 | 106 | int kai=0; |
eri | 0:1f542f8756d6 | 107 | int ang=0; |
eri | 0:1f542f8756d6 | 108 | while(n==2){ |
eri | 0:1f542f8756d6 | 109 | |
eri | 0:1f542f8756d6 | 110 | kai++; |
eri | 0:1f542f8756d6 | 111 | if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示 |
eri | 0:1f542f8756d6 | 112 | // if(spi.receive()){ |
eri | 0:1f542f8756d6 | 113 | // pc.printf("count1=%f ",motor1.getOmega()); |
eri | 0:1f542f8756d6 | 114 | pc.printf("count2=%f ",motor2.getOmega()); |
eri | 0:1f542f8756d6 | 115 | // pc.printf("duty1=%f ",motor1.duty); |
eri | 0:1f542f8756d6 | 116 | pc.printf("duty2=%f\r\n",motor2.duty); |
eri | 0:1f542f8756d6 | 117 | // pc.printf("ang=%f\r\n",ang); |
eri | 0:1f542f8756d6 | 118 | kai=0; |
eri | 0:1f542f8756d6 | 119 | // } |
eri | 0:1f542f8756d6 | 120 | } |
eri | 0:1f542f8756d6 | 121 | /* if(spi.receive()){ |
eri | 0:1f542f8756d6 | 122 | pc.printf("count1=%d",Ec1.getCount()); |
eri | 0:1f542f8756d6 | 123 | pc.printf("gyro=%d\r\n",spi.read()); |
eri | 0:1f542f8756d6 | 124 | }*/ |
eri | 0:1f542f8756d6 | 125 | if(spi.receive()){ |
eri | 0:1f542f8756d6 | 126 | ang=spi.read(); |
eri | 0:1f542f8756d6 | 127 | // pc.printf("ang=%d\r\n",ang); |
eri | 0:1f542f8756d6 | 128 | } |
eri | 0:1f542f8756d6 | 129 | if((ang < b) || (ang > c)){ //角 |
eri | 0:1f542f8756d6 | 130 | motor1.Sc(5); |
eri | 0:1f542f8756d6 | 131 | motor2.Sc(-5); |
eri | 0:1f542f8756d6 | 132 | |
eri | 0:1f542f8756d6 | 133 | }else{ |
eri | 0:1f542f8756d6 | 134 | Ec1.reset(); |
eri | 0:1f542f8756d6 | 135 | motor1.Sc(0); |
eri | 0:1f542f8756d6 | 136 | motor2.Sc(0); |
eri | 0:1f542f8756d6 | 137 | n=1; |
eri | 0:1f542f8756d6 | 138 | } |
eri | 0:1f542f8756d6 | 139 | } |
eri | 0:1f542f8756d6 | 140 | st(); |
eri | 0:1f542f8756d6 | 141 | } |
eri | 0:1f542f8756d6 | 142 | |
eri | 0:1f542f8756d6 | 143 | void turnL(int b,int c) |
eri | 0:1f542f8756d6 | 144 | { |
eri | 0:1f542f8756d6 | 145 | int kai=0; |
eri | 0:1f542f8756d6 | 146 | int ang=0; |
eri | 0:1f542f8756d6 | 147 | while(n==2){ |
eri | 0:1f542f8756d6 | 148 | |
eri | 0:1f542f8756d6 | 149 | kai++; |
eri | 0:1f542f8756d6 | 150 | if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示 |
eri | 0:1f542f8756d6 | 151 | // if(spi.receive()){ |
eri | 0:1f542f8756d6 | 152 | pc.printf("count1=%f ",motor1.getOmega()); |
eri | 0:1f542f8756d6 | 153 | // pc.printf("count2=%f ",motor2.getOmega()); |
eri | 0:1f542f8756d6 | 154 | pc.printf("duty1=%f\r\n ",motor1.duty); |
eri | 0:1f542f8756d6 | 155 | // pc.printf("duty2=%f\r\n",motor2.duty); |
eri | 0:1f542f8756d6 | 156 | // pc.printf("ang=%f\r\n",ang); |
eri | 0:1f542f8756d6 | 157 | kai=0; |
eri | 0:1f542f8756d6 | 158 | // } |
eri | 0:1f542f8756d6 | 159 | } |
eri | 0:1f542f8756d6 | 160 | /* if(spi.receive()){ |
eri | 0:1f542f8756d6 | 161 | pc.printf("n=%d",n); |
eri | 0:1f542f8756d6 | 162 | pc.printf("count1=%d",Ec1.getCount()); |
eri | 0:1f542f8756d6 | 163 | pc.printf("gyro=%d\r\n",spi.read()); |
eri | 0:1f542f8756d6 | 164 | }*/ |
eri | 0:1f542f8756d6 | 165 | if(spi.receive()){ |
eri | 0:1f542f8756d6 | 166 | ang=spi.read(); |
eri | 0:1f542f8756d6 | 167 | // pc.printf("ang=%d\r\n",ang); |
eri | 0:1f542f8756d6 | 168 | } |
eri | 0:1f542f8756d6 | 169 | if((ang < b) || (ang > c)){ //角度 |
eri | 0:1f542f8756d6 | 170 | motor1.Sc(-5); |
eri | 0:1f542f8756d6 | 171 | motor2.Sc(5); |
eri | 0:1f542f8756d6 | 172 | }else{ |
eri | 0:1f542f8756d6 | 173 | Ec1.reset(); |
eri | 0:1f542f8756d6 | 174 | motor1.Sc(0); |
eri | 0:1f542f8756d6 | 175 | motor2.Sc(0); |
eri | 0:1f542f8756d6 | 176 | n=1; |
eri | 0:1f542f8756d6 | 177 | } |
eri | 0:1f542f8756d6 | 178 | |
eri | 0:1f542f8756d6 | 179 | } |
eri | 0:1f542f8756d6 | 180 | st(); |
eri | 0:1f542f8756d6 | 181 | } |
eri | 0:1f542f8756d6 | 182 | |
eri | 0:1f542f8756d6 | 183 | |
eri | 0:1f542f8756d6 | 184 | int main() { |
eri | 0:1f542f8756d6 | 185 | |
eri | 0:1f542f8756d6 | 186 | servo.period_ms(20); |
eri | 0:1f542f8756d6 | 187 | spi.format(16,3); |
eri | 0:1f542f8756d6 | 188 | spi.frequency(1000000); |
eri | 0:1f542f8756d6 | 189 | |
eri | 0:1f542f8756d6 | 190 | |
eri | 0:1f542f8756d6 | 191 | ticker.attach(&calOmega,0.05); |
eri | 0:1f542f8756d6 | 192 | // ticker2.attach(&print,0.5); |
eri | 0:1f542f8756d6 | 193 | |
eri | 0:1f542f8756d6 | 194 | motor1.setPDparam(0.55,0.5); //PDパラメータを設定 |
eri | 0:1f542f8756d6 | 195 | motor2.setPDparam(0.6,0.5); //PDパラメータを設定 |
eri | 0:1f542f8756d6 | 196 | |
eri | 0:1f542f8756d6 | 197 | motor1.setDOconstant(13.1); |
eri | 0:1f542f8756d6 | 198 | motor2.setDOconstant(15.8); |
eri | 0:1f542f8756d6 | 199 | |
eri | 0:1f542f8756d6 | 200 | |
eri | 0:1f542f8756d6 | 201 | sw.mode(PullUp); |
eri | 0:1f542f8756d6 | 202 | |
eri | 0:1f542f8756d6 | 203 | |
eri | 0:1f542f8756d6 | 204 | |
eri | 0:1f542f8756d6 | 205 | if(sw==1){ //スタートゾーン1 |
eri | 0:1f542f8756d6 | 206 | |
eri | 0:1f542f8756d6 | 207 | servo.pulsewidth_us(900); |
eri | 0:1f542f8756d6 | 208 | str(540); |
eri | 0:1f542f8756d6 | 209 | turnL(2650,2750); |
eri | 0:1f542f8756d6 | 210 | str(1481); |
eri | 0:1f542f8756d6 | 211 | turnL(1750,1850); |
eri | 0:1f542f8756d6 | 212 | str(540); |
eri | 0:1f542f8756d6 | 213 | turnR(2650,2750); |
eri | 0:1f542f8756d6 | 214 | str(1481); |
eri | 0:1f542f8756d6 | 215 | servo.pulsewidth_us(1800); |
eri | 0:1f542f8756d6 | 216 | wait(0.5); |
eri | 0:1f542f8756d6 | 217 | back(0); |
eri | 0:1f542f8756d6 | 218 | n=2; |
eri | 0:1f542f8756d6 | 219 | turnR(3550,3600); |
eri | 0:1f542f8756d6 | 220 | str(540); |
eri | 0:1f542f8756d6 | 221 | turnR(850,950); |
eri | 0:1f542f8756d6 | 222 | str(1481); |
eri | 0:1f542f8756d6 | 223 | turnR(1750,1850); |
eri | 0:1f542f8756d6 | 224 | str(570); |
eri | 0:1f542f8756d6 | 225 | servo.pulsewidth_us(900); |
eri | 0:1f542f8756d6 | 226 | back(450); |
eri | 0:1f542f8756d6 | 227 | servo.pulsewidth_us(1800); |
eri | 0:1f542f8756d6 | 228 | str(570); |
eri | 0:1f542f8756d6 | 229 | |
eri | 0:1f542f8756d6 | 230 | //押し出し |
eri | 0:1f542f8756d6 | 231 | |
eri | 0:1f542f8756d6 | 232 | }else{ //スタートゾーン2 |
eri | 0:1f542f8756d6 | 233 | servo.pulsewidth_us(900); |
eri | 0:1f542f8756d6 | 234 | str(540); |
eri | 0:1f542f8756d6 | 235 | servo.pulsewidth_us(1800); |
eri | 0:1f542f8756d6 | 236 | wait(0.5); |
eri | 0:1f542f8756d6 | 237 | turnL(850,950); |
eri | 0:1f542f8756d6 | 238 | str(1481); |
eri | 0:1f542f8756d6 | 239 | turnL(0,50); |
eri | 0:1f542f8756d6 | 240 | str(540); |
eri | 0:1f542f8756d6 | 241 | turnR(850,950); |
eri | 0:1f542f8756d6 | 242 | str(1481); |
eri | 0:1f542f8756d6 | 243 | turnR(1750,1850); |
eri | 0:1f542f8756d6 | 244 | str(570); |
eri | 0:1f542f8756d6 | 245 | servo.pulsewidth_us(900); |
eri | 0:1f542f8756d6 | 246 | back(450); |
eri | 0:1f542f8756d6 | 247 | servo.pulsewidth_us(1800); |
eri | 0:1f542f8756d6 | 248 | str(570); |
eri | 0:1f542f8756d6 | 249 | |
eri | 0:1f542f8756d6 | 250 | |
eri | 0:1f542f8756d6 | 251 | |
eri | 0:1f542f8756d6 | 252 | |
eri | 0:1f542f8756d6 | 253 | |
eri | 0:1f542f8756d6 | 254 | } |
eri | 0:1f542f8756d6 | 255 | |
eri | 0:1f542f8756d6 | 256 | |
eri | 0:1f542f8756d6 | 257 | } //・距離とエンコーダ出力の関係 ・ジャイロの値 |
eri | 0:1f542f8756d6 | 258 |