Diff: main.cpp

Revision:

72:7d6177047823

Parent:

71:7d684ff43ddd

Child:

74:98bad67fde39

Child:

77:17469192b53a

--- a/main.cpp	Tue Sep 26 08:39:55 2017 +0000
+++ b/main.cpp	Wed Sep 27 08:30:19 2017 +0000
@@ -15,8 +15,6 @@
 #include "cyclic_io.h"
 #include "cylinder.h"
 #include "event_var.h"
-//#include "servo.h"
-
 
 //index_define
 #define SBDBT_TX    p13
@@ -26,6 +24,27 @@
 #define RS422_RX    p27
 #define CONTROL_INDICATOR p11
 
+#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 SHOULDER_RIGHT_INTERRUPT_MAX  p19
+#define SHOULDER_RIGHT_INTERRUPT_MIN  p20
+#define SHOULDER_LEFT_INTERRUPT_MAX   p7
+#define SHOULDER_LEFT_INTERRUPT_MIN   p8
+
+#define nucleo_num  6
+#define n1_id       3
+#define n2_id       4
+#define n3_id       5
+#define n4_id       6
+#define n5_id       7
+#define n6_id       8
+
+//index_namespace_const_variable
 namespace BAUD {
     const int PC    = 460800;
     const int SBDBT = 115200;
@@ -53,246 +72,198 @@
     const int CYLINDER_POSITION_MAX = 6;
 }
 
+namespace POWER {
+    const float MECANUM = 1.0;
+    const float SWORD   = 1.0;
+    const float SHOLDER = 1.0;
+}
 
-#define nucleo_num  6
-#define n1_id       3
-#define n2_id       4
-#define n3_id       5
-#define n4_id       6
-#define n5_id       7
-#define n6_id       8
+//index_namespace_variable
+namespace yaw{
+    float now;
+    float target;
+}
 
-#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
+namespace shot{
+    int   cylinderOriginFlag = 0;
+    int   cylinderPosNum = 0;
+    float cylinderPos[SHOT::CYLINDER_POSITION_MAX] = {0.0,90.0,325.0,450.0,580.0,700.0};
+    float cylinderPwm = 0;
+    float cylinderPower = 0.0;
+}
 
-#define mecanum_power   1.0
-#define sword_power     1.0
-#define sholder_power   1.0
-#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
-
+//index_Function
+void    setup();
+void    output();
+void    motorCalculation();
+//void  motorBoost();
+float   shotCylinderPowerCalclation();
+void    shotCylinderCalculation();
+void    firstMotion();
+void    swordCalculation();
+float   shoulderRightCalclation();
+float   shoulderLeftCalclation();
+void    shoulderInterruptIndicator();
 
 //index_setupPin
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
-DigitalIn interrupt_cylinder_min(SHOT_INTERRUPT);
-Serial pc(USBTX,USBRX);
-Rs422 rs422(RS422_TX, RS422_RX);
-Sbdbt sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO);
-Ticker output_timer;
-Mecanum mecanum;
-Bno055 bno055;
-PositionPid yaw_pid;
+DigitalOut  led1(LED1);
+DigitalOut  led2(LED2);
+DigitalOut  led3(LED3);
+DigitalOut  led4(LED4);
+Serial      pc(USBTX,USBRX);
+Rs422       rs422(RS422_TX, RS422_RX);
+Sbdbt       sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO);
+Ticker      outputTimer;
+Mecanum     mecanum;
+Bno055      bno055;
+PositionPid yawPid;
 Accelerator v1;
 Accelerator v2;
 Accelerator v3;
 Accelerator v4;
