F3RC1班
自動機
Diff: main.cpp
- Revision:
- 0:1f542f8756d6
- Child:
- 1:651bd0514eb9
diff -r 000000000000 -r 1f542f8756d6 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Aug 22 02:39:13 2017 +0000
@@ -0,0 +1,258 @@
+#include "mbed.h"
+#include "EC.h"
+
+PwmOut servo(PB_7);
+DigitalIn sw(PA_15);
+SPISlave spi(PB_15,PB_14,PB_13,PB_12);
+
+Ec Ec1(PB_6,PC_7,NC,1048,0.05);
+ Ticker ticker;
+ Ticker ticker2;
+ DigitalIn button(USER_BUTTON);
+ Serial pc(USBTX,USBRX);
+
+SpeedControl motor1(PA_10,PB_3,NC,1048,0.05,PA_1,PA_0); //left
+SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5); //right
+
+
+
+int n=1;
+
+
+void calOmega() //角速度計算関数
+{
+ motor1.CalOmega();
+ motor2.CalOmega();
+ Ec1.CalOmega();
+}
+
+void st()
+{
+ int kai=0;
+ while(kai<50000){
+ motor1.stop();
+ motor2.stop();
+ kai=kai + 1;
+ }
+}
+
+/*void print()
+{
+ // pc.printf("n=%d",n);
+ // pc.printf("count1=%d",Ec1.getCount());
+ // pc.printf("gyro=%d",spi.read());
+ pc.printf("count1=%f ",motor1.getOmega());
+ pc.printf("count2=%f ",motor2.getOmega());
+ pc.printf("duty1=%f ",motor1.duty);
+ pc.printf("duty2=%f\r\n",motor2.duty);
+}*/
+
+void str(int a) //
+{
+int kai=0;
+
+ while(n==1){
+ int dis=0.301*Ec1.getCount();
+ if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
+ // if(spi.receive()){
+ pc.printf("count1=%f ",motor1.getOmega());
+ pc.printf("count2=%f ",motor2.getOmega());
+ pc.printf("duty1=%f ",motor1.duty);
+ pc.printf("duty2=%f\r\n",motor2.duty);
+ kai=0;
+ // }
+ }
+ if(dis < a){ //距離
+ motor1.Sc(5);
+ motor2.Sc(5);
+ }else{
+ n=2;
+ }
+ kai++;
+ //pc.printf("kai=%d\r\n",kai);
+
+}
+st();
+}
+void back(int a)
+{
+ int kai=0;
+ while(n==2){
+ int dis=0.301*Ec1.getCount();
+ kai++;
+ if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
+ // if(spi.receive()){
+ pc.printf("count1=%f ",motor1.getOmega());
+ pc.printf("count2=%f ",motor2.getOmega());
+ pc.printf("duty1=%f ",motor1.duty);
+ pc.printf("duty2=%f\r\n",motor2.duty);
+ kai=0;
+ // }
+ }
+ if(dis > a){
+ motor1.Sc(-5);
+ motor2.Sc(-5);
+ }else{
+ motor1.Sc(0);
+ motor2.Sc(0);
+ n=1;
+ }
+ }
+ st();
+}
+
+void turnR(int b,int c)
+{
+ int kai=0;
+ int ang=0;
+ while(n==2){
+
+ kai++;
+ if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
+ // if(spi.receive()){
+ // pc.printf("count1=%f ",motor1.getOmega());
+ pc.printf("count2=%f ",motor2.getOmega());
+ // pc.printf("duty1=%f ",motor1.duty);
+ pc.printf("duty2=%f\r\n",motor2.duty);
+ // pc.printf("ang=%f\r\n",ang);
+ kai=0;
+ // }
+ }
+ /* if(spi.receive()){
+ pc.printf("count1=%d",Ec1.getCount());
+ pc.printf("gyro=%d\r\n",spi.read());
+ }*/
+ if(spi.receive()){
+ ang=spi.read();
+// pc.printf("ang=%d\r\n",ang);
+ }
+ if((ang < b) || (ang > c)){ //角
+ motor1.Sc(5);
+ motor2.Sc(-5);
+
+ }else{
+ Ec1.reset();
+ motor1.Sc(0);
+ motor2.Sc(0);
+ n=1;
+ }
+}
+st();
+}
+
+void turnL(int b,int c)
+{
+ int kai=0;
+ int ang=0;
+ while(n==2){
+
+ kai++;
+ if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
+ // if(spi.receive()){
+ pc.printf("count1=%f ",motor1.getOmega());
+ // pc.printf("count2=%f ",motor2.getOmega());
+ pc.printf("duty1=%f\r\n ",motor1.duty);
+ // pc.printf("duty2=%f\r\n",motor2.duty);
+ // pc.printf("ang=%f\r\n",ang);
+ kai=0;
+ // }
+ }
+/* if(spi.receive()){
+ pc.printf("n=%d",n);
+ pc.printf("count1=%d",Ec1.getCount());
+ pc.printf("gyro=%d\r\n",spi.read());
+ }*/
+ if(spi.receive()){
+ ang=spi.read();
+// pc.printf("ang=%d\r\n",ang);
+ }
+ if((ang < b) || (ang > c)){ //角度
+ motor1.Sc(-5);
+ motor2.Sc(5);
+ }else{
+ Ec1.reset();
+ motor1.Sc(0);
+ motor2.Sc(0);
+ n=1;
+ }
+
+ }
+ st();
+}
+
+
+int main() {
+
+ servo.period_ms(20);
+ spi.format(16,3);
+ spi.frequency(1000000);
+
+
+ ticker.attach(&calOmega,0.05);
+ // ticker2.attach(&print,0.5);
+
+ motor1.setPDparam(0.55,0.5); //PDパラメータを設定
+ motor2.setPDparam(0.6,0.5); //PDパラメータを設定
+
+ motor1.setDOconstant(13.1);
+ motor2.setDOconstant(15.8);
+
+
+ sw.mode(PullUp);
+
+
+
+ if(sw==1){ //スタートゾーン１
+
+ servo.pulsewidth_us(900);
+ str(540);
+ turnL(2650,2750);
+ str(1481);
+ turnL(1750,1850);
+ str(540);
+ turnR(2650,2750);
+ str(1481);
+ servo.pulsewidth_us(1800);
+ wait(0.5);
+ back(0);
+ n=2;
+ turnR(3550,3600);
+ str(540);
+ turnR(850,950);
+ str(1481);
+ turnR(1750,1850);
+ str(570);
+ servo.pulsewidth_us(900);
+ back(450);
+ servo.pulsewidth_us(1800);
+ str(570);
+
+ //押し出し
+
+ }else{ //スタートゾーン２
+ servo.pulsewidth_us(900);
+ str(540);
+ servo.pulsewidth_us(1800);
+ wait(0.5);
+ turnL(850,950);
+ str(1481);
+ turnL(0,50);
+ str(540);
+ turnR(850,950);
+ str(1481);
+ turnR(1750,1850);
+ str(570);
+ servo.pulsewidth_us(900);
+ back(450);
+ servo.pulsewidth_us(1800);
+ str(570);
+
+
+
+
+
+ }
+
+
+} //・距離とエンコーダ出力の関係 ・ジャイロの値
+