Diff: main.cpp

Revision:

10:73148221684e

Parent:

9:8f9607783d2d

Child:

11:a2e3d11f5750

--- a/main.cpp	Fri Sep 01 08:19:08 2017 +0000
+++ b/main.cpp	Mon Sep 04 02:59:46 2017 +0000
@@ -4,19 +4,25 @@
 #include "pin_config.h"
 #include "FEP.h"
 #include "controller.h"
+#include "PID.h"
+#include "R1370.h"
 
-#define stickrange 0.25
 #define DEBUG 
+
+#define KP 5
+#define KI 0 
+#define KD 0
+#define RATE 0.01
 Omni omni(4);
 Serial RS485(RS485_TX,RS485_RX,38400);
 Serial pc(USBTX,USBRX,115200);
 Controller con(FEP_TX,FEP_RX,200);
-DigitalOut RS485control(D2);
-DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
+DigitalOut RS485control(PA_4);
+//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};
-
- 
+R1370 R1370(R1370_TX,R1370_RX);
+PID pid(KP,KI,KD,RATE); 
 ikarashiMDC wheels[] {
     ikarashiMDC(&RS485control,0,0,SM,&RS485),
     ikarashiMDC(&RS485control,1,3,SM,&RS485),
@@ -28,6 +34,7 @@
 };
 void init()
 {
+    pc.printf("Hello\n");
     int i;
     for(i = 0;i < 4;i++) {
         wheels[i].braking = true;
@@ -35,9 +42,18 @@
     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;
+        //leds[i] = 0;
     }
 }
+void pidInit()
+{
+    pid.reset();
+    pid.setInputLimits(-180.0,180.0);
+    pid.setOutputLimits(-1.0,1.0);
+    pid.setMode(1);
+    pid.setSetPoint(0.0);
+    pid.setBias(0.0);
+}
 void AllActuatorStop()
 {
 #ifdef DEBUG
@@ -56,22 +72,24 @@
     uint8_t fep_temp;
     double pwm = 0.0;
     init();
-    while(1) {
-        if(con.receiveState()==0) {
+    pidInit();
+    while(1) 
+    {
+        if(con.receiveState()==0) 
+        {
             error_val = 0;
-            leds[0] = 0;
-        } else if(fep_temp==FEP_NO_RESPONSE) {
-            leds[0] = 1;
-            continue;
-        } else {
-            leds[0] = 1;
-            error_val++;
-        }
-        if(error_val < 4) {
-            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));
+            //leds[0] = 0;
+            if((R1370.reciveState() == 0)&&(error_val < 4))
+            {
+                //omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0)/-2.0);
+                pid.setProcessValue(R1370.getAngle());
+                pc.printf("%lf    %lf     ",R1370.getAngle(),-1*pid.compute());
+                omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());           
+                for(int i = 0; i < 4; i++) {
+                    pc.printf("%lf,",omni.getOutput(i));
+                    wheels[i].setSpeed(omni.getOutput(i));
+                }
+                pc.printf("\n");
             }
             //昇降
             if(con.getButton1(0) == 1 && con.getButton1(1) == 0) {
@@ -120,13 +138,6 @@
                 angle[0]=0;
                 angle[1]=0;    
             }
-#ifdef DEBUG
-            printf("Status of airFlags %d,%d\r\n",airStatus1,airStatus2);
-#endif
-        } else if((fep_temp==FEP_NO_RESPONSE)&&(error_val > 4)){
-            AllActuatorStop();
-        } else {
-            AllActuatorStop();
         }
     }
 }