Diff: main.cpp

Revision:

0:2694d5a2f90a

Child:

1:4b622e363fa1

diff -r 000000000000 -r 2694d5a2f90a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 01 23:50:06 2017 +0000
@@ -0,0 +1,257 @@
+#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 dis=0;  
+int ang=0;                             
+                                     
+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;
+ }
+}
+
+int Dis(){
+    dis=0.301*Ec1.getCount(); 
+    return dis;
+    }
+    
+
+
+/*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){
+   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("gyro=%d",spi.read());
+            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){
+ 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(-4.9);
+     motor2.Sc(-5.2);
+    }else{
+     motor1.Sc(0);
+     motor2.Sc(0);
+     n=1;
+     }
+ }
+ st();
+}
+
+void turnR(double b)
+{
+ // int kai=0;
+ //int ang=0;
+
+
+ if(b > 0){                   //角
+   motor1.Sc(5);
+   motor2.Sc(0);
+   wait(b);
+   
+   Ec1.reset();
+   motor1.Sc(0);
+   motor2.Sc(0);
+   wait(0.5);
+   
+   }else{
+     motor1.Sc(0);
+     motor2.Sc(-5);
+     wait(-b);
+     
+     Ec1.reset();
+     motor1.Sc(0);
+     motor2.Sc(0);
+     wait(0.5);
+     
+}
+n=1;
+}
+
+void turnL(double b)
+{
+ // int kai=0;
+ //int ang=0;
+
+
+ if(b > 0){                   //角
+   motor1.Sc(0);
+   motor2.Sc(5);
+   wait(0.815);
+   
+   Ec1.reset();
+   motor1.Sc(0);
+   motor2.Sc(0);
+   wait(0.5);
+   
+   
+   }else{
+     motor1.Sc(-5);
+     motor2.Sc(0);
+     wait(0.81);
+     
+     Ec1.reset();
+     motor1.Sc(0);
+     motor2.Sc(0);
+     wait(0.5);
+     
+}
+n=1;
+}
+
+/*void turn(){
+     motor1.Sc(-5);
+     motor2.Sc(5);
+     wait(1.5);
+     
+    
+    }*/
+
+
+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.3,0.4);  //PDパラメータを設定
+    motor2.setPDparam(0.35,0.4);  //PDパラメータを設定
+    
+    motor1.setDOconstant(14.4);         //duty比と角速度の変換係数を46.3に設定←設定必要かも
+    motor2.setDOconstant(15.8);        
+    
+    
+    sw.mode(PullUp);
+    
+
+    
+  if(sw==1){                       //スタートゾーン１
+    
+            servo.pulsewidth_us(900);
+            str(395);
+            turnL(0.8); 
+            str(1151);
+            turnL(0.8);
+            str(310);
+            turnR(0.74);
+            servo.pulsewidth_us(1800);
+            str(1326);
+            servo.pulsewidth_us(900); 
+            wait(0.5);
+            back(0);
+            turnR(-0.84);
+            str(340);
+            turnR(0.8);
+            str(1131);
+            turnR(0.8);
+            str(375);
+            servo.pulsewidth_us(1800);
+            back(245);
+            servo.pulsewidth_us(900);
+            str(365);          
+                    
+           
+                                                            //押し出し
+                                                                   
+    }else{                                                   //スタートゾーン２
+           servo.pulsewidth_us(1800);
+           str(400);
+           servo.pulsewidth_us(900);
+           wait(0.5);                                           //
+           turnL(0.8);
+           str(1131);
+           turnL(0.81);
+           str(270);
+           turnR(0.77);
+            str(1131);
+            turnR(0.77);
+            str(380);
+            servo.pulsewidth_us(1800);
+            back(260);
+            servo.pulsewidth_us(900);
+            str(390);
+            
+            
+            
+        
+    
+  }
+    
+    
+}                       //・距離とエンコーダ出力の関係      ・ジャイロの値
+