Revision 2:d213999ee436, committed 2020-03-05

Comitter:

maxnagazumi

Date:

Thu Mar 05 01:58:48 2020 +0000

Parent:

1:9732c03b0de4

Commit message:

3/5;

Changed in this revision

--- a/CUIspeedcontorl.lib	Thu Nov 14 07:22:16 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/teams/ROBOSTEP_LIBRARY/code/SpeedController/#32f31b0f5815

--- a/main.cpp	Thu Nov 14 07:22:16 2019 +0000
+++ b/main.cpp	Thu Mar 05 01:58:48 2020 +0000
@@ -1,18 +1,21 @@
 #include "mbed.h"
 #include "EC.h" //Encoderライブラリをインクルード
-#include "SpeedController.h"    //SpeedControlライブラリをインクルード
 #define RESOLUTION 500
 
-//PwmOut f(PA_9);
-//PwmOut b(PA_10);
-DigitalIn s(PB_7,PullUp);
-DigitalOut out1(PA_8);
+//Serial pc(USBTX, USBRX); // tx, rx
+PwmOut f(p26);
+PwmOut b(p25);
 Ticker ticker;
-Ec4multi EC(PA_6,PA_7,RESOLUTION);
-InterruptIn X(PA_4);
-SpeedControl motor(PA_10,PA_9,50,EC);
+Ec1multi EC(p16,p17,RESOLUTION);
+InterruptIn X(p15);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 
-void calOmega()
+DigitalOut out(p20);
+
+void cal()
 {
     EC.calOmega();
 }
@@ -23,51 +26,66 @@
 {
     X_count++;
 }
-/*void shot()
-{
-    out1 = 1;
-}
-*/
+
 int main()
 {
-     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)");
+    f.period_us(50);
+    b.period_us(50);
+    out=1;
+    double a=0,r=0.4,v=0;
+    int i=0,count;//rで半径指定 a*r=v
+    ticker.attach(&cal,0.05);
+
+
+    while(1) {
+        printf("set");
+        if(X_count ==1) {
+            EC.reset();
+            X_count =0;
+            break;
+        }
+    }
+    //角度リセット
+    out = 0;
+    wait(5);
     while(1) {
-        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;
+        printf("%.3f %.3f %d %d\r\n",a,v,X_count,i);
+        a=EC.getOmega();
+        count=EC.getCount();
+        i =count%500;
+        v=a*r;
+        f=0.45;//速度一定
+        b=0.0;
+        led1=1;
+        led2=0;
+        led3=0;
+        led4=1;
+        if(X_count>3) {
+            if(i < -380 && i > -390) {//離す角度を決める
+                out=1; //ボールを離す
+                printf("shot\r\n");
+                led1=1;
+                led2=1;
+                led3=1;
+                led4=1;
+                break;
             }
         }
-        motor.stop();
-        motor.reset();
-        if(s.read() ==0)break;
+    }
+    printf("stop\r\n");
+    while(1) {
+        printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,i);
+        X.rise(&Xcount);
+        a=EC.getOmega();
+        count=EC.getCount();
+        i =count%500;
+        f=0;
+        b=0;
+        led1=0;
+        led2=1;
+        led3=1;
+        led4=0;
+    }
+}
 
-    }
-    return 0;
-}