Diff: main.cpp

Revision:

7:ef72ec7390c2

Parent:

6:259deb365510

Child:

8:244c057d195c

--- a/main.cpp	Sat Aug 05 07:10:23 2017 +0000
+++ b/main.cpp	Thu Aug 31 12:10:40 2017 +0000
@@ -1,12 +1,13 @@
 #include "mbed.h"
 #include "omni.h"
-#include "MotorDriverController.h"
+#include "ikarashiMDC.h"
 #include "pin_config.h"
 #include "FEP.h"
 
 #define DEBUG 
-MDC motor(i2c_sda,i2c_scl);
-Omni omni(4, 135);
+Omni omni(4);
+Serial serial(PC_12,PD_2);
+DigitalOut serialcontrol(D2);
 FEP fep(PC_10,PC_11,200);
 DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
 DigitalOut attack[2] = {PA_10,PC_4};
@@ -14,8 +15,18 @@
 #ifdef DEBUG
     Serial pc(USBTX,USBRX,115200);
 #endif 
+ikarashiMDC wheels[] {
+    ikarashiMDC(&serialcontrol,0,0,SM,&serial),
+    ikarashiMDC(&serialcontrol,1,0,SM,&serial),
+    ikarashiMDC(&serialcontrol,1,4,SM,&serial),
+    ikarashiMDC(&serialcontrol,0,4,SM,&serial)
+};
+ikarashiMDC lift[] {
+    ikarashiMDC(&serialcontrol,1,2,SM,&serial)
+};
 void init()
 {
+    omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
     for(int i = 0;i<4;i++) {
         leds[i] = 0;
     }
@@ -23,13 +34,8 @@
 void AllActuatorStop()
 {
 #ifdef DEBUG
-    pc.printf("All actuators are stop\n");
+    pc.printf("All actuators stop\n");
 #endif
-    omni.stop();
-    for(int j = 0; j < 4; j++) {
-        motor.write(7,j,0);
-        motor.write(6,j,0);
-    }
     for(int i=0;i<1;i++)
     {
         attack[i]=0;
@@ -42,12 +48,8 @@
     char data[10] = { 0 };
     int  i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 };
     uint8_t fep_temp;
-    double polarVector[2];
-    double deg = 360;
+    double stick[4] = { 0 };
     double pwm = 0.0;
-    for(int j = 0; j < 4; j++) {
-        motor.write(7,j,0);
-    }
     
     while(1) {
         fep_temp=fep.read(data,6);
@@ -61,6 +63,7 @@
             leds[0] = 0;
             tem[0] = data[4];
             tem[1] = data[5];
+            for(i = 0; i < 4; i++) stick[i] = -1*(data[i]-128)/128.0;
             for(i = 0; i < 7; i++) {
                 Button1[i] = tem[0] % 2;
                 tem[0] /= 2;
@@ -102,42 +105,9 @@
             error_val++;
         }
         if(error_val < 4) {
-            polarVector[1] = 0.6;
-            //移動方向
-            if(Button1[2] == 0 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 1) {
-                deg = 270;
-                leds[1] = 0;
-            }else if(Button1[2] == 1 && Button1[3] == 0 && Button1[4] == 1 && Button1[5] == 1) {
-                deg = 180;
-                leds[1] = 0;
-            }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 0 && Button1[5] == 1) {
-                deg = 90;
-                leds[1] = 0;
-            }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 0) {
-                deg = 0;
-                leds[1] = 0;
-            }else {
-                deg = 360;
-                polarVector[1] = 0;
-                leds[1] = 1;
-            }
-            polarVector[0] = deg;
-            if(data[2] < 50) {
-                omni.computePolar(polarVector, 0.15);
-                leds[2] = 1;
-                leds[3] = 0;
-            } else if(data[2] > 200) {
-                omni.computePolar(polarVector, -0.15);
-                leds[2] = 0;
-                leds[3] = 1;
-            } else {
-                omni.computePolar(polarVector, 0);
-                leds[2] = 0;
-                leds[3] = 0;
-            }
-            
-            for(int j = 0; j < 4; j++) {
-                motor.write(7,j,omni.getOutput(j));
+            omni.computeXY(data[3],data[2],data[0]);           
+            for(int i = 0; i < 4; i++) {
+                wheels[i].setSpeed(omni.getOutput(i));
             }
             //昇降
             if(Button1[0] == 1 && Button1[1] == 0) {
@@ -147,7 +117,7 @@
             } else {
                 pwm = -0.0;
             }
-            motor.write(6,0,pwm);
+            lift[0].setSpeed(pwm);
             //アーム攻撃(toggle)
             if((Button2[1]==0)&&(airFlag1 == 0))
             {