F3RC1班 / Mbed 2 deprecated f3rc_auto

自動機

Dependencies:   EC mbed

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?

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