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