Diff: main.cpp

Revision:

56:a7bd860b85b6

Parent:

55:2dd2f161ebaf

Child:

57:68df771fd8a1

--- a/main.cpp	Sat Sep 16 05:59:00 2017 +0000
+++ b/main.cpp	Sat Sep 16 07:56:19 2017 +0000
@@ -1,3 +1,4 @@
+//index_include
 #include "mbed.h"
 #include "math.h"
 #include "bit_test.h"
@@ -15,6 +16,7 @@
 #include "cylinder.h"
 #include "varEvent.h"
 
+//index_define
 #define pc_baud     460800
 #define sbdbt_tx    p13
 #define sbdbt_rx    p14
@@ -55,8 +57,9 @@
 #define pin_cylinder_reload p15
 #define pin_sbdbt_pairing p12
 #define pin_sbdbt_indicator p11
-#define cylinder_pos_max 4
+#define cylinder_pos_max 6
 
+//index_setupPin
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
@@ -87,6 +90,7 @@
 DigitalOut sbdbt_indigator(pin_sbdbt_indicator);
 varEvent cylinder_event;
 
+//index_setupFunction
 void setup();
 void output();
 void motor_cal();
@@ -99,6 +103,7 @@
 float shoulder_right_cal();
 float shoulder_left_cal();
 
+//index_setupVariable
 //output
 float yaw, target_yaw;
 //cylinder_origin
@@ -106,7 +111,7 @@
 //cylinder
 float cylinder_pwm;
 int cylinder_pos_num = 0;
-float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,700.0};
+float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,450.0,580.0,700.0};
 
 int main()
 {
@@ -269,10 +274,18 @@
 //mecanum
 void motor_cal()
 {
+    static float out_right_x;
     yaw = bno055.getYawRad();
     target_yaw = yaw;
-    yaw_pid.cal(target_yaw, yaw, output_period);
-    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, sbdbt.right_x, bno055.getYawRad());
+    yaw_pid.cal(target_yaw, yaw, output_period); 
+    
+    //後進時に方向が逆転するため
+    if(sbdbt.right_y < 0.0){
+        out_right_x = -1.0*sbdbt.right_x;
+    }else{
+        out_right_x = sbdbt.right_x;
+    }
+    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, bno055.getYawRad());
 //    pc.printf("%f\t data %f\t %f\t %f\t %f\t\r\n", bno055.getYawRad(), sbdbt.left_x, sbdbt.left_y, mecanum.VX(), mecanum.VY());
 }
 void output()
@@ -283,10 +296,8 @@
     servo_out();
     cylinder_origin();
     //boost();
-
+    sbdbt_indigator = sbdbt.get_pairingState();
     //led1 = sbdbt.get_pairingState();
-    sbdbt_indigator = sbdbt.get_pairingState();
-
     if(sbdbt.up)cylinder_origin_flag = 1;
     if(sbdbt.left)bno055.yaw_origin();
     cylinder_reload.cyclic(sbdbt.down);