F3RC1班 / Mbed 2 deprecated f3rc_auto_2

自動機 時間制限ver.

Dependencies:   EC mbed

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?

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 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){ //スタートゾーン１
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{ //スタートゾーン２
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