-DigitalOut cylinder_on(SHOT_CYLINDER_ON);
-DigitalOut cylinder_off(SHOT_CYLINDER_OFF);
-CyclicVar sword;
-CyclicVar cyclic_cylinder_position;
-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(RELOAD_CYLINDER);
-//PwmOut servo(pin_servo);
-DigitalOut sbdbt_indigator(CONTROL_INDICATOR);
-eventVar cylinder_event;
-
+eventVar    shotCylinderEvent;
+CyclicVar   shotPositionCyclic;
+DigitalIn   shotInterrupt(SHOT_INTERRUPT);
+DigitalOut  shotCylinderOn(SHOT_CYLINDER_ON);
+DigitalOut  shotCylinderOff(SHOT_CYLINDER_OFF);
+Encoder     shotEncoder(SHOT_ENCODER_A,SHOT_ENCODER_B);
+DigitalIn   shoulderRightInterruptMin(SHOULDER_RIGHT_INTERRUPT_MIN);
+DigitalIn   shoulderRightInterruptMax(SHOULDER_RIGHT_INTERRUPT_MAX);
+DigitalIn   shoulderLeftInterruptMin(SHOULDER_LEFT_INTERRUPT_MIN);
+DigitalIn   shoulderLeftInterruptMax(SHOULDER_LEFT_INTERRUPT_MAX);
+CyclicVar   swordSwingCyclic;
+CyclicIo    reloadCylinder(RELOAD_CYLINDER);
+DigitalOut  controlIndigator(CONTROL_INDICATOR);
 
-//index_setupFunction
-void setup();
-void output();
-void motor_cal();
-void cylinder_cal();
-void boost();
-void cylinder_origin();
-void firstmotion();
-void sword_cal();
-void servo_origin();
-float shoulder_right_cal();
-float shoulder_left_cal();
-
-//index_setupVariable
-//output
-float yaw, target_yaw;
-//cylinder_origin
-int cylinder_origin_flag = 0;
-//cylinder
-float cylinder_pwm;
-int cylinder_pos_num = 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()
 {
     setup();
     while(1) {
-        led1 = interrupt_sholderleft_max;
-        led2 = interrupt_sholderleft_min;
-        led3 = interrupt_sholderright_max;
-        led4 = interrupt_sholderright_min;
-        
-        //pc.printf("cylinder pos : %f\r\n",enc_cylinder.deg());
+        //pc.printf("cylinder pos : %f\r\n",shotEncoder.deg());
         //pc.printf("riseState %d  :  fallState %d\r\n",event.getRise(),event.getFall());
     }
 }
 
 void setup()
 {
-    wait(2.0);
+    wait(1.0);
     bno055.begin();
     wait(1.0);
     bno055.firstRead();
     pc.baud(BAUD::PC);
     sbdbt.begin(BAUD::SBDBT);
     rs422.begin(BAUD::RS422);
-    firstmotion();
-    cylinder_pos_num = 2;                   //セットアップタイムでの初期装填のため
-    output_timer.attach(&output, CYCLE::OUTPUT);
-    yaw_pid.setup(YAW::KP, YAW::KI, YAW::KD, CYCLE::OUTPUT);
-    mecanum.setupdeg(bno055.getYawRad());   //基盤が前後逆の場合+180
+    firstMotion();
+    shot::cylinderPosNum = 1;                   //セットアップタイムでの初期装填のため
+    outputTimer.attach(&output, CYCLE::OUTPUT);
+    yawPid.setup(YAW::KP, YAW::KI, YAW::KD, CYCLE::OUTPUT);
+    mecanum.setupdeg(bno055.getYawRad());       //基盤が前後逆の場合+180
     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(SHOT::ENCORDER_KP, SHOT::ENCORDER_KI, SHOT::ENCORDER_KD, CYCLE::OUTPUT);
-    //servo.period(0.020);
+    shotEncoder.setPpr(100);
+    shotEncoder.setup(SHOT::ENCORDER_KP, SHOT::ENCORDER_KI, SHOT::ENCORDER_KD, CYCLE::OUTPUT);
+}
+
+//index_sword
+void shoulderInterruptIndicator(){
+    led1 = shoulderLeftInterruptMax;
+    led2 = shoulderLeftInterruptMin;
+    led3 = shoulderRightInterruptMax;
+    led4 = shoulderRightInterruptMin;
 }
 
