F3RC1班 / Mbed 2 deprecated f3rc_auto_2

自動機 時間制限ver.

Dependencies:   EC mbed

main.cpp@1:4b622e363fa1, 2017-09-02 (annotated)

Committer:
eri
Date:
Sat Sep 02 06:17:48 2017 +0000
Revision:
1:4b622e363fa1
Parent:
0:2694d5a2f90a 
??

Who changed what in which revision?

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