Diff: main.cpp

Revision:

15:d4ff132a616d

Parent:

14:1d9ae3128002

Child:

16:7eaf3343f45e

--- a/main.cpp	Tue Sep 05 00:41:30 2017 +0000
+++ b/main.cpp	Thu Sep 14 18:22:29 2017 +0900
@@ -1,5 +1,5 @@
+#include "omni.h"
 #include "mbed.h"
-#include "omni.h"
 #include "ikarashiMDC.h"
 #include "pin_config.h"
 #include "FEP.h"
@@ -7,23 +7,22 @@
 #include "PID.h"
 #include "R1370.h"
 
-#define DEBUG 
+#define DEBUG
 
-#define KP 2.5
-#define KI 0 
-#define KD 0.05
+#define KP 0.0//1.5 //2.5
+#define KI 0.0 //0s
+#define KD 0.0 //0.05
 #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(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};
+DigitalOut attack[2] = {air1_0,air1_1};
+DigitalOut angle[2] = {air2_0,air2_1};
 DigitalOut powerSw(powerSW);
 R1370 R1370(R1370_TX,R1370_RX);
-PID pid(KP,KI,KD,RATE); 
+PID pid(KP,KI,KD,RATE);
 ikarashiMDC wheels[] {
     ikarashiMDC(&RS485control,0,0,SM,&RS485),
     ikarashiMDC(&RS485control,1,3,SM,&RS485),
@@ -40,11 +39,8 @@
     for(i = 0;i < 4;i++) {
         wheels[i].braking = true;
     }
+    omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
     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;
-    }
 }
 void pidInit()
 {
@@ -55,99 +51,98 @@
     pid.setSetPoint(0.0);
     pid.setBias(0.0);
 }
-void AllActuatorStop()
+void gyroInit()
 {
-#ifdef DEBUG
-    pc.printf("All actuators stop\n");
-#endif
-    for(int i=0;i<1;i++)
-    {
-        attack[i]=0;
-        angle[i]=0;
-    }
+    R1370.update();
+    R1370.setZeroPoint(R1370.getAngle());
 }
 int main()
-{   
+{
     bool airFlag1 = 0,airFlag2 = 0,airStatus1 = 0,airStatus2 = 0,pidflag = 0;
     int error_val = 0,i = 0;
-    double pwm = 0.0;
+    float nowAngle = 0;//,oldAngle = 0;
     init();
     pidInit();
-    while(1) 
+    gyroInit();
+    while(1)
     {
-        if((con.receiveState()==0)&&(R1370.update()==0)) 
+        if(con.receiveState()==0)
         {
-            if(con.getButton1(6)==0) {
-                powerSw = 0;
-            } else {
-                powerSw = 1;
-            }
-            error_val = 0;
-            pc.printf("%lf    %lf     ",R1370.getAngle(),-1*pid.compute());
-            if((con.getStick(0)==0)&&(pidflag==1)) {
-                pidflag = 0;
-                pid.setSetPoint(R1370.getAngle());;           
+            if(R1370.update()==0)
+            {
+                if(con.getButton1(6)==0) {
+                    powerSw = 0;
+                } else {
+                    powerSw = 1;
+                }
+                error_val = 0;
+                nowAngle = R1370.getAngle();
+                if((con.getStick(0)==0)&&(pidflag==1)) {
+                    pc.printf("zeroPointSet\n");
+                    pidflag = 0;
+                    R1370.setZeroPoint(nowAngle);
+                }else if(con.getStick(0)!=0) {
+                    pidflag = 1;
+                    omni.computeXY(-1*con.getStick(2),con.getStick(3),(-1*con.getStick(0))/3.0);
+                }else if((con.getStick(0)==0)&&(pidflag==0)) {
+                    pid.setProcessValue(R1370.getDeviation(nowAngle));
+                    omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());
+                }
+                pc.printf("Now%lf:D%lf:set%lf:PID%lf   ",R1370.getAngle(),R1370.getDeviation(nowAngle),R1370.getZeroPoint(),-1*pid.compute());
+                for(i = 0; i < 4; i++) {
+                    pc.printf("%lf,",omni.getOutput(i));
+                    wheels[i].setSpeed(omni.getOutput(i));
+                }
+                pc.printf("\n");
+                //昇降
+                lift[0].setSpeed(con.getStick(1));
+                //アーム攻撃(toggle)
+                pc.printf("%d\n",con.getButton2(1));
+                if((con.getButton2(1)==0)&&(airFlag1 == 0))
+                {
+                    if(airStatus1==1) {
+                        attack[0]=0;
+                        attack[1]=1;
+                        airFlag1=1;
+                        airStatus1=0;
+                    }else if(airStatus1==0) {
+                        attack[0]=1;
+                        attack[1]=0;
+                        airFlag1=1;
+                        airStatus1=1;
+                    }
+                }else if(con.getButton2(1)==1){
+                    airFlag1=0;
+                    attack[0]=0;
+                    attack[1]=0;
+                }
+                //アーム角度(toggle)
+                if((con.getButton2(3)==0)&&(airFlag2 == 0))
+                {
+                    if(airStatus2==1) {
+                        angle[0]=0;
+                        angle[1]=1;
+                        airFlag2=1;
+                        airStatus2=0;
+                    }else if(airStatus2==0) {
+                        angle[0]=1;
+                        angle[1]=0;
+                        airFlag2=1;
+                        airStatus2=1;
+                    }
+                }else if(con.getButton2(3)==1){
+                    airFlag2=0;
+                    angle[0]=0;
+                    angle[1]=0;
+                }
             }else {
-                pidflag = 1;
-                omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0));
-            }
-            pid.setProcessValue(R1370.getAngle()); 
-            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) {
-                pwm = -0.9;
-            } else if(con.getButton1(0) == 0 && con.getButton1(1) == 1) {
-                pwm = 0.9;
-            } else {
-                pwm = 0.0;
-            }
-            lift[0].setSpeed(pwm);
-            //アーム攻撃(toggle)
-            if((con.getButton2(1)==0)&&(airFlag1 == 0))
-            {
-                if(airStatus1==1) {
-                    attack[0]=0;
-                    attack[1]=1;
-                    airFlag1=1;
-                    airStatus1=0;
-                }else if(airStatus1==0) {
-                    attack[0]=1;
-                    attack[1]=0;
-                    airFlag1=1;
-                    airStatus1=1;
-                }                   
-            }else if(con.getButton2(1)==1){
-                airFlag1=0;
-                attack[0]=0;
-                attack[1]=0;    
-            }
-            //アーム角度(toggle)
-            if((con.getButton2(3)==0)&&(airFlag2 == 0))
-            {
-                if(airStatus2==1) {
-                    angle[0]=0;
-                    angle[1]=1;
-                    airFlag2=1;
-                    airStatus2=0;
-                }else if(airStatus2==0) {
-                    angle[0]=1;
-                    angle[1]=0;
-                    airFlag2=1;
-                    airStatus2=1;
-                }                   
-            }else if(con.getButton2(3)==1){
-                airFlag2=0;
-                angle[0]=0;
-                angle[1]=0;    
+                //pc.printf("gyro_error\n");
             }
         }else {
+            //pc.printf("FEP_error\n");
             error_val++;
-            if(error_val > 10) powerSw = 0;
+            if(error_val > 5) powerSw = 0;
+            else powerSw = 1;
         }
-    wait(RATE);
     }
 }