-//Sword
-float shoulder_right_cal()
-{
-    if(interrupt_sholderright_max==0&&sbdbt.sankaku==1) {
+float shoulderRightCalclation(){
+    if(shoulderRightInterruptMax==0&&sbdbt.sankaku==1) {
+        return 0.0;
+    }
+    if(shoulderRightInterruptMin==0&&sbdbt.batu==1) {
         return 0.0;
     }
-    if(interrupt_sholderright_min==0&&sbdbt.batu==1) {
+    return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
+}
+
+float shoulderLeftCalclation(){
+    if(shoulderLeftInterruptMax==0&&sbdbt.sankaku==1) {
+        return 0.0;
+    }
+    if(shoulderLeftInterruptMin==0&&sbdbt.batu==1) {
         return 0.0;
     }
     return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
 }
 
-float shoulder_left_cal()
-{
-    if(interrupt_sholderleft_max==0&&sbdbt.sankaku==1) {
-        return 0.0;
-    }
-    if(interrupt_sholderleft_min==0&&sbdbt.batu==1) {
-        return 0.0;
-    }
-    return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
-}
-void sword_cal()
-{
-    sword.cyclic(sbdbt.maru);
+void swordCalculation(){
+    swordSwingCyclic.cyclic(sbdbt.maru);
 }
 
-//cylinder
-void firstmotion()
-{
-        float cylinder = 0.0;
-        float sholderL = 0.0;
-        float sholderR = 0.0;
-    while(interrupt_cylinder_min == 1 || interrupt_sholderleft_max == 1 || interrupt_sholderright_max == 1){
-        if(interrupt_cylinder_min == 1){
+//index_shot
+void firstMotion(){
+    float cylinder = 0.0;
+    float sholderL = 0.0;
+    float sholderR = 0.0;
+    while(shotInterrupt == 1 || shoulderLeftInterruptMax == 1 || shoulderRightInterruptMax == 1){
+        if(shotInterrupt == 1){
             cylinder = 2.0;
         }else{
             cylinder = 0.0;
         }
-        if(interrupt_sholderright_max == 1){
+        if(shoulderRightInterruptMax == 1){
             sholderR = -2.0;
         }else{
             sholderR = 0.0;
         }
-        if(interrupt_sholderleft_max == 1){
+        if(shoulderLeftInterruptMax == 1){
             sholderL = 2.0;
         }else{
             sholderL = 0.0;
         }
-            rs422.put(5, cylinder, 0.0);
-            rs422.put(6, sholderR, sholderL);
+        rs422.put(5, cylinder, 0.0);
+        rs422.put(6, sholderR, sholderL);
     }
-    enc_cylinder.origin();
+    shotEncoder.origin();
 }
 
-//cylinder_origin_flagを1にすることで動作する
-void cylinder_origin()
-{
-}
-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);   
-        }
+//cylinderOriginFlagを1にすることで動作する
+float shotCylinderPowerCalclation(){
+    if(shotInterrupt == 0&&shot::cylinderOriginFlag == 1) {
+        shot::cylinderOriginFlag = 0;
+        shotEncoder.origin();
+        shot::cylinderPosNum = 0;
+        shot::cylinderPower = -0.0;
+        return 0.0;
+    }else if(shot::cylinderOriginFlag == 1){
+        return -0.8;
+    }else{
+        return -1*shotEncoder.duty_enableWidth(-10.0,10.0);   
+    }
 }
 
-void cylinder_cal()
-{
+void shotCylinderCalculation(){
     //cylinder ON/OFF
     if(sbdbt.shikaku){
-        cylinder_on = 1;
-        cylinder_off = 0;
+        shotCylinderOn = 1;
+        shotCylinderOff = 0;
     }else{
-        cylinder_on = 0;
-        cylinder_off = 1;    
+        shotCylinderOn = 0;
+        shotCylinderOff = 1;    
     }
-    
-    cylinder_event.input(sbdbt.right);
-    if(cylinder_event.getRise()) {        //cylinder degset
-        cylinder_pos_num++;
-        if(cylinder_pos_num >= SHOT::CYLINDER_POSITION_MAX) {
-            //cylinder_pos_num = 0;
-            cylinder_origin_flag=1;
+    shotCylinderEvent.input(sbdbt.right);
+    if(shotCylinderEvent.getRise()) {        //cylinder degset
+        shot::cylinderPosNum++;
+        if(shot::cylinderPosNum >= SHOT::CYLINDER_POSITION_MAX) {
+            //shot::cylinderPosNum = 0;
+            shot::cylinderOriginFlag=1;
         }
     }
-    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());
+    shotEncoder.cal((float)shot::cylinderPos[shot::cylinderPosNum],CYCLE::OUTPUT); //set cylinder_tergetPos
+    //pc.printf("target\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",shot::cylinderPos[shot::cylinderPosNum],shotEncoder.deg(),shotEncoder.pulse(),shotEncoder.duty());
 }
 
-//function
-/*void servo_max()
-{
-    servo.pulsewidth(0.0022);
-}
-void servo_min()
-{
-    servo.pulsewidth(0.0010);
-}
-void servo_out()
-{
-    cyclic_servo.cyclic(sbdbt.down);    //setServoControl
-    if(cyclic_servo.getState()==0) {
-        servo_min();
-    } else if(cyclic_servo.getState()==1) {
-        servo_max();
-    }
-}*/
-
-
 //functionBoost
-void boost()
-{
+void motorBoost(){
     if(sbdbt.r2) {
         mecanum.boost_forward();
     }
@@ -309,13 +280,12 @@
     */
 }
 
-//functionMecanum
-void motor_cal()
-{
+//index_mecanum
+void motorCalculation(){
     static float out_right_x;
-    yaw = bno055.getYawRad();
-    target_yaw = yaw;
-    yaw_pid.calculate(target_yaw, yaw); 
+    yaw::now = bno055.getYawRad();
+    yaw::target = yaw::now;
+    yawPid.calculate(yaw::target, yaw::now); 
     
     //後進時に方向が逆転するため
     if(sbdbt.right_y < 0.0){
@@ -326,47 +296,48 @@
     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());
 }
+
+//index_output
 void output()
 {
-    motor_cal();
-    cylinder_cal();
-    sword_cal();
+    motorCalculation();
+    shotCylinderCalculation();
+    swordCalculation();
     //servo_out();
-    cylinder_origin();
-    //boost();
-    sbdbt_indigator = sbdbt.get_pairingState();
+    //motorBoost();
+    controlIndigator = sbdbt.get_pairingState();
     //led1 = sbdbt.get_pairingState();
-    if(sbdbt.up)cylinder_origin_flag = 1;
+    shoulderInterruptIndicator();
+    if(sbdbt.up)shot::cylinderOriginFlag = 1;
     if(sbdbt.left)bno055.yaw_origin();
-    cylinder_reload.cyclic(sbdbt.down);
-
+    reloadCylinder.cyclic(sbdbt.down);
+    
     static int counter;
-    static float mecanumV1, mecanumV2, mecanumV3, mecanumV4;
     int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
 
     //sbdbtがpairingしている場合のみ動作
     if(sbdbt.get_pairingState()) {
         switch (counter) {
             case 0:
-                rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)));
+                rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*POWER::MECANUM)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*POWER::MECANUM)+(sbdbt.right_y*0.8)),1.0,-1.0)));
                 counter++;
                 break;
             case 1:
-                //.duty(<cal>*<powerControle>+(<boost>*0.5))
-                rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)));
+                //.duty(<cal>*<powerControle>+(<motorBoost>*0.5))
+                rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*POWER::MECANUM)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*POWER::MECANUM)-(sbdbt.right_y*0.8)),1.0,-1.0)));
                 counter ++;
                 break;
             case 2:
                 //rs422.put(id[counter], sbdbt.right_y, 0.0);
-                rs422.put(id[counter], limitf(cylinder_power_cal(),1.0,-1.0), 0.0);
+                rs422.put(id[counter], limitf(shotCylinderPowerCalclation(),1.0,-1.0), 0.0);
                 counter ++;
                 break;
             case 3:
-                rs422.put(id[counter], limitf((-1*shoulder_right_cal()*sholder_power),1.0,-1.0), limitf((shoulder_left_cal()*sholder_power),1.0,-1.0));
+                rs422.put(id[counter], limitf((-1*shoulderRightCalclation()*POWER::SHOLDER),1.0,-1.0), limitf((shoulderLeftCalclation()*POWER::SHOLDER),1.0,-1.0));
                 counter ++;
                 break;
             case 4:
-                rs422.put(id[counter], limitf(((float)sword.getState()*sword_power),1.0,-1.0),0.0);
+                rs422.put(id[counter], limitf(((float)swordSwingCyclic.getState()*POWER::SWORD),1.0,-1.0),0.0);
                 counter = 0;
                 break;
             default: