Diff: main.cpp

Revision:

2:8d16db0d55b7

Parent:

1:5c0b7355adcd

Child:

3:4fe8e6e455d0

diff -r 5c0b7355adcd -r 8d16db0d55b7 main.cpp
--- a/main.cpp	Thu May 16 08:43:30 2019 +0000
+++ b/main.cpp	Tue May 21 08:39:27 2019 +0000
@@ -1,42 +1,45 @@
-#include "mbed.h"           //wheel_test3
+#include "mbed.h"           //wheel_test4
 #include "controller.h"
 #include "ikarashiMDC.h"
 #include "R1370.h"
+#include "omni_wheel.h"
+#include "PID.h"
 #define PI 3.141593
-#include"omni_wheel.h"
-#include"PID.h"
 
-PID pid1(3.0,3.0,0.000005,0.01);
-OmniWheel omni(3);
+PID pid1(4.5,100.0,0.00000,0.01);
+OmniWheel omni(4);
 Controller pad(PC_10, PC_11, 208);
 R1370 R1370(PB_6,PA_10);
 Serial pc(USBTX, USBRX, 115200);
 Serial serial(PC_6, PC_7, 115200);
 DigitalOut serialcontrol(D10);
+//DigitalOut reset(PC_9);
+
 
 ikarashiMDC motor[]=
 {
     ikarashiMDC(&serialcontrol,2,0,SM,&serial),
     ikarashiMDC(&serialcontrol,2,1,SM,&serial),
-    ikarashiMDC(&serialcontrol,2,2,SM,&serial)
+    ikarashiMDC(&serialcontrol,2,2,SM,&serial),
+    ikarashiMDC(&serialcontrol,2,3,SM,&serial),
 };
 
+
 int main()
 {
-    pid1.setInputLimits(-90,90);
-    pid1.setOutputLimits(-0.5,0.5);
+//    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;
+    pid1.setInputLimits(-180,180);
+    pid1.setOutputLimits(-0.3,0.3);
     pid1.setBias(0);
     pid1.setMode(1);
-    pid1.setSetPoint(0);
-    omni.wheel[0].setRadian(PI * 1.0 / 6.0);
-    omni.wheel[1].setRadian(PI * 5.0 / 6.0);
-    omni.wheel[2].setRadian(PI * 9.0 / 6.0);
-  
-    motor[0].braking = true;
-    int b[11];
-    double rad[2],dis[2];
-    double value[3];
-    pc.printf("saaa");
+    pid1.setSetPoint(0);////
+    omni.wheel[0].setRadian(PI * 1.0 / 4.0);
+    omni.wheel[1].setRadian(PI * 3.0 / 4.0);
+    omni.wheel[2].setRadian(PI * 5.0 / 4.0);
+    omni.wheel[3].setRadian(PI * 7.0 / 4.0);
     while(1){
         if(pad.receiveState()){
             for(int i = 0; i < 13; i++){
@@ -47,23 +50,41 @@
                 dis[i] = pad.getNorm(i);
                 rad[i] = PI - rad[i];
             }
-            double X = dis[0]*cos(rad[0]);
-            double Y = dis[0]*sin(rad[0]);
+            X = dis[0] * cos(rad[0]);
+            Y = dis[0] * sin(rad[0]);
             R1370.update();
-            double angle = R1370.getAngle();
-            pid1.setProcessValue(angle);
+            /*
+            ここまでテンプル
+            */
+            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;
+            }
+            now_angle = start_angle - original_angle;
+            if(now_angle < -180)now_angle = now_angle + 360;
+            pid1.setProcessValue(now_angle);
             double spin_power = pid1.compute();
             omni.computeXY(X,Y,-spin_power);
-            pc.printf("%.3f||%.3f||%.3f||%.3f\n\r",X,Y,angle,spin_power);
-            for(int i = 0; i < 3; i++)
-                motor[i].setSpeed(omni.wheel[i]);
+            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]);
             
         }else{
             pc.printf("error\n\r");
-            for (int i = 0; i < 3; i++)
-            {
-                motor[i].setSpeed(0);
-            }
+            for (int i = 0; i < 4; i++)motor[i].setSpeed(0);
         }
     }
 }
\ No newline at end of file