Revision 3:4fe8e6e455d0, committed 2019-06-11

Comitter:

piroro4560

Date:

Tue Jun 11 09:10:36 2019 +0000

Parent:

2:8d16db0d55b7

Commit message:

dekiterukamo

Changed in this revision

diff -r 8d16db0d55b7 -r 4fe8e6e455d0 main.cpp
--- a/main.cpp	Tue May 21 08:39:27 2019 +0000
+++ b/main.cpp	Tue Jun 11 09:10:36 2019 +0000
@@ -6,7 +6,7 @@
 #include "PID.h"
 #define PI 3.141593
 
-PID pid1(4.5,100.0,0.00000,0.01);
+PID pid1(5.0,0.001,0.0001,0.01);
 OmniWheel omni(4);
 Controller pad(PC_10, PC_11, 208);
 R1370 R1370(PB_6,PA_10);
@@ -29,8 +29,13 @@
 {
 //    reset = 1;
     motor[0].braking = true;
-    int b[11];
-    double rad[2], dis[2], value[3], X, Y, set_value, original_angle, now_angle, start_angle;
+    int b[11], b2[11];
+    double rad[2], dis[2], value[3], X, Y, original_angle, now_angle, start_angle, spin_power;
+    /*
+     * original_angle : 
+     * now_angle      : 
+     * start_angle    : 
+     */
     pid1.setInputLimits(-180,180);
     pid1.setOutputLimits(-0.3,0.3);
     pid1.setBias(0);
@@ -43,7 +48,7 @@
     while(1){
         if(pad.receiveState()){
             for(int i = 0; i < 13; i++){
-                b[i] = pad.getButton1(i);
+                b[i] = 1 - pad.getButton1(i);
             }
             for(int i = 0; i < 2; i++){
                 rad[i] = pad.getRadian(i);
@@ -53,35 +58,24 @@
             X = dis[0] * cos(rad[0]);
             Y = dis[0] * sin(rad[0]);
             R1370.update();
-            /*
-            ここまでテンプル
-            */
+            /*ここまでテンプル*/
             start_angle = R1370.getAngle();
             /*
             旋回
             */
-            if(b[7]==0)set_value++;
-            if(b[9]==0)set_value--;
-            set_value %= 4;
-            switch(((set_value & 4) + 4) & 4){
-                case 1: original_angle = 90;break;
-                case 2: original_angle = 179;break;
-                case 3: original_angle = -90;break;
-                default: original_angle = 0;break;
             /*
             リセット
             */
-            if(b[6] == 0){
-                original_angle=angle;
+            if(b[6] == 1){
+                original_angle=start_angle;
             }
             now_angle = start_angle - original_angle;
-            if(now_angle < -180)now_angle = now_angle + 360;
             pid1.setProcessValue(now_angle);
-            double spin_power = pid1.compute();
+            spin_power = pid1.compute();
             omni.computeXY(X,Y,-spin_power);
             pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||\n\r",X, Y, start_angle, now_angle, spin_power);
             for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]);
-            
+            for(int i = 0; i < 11; ++i)b2[i] = b[i];
         }else{
             pc.printf("error\n\r");
             for (int i = 0; i < 4; i++)motor[i].setSpeed(0);