Diff: main.cpp

Revision:

71:7d684ff43ddd

Parent:

69:b2c8a0c5662c

Child:

72:7d6177047823

--- a/main.cpp	Sun Sep 24 08:37:15 2017 +0000
+++ b/main.cpp	Tue Sep 26 08:39:55 2017 +0000
@@ -15,51 +15,69 @@
 #include "cyclic_io.h"
 #include "cylinder.h"
 #include "event_var.h"
-#include "servo.h"
+//#include "servo.h"
 
 
 //index_define
-#define pc_baud     460800
-#define sbdbt_tx    p13
-#define sbdbt_rx    p14
-#define sbdbt_baud  115200
-#define rs422_tx    p28
-#define rs422_rx    p27
-#define rs422_baud  115200
-#define output_period   0.015
+#define SBDBT_TX    p13
+#define SBDBT_RX    p14
+#define SBDBT_STO p12
+#define RS422_TX    p28
+#define RS422_RX    p27
+#define CONTROL_INDICATOR p11
+
+namespace BAUD {
+    const int PC    = 460800;
+    const int SBDBT = 115200;
+    const int RS422 = 115200;
+}
+
+namespace CYCLE {
+    const float OUTPUT  = 0.015;
+}
+
+namespace MECANUM {
+    const float ACCELERATION = 15;
+}
+
+namespace YAW {
+    const float KP = 0.01;
+    const float KI = 0.01;
+    const float KD = 0.01;
+}
+
+namespace SHOT {
+    const float ENCORDER_KP = 0.0400;
+    const float ENCORDER_KI = 0.0001;
+    const float ENCORDER_KD = 0.0003;
+    const int CYLINDER_POSITION_MAX = 6;
+}
+
+
 #define nucleo_num  6
-#define pi          3.141592
 #define n1_id       3
 #define n2_id       4
 #define n3_id       5
 #define n4_id       6
 #define n5_id       7
 #define n6_id       8
-#define yaw_Kp      0.01
-#define yaw_Ki      0.01
-#define yaw_Kd      0.01
-#define acceleration    15  //25
-#define pin_cylinder_on     p18
-#define pin_cylinder_off    p17
-#define pin_interrupt_cylinder_min       p29
-#define encoder_A       p21
-#define encoder_B       p22
-#define enc_Kp      0.0400
-#define enc_Ki      0.0001
-#define enc_Kd      0.0003
+
+#define SHOT_CYLINDER_ON     p18
+#define SHOT_CYLINDER_OFF    p17
+#define SHOT_INTERRUPT       p29
+#define SHOT_ENCODER_A       p21
+#define SHOT_ENCODER_B       p22
+#define RELOAD_CYLINDER p15
+
 #define mecanum_power   1.0
 #define sword_power     1.0
 #define sholder_power   1.0
-#define pin_interrupt_sholderright_max  p19 //p21
-#define pin_interrupt_sholderright_min  p20 //p22
-#define pin_interrupt_sholderleft_max   p7
-#define pin_interrupt_sholderleft_min   p8
+#define SHOULDER_RIGHT_INTERRUPT_MAX  p19 //p21
+#define SHOULDER_RIGHT_INTERRUPT_MIN  p20 //p22
+#define SHOULDER_LEFT_INTERRUPT_MAX   p7
+#define SHOULDER_LEFT_INTERRUPT_MIN   p8
 //#define pin_servo p21
-#define servo_reload_time 1.0
-#define pin_cylinder_reload p15
-#define pin_sbdbt_pairing p12
-#define pin_sbdbt_indicator p11
-#define cylinder_pos_max 6
+//#define servo_reload_time 1.0
 
 
 //index_setupPin
@@ -67,10 +85,10 @@
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
-DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
+DigitalIn interrupt_cylinder_min(SHOT_INTERRUPT);
 Serial pc(USBTX,USBRX);
-Rs422 rs422(rs422_tx, rs422_rx);
-Sbdbt sbdbt(sbdbt_tx, sbdbt_rx, pin_sbdbt_pairing);
+Rs422 rs422(RS422_TX, RS422_RX);
+Sbdbt sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO);
 Ticker output_timer;
 Mecanum mecanum;
 Bno055 bno055;
@@ -79,19 +97,19 @@
 Accelerator v2;
 Accelerator v3;
 Accelerator v4;
-DigitalOut cylinder_on(pin_cylinder_on);
-DigitalOut cylinder_off(pin_cylinder_off);
+DigitalOut cylinder_on(SHOT_CYLINDER_ON);
+DigitalOut cylinder_off(SHOT_CYLINDER_OFF);
 CyclicVar sword;
 CyclicVar cyclic_cylinder_position;
-DigitalIn interrupt_sholderright_min(pin_interrupt_sholderright_min);
-DigitalIn interrupt_sholderright_max(pin_interrupt_sholderright_max);
-DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min);
-DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max);
-Encoder enc_cylinder(encoder_A,encoder_B);
+DigitalIn interrupt_sholderright_min(SHOULDER_RIGHT_INTERRUPT_MIN);
+DigitalIn interrupt_sholderright_max(SHOULDER_RIGHT_INTERRUPT_MAX);
+DigitalIn interrupt_sholderleft_min(SHOULDER_LEFT_INTERRUPT_MIN);
+DigitalIn interrupt_sholderleft_max(SHOULDER_LEFT_INTERRUPT_MAX);
+Encoder enc_cylinder(SHOT_ENCODER_A,SHOT_ENCODER_B);
 CyclicVar cyclic_servo;
-CyclicIo cylinder_reload(pin_cylinder_reload);
+CyclicIo cylinder_reload(RELOAD_CYLINDER);
 //PwmOut servo(pin_servo);
-DigitalOut sbdbt_indigator(pin_sbdbt_indicator);
+DigitalOut sbdbt_indigator(CONTROL_INDICATOR);
 eventVar cylinder_event;
 
 
@@ -116,7 +134,8 @@
 //cylinder
 float cylinder_pwm;
 int cylinder_pos_num = 0;
-float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,450.0,580.0,700.0};
+float cylinder_pos[SHOT::CYLINDER_POSITION_MAX] = {0.0,90.0,325.0,450.0,580.0,700.0};
+float cylinder_power = 0.0;
 
 int main()
 {
@@ -138,20 +157,20 @@
     bno055.begin();
     wait(1.0);
     bno055.firstRead();
-    pc.baud(pc_baud);
-    sbdbt.begin(sbdbt_baud);
-    rs422.begin(rs422_baud);
+    pc.baud(BAUD::PC);
+    sbdbt.begin(BAUD::SBDBT);
+    rs422.begin(BAUD::RS422);
     firstmotion();
     cylinder_pos_num = 2;                   //セットアップタイムでの初期装填のため
-    output_timer.attach(&output, output_period);
-    yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd,output_period);
+    output_timer.attach(&output, CYCLE::OUTPUT);
+    yaw_pid.setup(YAW::KP, YAW::KI, YAW::KD, CYCLE::OUTPUT);
     mecanum.setupdeg(bno055.getYawRad());   //基盤が前後逆の場合+180
-    v1.setup(acceleration,output_period);
-    v2.setup(acceleration,output_period);
-    v3.setup(acceleration,output_period);
-    v4.setup(acceleration,output_period);
+    v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
+    v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
+    v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
+    v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
     enc_cylinder.setPpr(100);
-    enc_cylinder.setup(enc_Kp,enc_Ki,enc_Kd,output_period);
+    enc_cylinder.setup(SHOT::ENCORDER_KP, SHOT::ENCORDER_KI, SHOT::ENCORDER_KD, CYCLE::OUTPUT);
     //servo.period(0.020);
 }
 
@@ -213,14 +232,20 @@
 //cylinder_origin_flagを1にすることで動作する
 void cylinder_origin()
 {
-    if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1) {
-        cylinder_origin_flag = 0;
-        enc_cylinder.origin();
-        cylinder_pos_num = 0;
-    } else if(cylinder_origin_flag == 1) {
-        rs422.put(5, -0.8, 0.0);
-    }
 }
+float cylinder_power_cal(){
+        if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1) {
+            cylinder_origin_flag = 0;
+            enc_cylinder.origin();
+            cylinder_pos_num = 0;
+            cylinder_power = -0.0;
+        }else if(cylinder_origin_flag == 1){
+            return -0.8;
+        }else{
+            return -1*enc_cylinder.duty_enableWidth(-10.0,10.0);   
+        }
+}
+
 void cylinder_cal()
 {
     //cylinder ON/OFF
@@ -235,12 +260,12 @@
     cylinder_event.input(sbdbt.right);
     if(cylinder_event.getRise()) {        //cylinder degset
         cylinder_pos_num++;
-        if(cylinder_pos_num >= cylinder_pos_max) {
+        if(cylinder_pos_num >= SHOT::CYLINDER_POSITION_MAX) {
             //cylinder_pos_num = 0;
             cylinder_origin_flag=1;
         }
     }
-    enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //set cylinder_tergetPos
+    enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],CYCLE::OUTPUT); //set cylinder_tergetPos
 
     pc.printf("terget\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.pulse(),enc_cylinder.duty());
 }
@@ -333,7 +358,7 @@
                 break;
             case 2:
                 //rs422.put(id[counter], sbdbt.right_y, 0.0);
-                rs422.put(id[counter], limitf((-1*enc_cylinder.duty_enableWidth(-10.0,10.0)),1.0,-1.0), 0.0);
+                rs422.put(id[counter], limitf(cylinder_power_cal(),1.0,-1.0), 0.0);
                 counter ++;
                 break;
             case 3: