Diff: main.cpp

Revision:

0:46c70e5b719d

Child:

1:5c0b7355adcd

diff -r 000000000000 -r 46c70e5b719d main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 16 07:41:44 2019 +0000
@@ -0,0 +1,77 @@
+#include "mbed.h"           //wheel_test3
+#include "controller.h"
+#include "ikarashiMDC.h"
+#include "R1370.h"
+#include "wheel_an.h"
+#define PI 3.141593
+
+Controller pad(PC_10, PC_11, 208);
+R1370 R1370(D10,D2);
+Serial pc(USBTX, USBRX, 115200);
+Serial serial(PC_6, PC_7, 115200);
+DigitalOut serialcontrol(PD_2);
+
+ikarashiMDC ikarashi[]
+{
+    ikarashiMDC(&serialcontrol,2,0,SM,&serial),
+    ikarashiMDC(&serialcontrol,2,1,SM,&serial),
+    ikarashiMDC(&serialcontrol,2,2,SM,&serial)
+};
+
+Wheelan wheel[]
+{
+    Wheelan(PI * 1 / 6),
+    Wheelan(PI * 5 / 6),
+    Wheelan(PI * 9 / 6)
+};
+
+int main()
+{
+    ikarashi[0].braking = true;
+    int b[11];
+    double rad[2],dis[2];
+    double value[3];
+    while(1){
+        if(pad.receiveState()){
+            for(int i = 0; i < 13; i++){
+                b[i] = pad.getButton1(i);
+            }
+            for(int i = 0; i < 2; i++){
+                rad[i] = pad.getRadian(i);
+                dis[i] = pad.getNorm(i);
+                rad[i] = PI - rad[i];
+            }
+            R1370.update();
+            pc.printf("degree : %.3lf    ", R1370.getAngle());
+
+            for (int i = 0; i < 3; ++i)
+            {
+                value[i] = wheel[i].an_uc(rad[1], dis[1]);
+            }
+
+            if(b[7] == 1 && b[9] == 0) {
+                value[0] -= 0.3;
+                value[1] -= 0.3;
+                value[2] -= 0.3;
+            } else if (b[9] == 1 && b[7] == 0) {
+                value[0] += 0.3;
+                value[1] += 0.3;
+                value[2] += 0.3;
+            }
+
+            for (int i = 0; i < 3; ++i)
+            {
+                pc.printf("%lf ", value[i]);
+                ikarashi[i].setSpeed(value[i]);
+            }
+            pc.printf("\n\r");
+
+        }else{
+            pc.printf("error\n\r");
+            for (int i = 0; i < 3; i++)
+            {
+                ikarashi[i].setSpeed(0);
+            }
+        }
+    }
+}
\ No newline at end of file