Diff: main.cpp

Revision:

9:8f9607783d2d

Parent:

8:244c057d195c

Child:

10:73148221684e

--- a/main.cpp	Fri Sep 01 06:38:15 2017 +0000
+++ b/main.cpp	Fri Sep 01 08:19:08 2017 +0000
@@ -3,17 +3,19 @@
 #include "ikarashiMDC.h"
 #include "pin_config.h"
 #include "FEP.h"
+#include "controller.h"
 
 #define stickrange 0.25
 #define DEBUG 
 Omni omni(4);
-Serial RS485(PC_12,PD_2,38400);
+Serial RS485(RS485_TX,RS485_RX,38400);
+Serial pc(USBTX,USBRX,115200);
+Controller con(FEP_TX,FEP_RX,200);
 DigitalOut RS485control(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};
 DigitalOut angle[2] = {PB_3,PB_5};
-Serial pc(USBTX,USBRX,115200);
+
  
 ikarashiMDC wheels[] {
     ikarashiMDC(&RS485control,0,0,SM,&RS485),
@@ -22,17 +24,15 @@
     ikarashiMDC(&RS485control,0,3,SM,&RS485)
 };
 ikarashiMDC lift[] {
-    ikarashiMDC(&RS485control,1,2,SM,&RS485)
+    ikarashiMDC(&RS485control,1,1,SM,&RS485)
 };
 void init()
 {
     int i;
-#ifdef DEBUG
-    pc.printf("Hello\n");
-#endif
     for(i = 0;i < 4;i++) {
         wheels[i].braking = true;
     }
+    lift[0].braking = true;
     omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
     for(i = 0;i<4;i++) {
         leds[i] = 0;
@@ -52,80 +52,38 @@
 int main()
 {   
     bool airFlag1=0,airFlag2=0,airStatus1=0,airStatus2=0;
-    char data[10] = { 0 };
-    int  i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 };
+    int error_val = 0;
     uint8_t fep_temp;
-    double stick[4] = { 0 };
     double pwm = 0.0;
     init();
     while(1) {
-        fep_temp=fep.read(data,6);
-        if(fep_temp==0) {
-#ifdef DEBUG
-            pc.printf("Recieve succeeded,%s\r\n",data);
-#endif
+        if(con.receiveState()==0) {
             error_val = 0;
             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;
-                if((stick[i]<stickrange)&&(stick[i]>-1*stickrange)) stick[i] = 0;
-            }
-            for(i = 0; i < 7; i++) {
-                Button1[i] = tem[0] % 2;
-                tem[0] /= 2;
-#ifdef DEBUG
-                pc.printf("%d,",Button1[i]);
-#endif
-            }
-            pc.printf("\n");
-            for(i = 0; i < 6; i++) {
-                Button2[i] = tem[1] % 2;
-                tem[1] /= 2;
-
-#ifdef DEBUG
-                pc.printf("%d,",Button2[i]);
-#endif
-            }
-
-#ifdef DEBUG
-            for(i=0;i < 4; i++) {
-                pc.printf("%d,",data[i]);
-            }
-            pc.printf("\r\n");
-#endif 
         } else if(fep_temp==FEP_NO_RESPONSE) {
-
-#ifdef DEBUG
-            pc.printf("No response\r\n");
-#endif
             leds[0] = 1;
             continue;
         } else {
-#ifdef DEBUG
-            pc.printf("Recieve failed\r\n");
-#endif
             leds[0] = 1;
             error_val++;
         }
         if(error_val < 4) {
-            omni.computeXY(stick[2],stick[3],stick[0]/2.0);           
+            omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0)/-2.0);           
             for(int i = 0; i < 4; i++) {
                 pc.printf("%lf,",omni.getOutput(i));
                 wheels[i].setSpeed(omni.getOutput(i));
             }
             //昇降
-            if(Button1[0] == 1 && Button1[1] == 0) {
+            if(con.getButton1(0) == 1 && con.getButton1(1) == 0) {
+                pwm = -0.9;
+            } else if(con.getButton1(0) == 0 && con.getButton1(1) == 1) {
                 pwm = 0.9;
-            } else if(Button1[0] == 0 && Button1[1] == 1) {
-                pwm = -0.9;
             } else {
                 pwm = 0.0;
             }
             lift[0].setSpeed(pwm);
             //アーム攻撃(toggle)
-            if((Button2[1]==0)&&(airFlag1 == 0))
+            if((con.getButton2(1)==0)&&(airFlag1 == 0))
             {
                 if(airStatus1==1) {
                     attack[0]=0;
@@ -138,13 +96,13 @@
                     airFlag1=1;
                     airStatus1=1;
                 }                   
-            }else if(Button2[1]==1){
+            }else if(con.getButton2(1)==1){
                 airFlag1=0;
                 attack[0]=0;
                 attack[1]=0;    
             }
             //アーム角度(toggle)
-            if((Button2[3]==0)&&(airFlag2 == 0))
+            if((con.getButton2(3)==0)&&(airFlag2 == 0))
             {
                 if(airStatus2==1) {
                     angle[0]=0;
@@ -157,7 +115,7 @@
                     airFlag2=1;
                     airStatus2=1;
                 }                   
-            }else if(Button2[3]==1){
+            }else if(con.getButton2(3)==1){
                 airFlag2=0;
                 angle[0]=0;
                 angle[1]=0;