Diff: main.cpp

Revision:

1:9732c03b0de4

Parent:

0:ef259ff25554

Child:

2:d213999ee436

--- a/main.cpp	Fri Oct 18 09:49:47 2019 +0000
+++ b/main.cpp	Thu Nov 14 07:22:16 2019 +0000
@@ -3,23 +3,71 @@
 #include "SpeedController.h"    //SpeedControlライブラリをインクルード
 #define RESOLUTION 500
 
-PwmOut f(PA_9);
-PwmOut b(PA_10);
+//PwmOut f(PA_9);
+//PwmOut b(PA_10);
 DigitalIn s(PB_7,PullUp);
+DigitalOut out1(PA_8);
+Ticker ticker;
+Ec4multi EC(PA_6,PA_7,RESOLUTION);
+InterruptIn X(PA_4);
+SpeedControl motor(PA_10,PA_9,50,EC);
 
-Ec4multi EC(PA_6,PA_7,RESOLUTION);
+void calOmega()
+{
+    EC.calOmega();
+}
+
+int X_count=0;
+
+void Xcount()
+{
+    X_count++;
+}
+/*void shot()
+{
+    out1 = 1;
+}
+*/
 int main()
 {
-printf("%d\n",s.read());
-f.period_us(50);
-b.period_us(50);
+     out1 =0;
+    motor.setPDparam(0.01,0.01);
+    motor.setEquation(0.005,0.01,0.023,0.0);
+    /*
+    b.setPDparam(double kp,double kd);
+    b.setEquation(double cf,double df,double cb,double db);
+    */
+    //int i=0;
+    double a=0,r=0.4,v=0;//rで半径指定 a*r=v
+    X.rise(&Xcount);
+    ticker.attach(&calOmega,0.05);
+    printf("%d\n",s.read());
+    motor.setDutyLimit(0.6);
+    printf("Omega  V(m/s)");
     while(1) {
-        printf("omega=%f\r\n",EC.getOmega());
-        wait(0.1);
-        f = 0.2;
+        if(s.read()==1) {
+            while(1) {
+                a=EC.getOmega();
+                v=a*r;
+                printf("%f   %f %d\r\n",a,v,X_count);
+                motor.Sc(10.5);
+                motor.setDutyLimit(0.4);
+                /*if(a>16.9||a<17.1) {
+                    i++;
+                }*/
+                /*if(X_count==6) {
+                    out1 =1;
+                    printf("shot\n");break;
+                }
+                */
+
+                if(s.read()==0)break;
+            }
+        }
+        motor.stop();
+        motor.reset();
         if(s.read() ==0)break;
+
     }
-    f=0;
-    b=0;
     return 0;
 }