Diff: main.cpp

Revision:

0:8ffb3e099b90

diff -r 000000000000 -r 8ffb3e099b90 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 11 05:14:21 2020 +0000
@@ -0,0 +1,128 @@
+#include"mbed.h"
+#include"KondoServo.h"
+#include "EC.h" //Encoderライブラリをインクルード
+#define RESOLUTION 500
+
+//Serial pc(USBTX, USBRX); // tx, rx
+PwmOut f(p26);//モーター
+PwmOut b(p25);
+
+Ticker ticker;//割込み
+
+Ec1multi EC(p16,p17,RESOLUTION);//エンコーダー
+InterruptIn X(p15);
+
+KondoServo servo(p28,p27);//サーボ
+
+DigitalOut out1(p19);//エアシリンダー（つかむ方）
+DigitalOut out2(p20);//エアシリンダー（投擲）
+
+DigitalIn button(p14,PullUp);//タッチセンサー
+
+DigitalOut led1(LED1);//LED
+DigitalOut led2(LED2);
+
+void cal()
+{
+    EC.calOmega();
+}
+
+int X_count=0;
+
+void Xcount()
+{
+    X_count++;
+}
+
+int main()
+{
+    X.rise(&Xcount);
+    f.period_us(50);
+    b.period_us(50);
+    out1 = 0;
+    out2 = 0;
+    double a=0,r=0.4,v=0;//rで半径指定 a*r=v
+    int count=0;
+    ticker.attach(&cal,0.05);
+
+    int id = 0;
+    double SERVO2DEG = 270.0 / (11500 - 3500);
+    double first = (6800 - 3500) * SERVO2DEG;
+    double grab = (3800 - 3500) * SERVO2DEG;
+    double pass = (5000 - 3500) * SERVO2DEG;
+
+    servo.set_degree(id, first);
+    printf("start\r\n");
+    wait(1);
+
+    servo.set_degree(id, grab);
+    wait(1);
+    led1 = 1;//掴む
+    out1 = 1;
+    printf("grab\r\n");
+    wait(1);
+
+    while(1) {//投擲アームをセット
+        a = EC.getOmega();
+        count = EC.getCount();
+        v = a*r;
+        f = 0.0;//速度一定
+        b = 0.2;
+        printf("%.3f %.3f %d\r\n",a,v,count);
+        
+        if(X_count == 1) {
+            f = 0.0;
+            b = 0.0;
+            break;
+        }
+    }
+
+    servo.set_degree(id, pass);
+    printf("set\r\n");
+
+    while(1) {
+        a = EC.getOmega();
+        count = EC.getCount();
+        v = a*r;
+        f = 0.0;//速度一定
+        b = 0.05;
+        printf("%.3f %.3f\r\n",a,v);
+        if(button.read() == 0) {
+            wait(0.05);
+            if(button.read() == 0) {
+                f = 0.0;
+                b = 0.0;
+                wait(0.5);
+                break;
+            }
+        }
+    }
+    led2 = 1;//投擲アームが掴む
+    out2 = 1;
+    wait (1);
+
+    led1 = 0;//離す
+    out1 = 0;
+    printf("ok\r\n");
+    wait(1);
+    servo.set_degree(id, first);
+    wait(1);
+
+    while(1) {//投擲アームを上に
+        a = EC.getOmega();
+        count = EC.getCount();
+        v = a*r;
+        f = 0.1;//速度一定
+        b = 0.0;
+        printf("%.3f %.3f\r\n",a,v);
+        if(X_count == 2) {
+            EC.reset();
+            X_count =0;
+            break;
+        }
+    }
+
+    servo.set_degree(id, grab);//掴むアームをセット
+    printf("finish\r\n");
+    return 0;
+}
\ No newline at end of file