Diff: main.cpp

Revision:

0:a40701dd7f26

diff -r 000000000000 -r a40701dd7f26 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 13 06:49:35 2019 +0000
@@ -0,0 +1,104 @@
+#include "mbed.h"
+#include "EC.h" //Encoderライブラリをインクルード
+#include "SpeedController.h"
+#define RESOLUTION 2048//分解能
+
+Ec4multi EC_backdrop(p15,p16,RESOLUTION);
+SpeedControl backdrop(p21,p22,50,EC_backdrop);
+Serial pc(USBTX,USBRX);
+DigitalOut snatch(p8);
+DigitalOut pass1(p27);
+DigitalOut pass2(p28);
+//Ticker motor_tick;  //角速度計算用ticker
+double a=0;//now
+double b=0;//target
+double turn=0;
+void tsukami()
+{
+    b=1.3;
+}
+void put()
+{
+    b=-0.7;
+}
+void top()
+{
+    b=0;
+}
+int main()
+{
+    pc.printf("setting please");
+    while(1) {
+        double old_turn=turn;
+        a=EC_backdrop.getRad();
+        if(a-b>=0.1) {
+            turn=0.1;
+            pc.printf("F");
+
+        } else if (a-b>=0.05) {
+            turn=10*(a-b)*(a-b);
+            pc.printf("f");
+        } else if (b-a>=0.1) {
+            turn=-0.1;
+            pc.printf("B");
+        } else if (b-a>=0.05) {
+            turn=-10*(a-b)*(a-b);
+            pc.printf("b");
+        } else {
+            backdrop.stop();
+            backdrop.turn(0);
+            turn=0;
+            pc.printf("s");
+        }
+
+        if(turn*old_turn<0)turn=0;
+        backdrop.turn(turn);
+        pc.printf("%lf",EC_backdrop.getRad());
+        if(pc.readable()) {
+            char sel=pc.getc();
+            if(sel=='z') {
+                pc.printf("z\r\n");
+                tsukami();
+            } else if(sel=='x') {
+                pc.printf("x\r\n");
+                put();
+            } else if(sel=='c') {
+                pc.printf("c\r\n");
+                top();
+            }
+            /*            if(sel=='q') {
+                            printf("\r\n");
+                            if(denjiben==0)denjiben=1;
+                            else denjiben=0;
+                        }*/
+            if(sel=='1') {
+                snatch=0;
+                printf("snatch_off\r\n");
+            }
+            if(sel=='2') {
+                snatch=1;
+                printf("snatch_on\r\n");
+            }
+            if(sel=='3') {
+                pass1=0;
+                printf("pass1_off\r\n");
+            }
+            if(sel=='4') {
+                pass1=1;
+                printf("pass1_on\r\n");
+            }
+            if(sel=='5') {
+                pass2=0;
+                printf("pass2_off\r\n");
+            }
+            if(sel=='6') {
+                pass2=1;
+                printf("pass2_on\r\n");
+            }
+            //     if(sel=='a') {
+            //         //pc.printf("x\r\n");
+            //         //put();
+            //     }
+        }
+    }
+}
\ No newline at end of file