Diff: main.cpp

Revision:

74:98bad67fde39

Parent:

72:7d6177047823

Child:

75:cd507886ee57

--- a/main.cpp	Wed Sep 27 09:46:22 2017 +0000
+++ b/main.cpp	Sun Oct 22 10:55:47 2017 +0000
@@ -24,25 +24,24 @@
 #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_LEFT_HIGH p1
+#define SHOULDER_LEFT_LOW p1
+#define SHOULDER_RIGHT_HIGH p2
+#define SHOULDER_RIGHT_LOW p2
 
-#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 GROUND_ENCORDER_X_A p3
+#define GROUND_ENCORDER_X_B p4
+#define GROUND_ENCORDER_Y_A p5
+#define GROUND_ENCORDER_Y_B p6
 
-#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
+#define GUN_ENCORDER_A p7
+#define GUN_ENCORDER_B p8
+
+#define nucleo_num  4
+#define n0_id       0
+#define n1_id       1
+#define n2_id       2
+#define n3_id       3
 
 //index_namespace_const_variable
 namespace BAUD {
@@ -59,23 +58,11 @@
     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;
-}
-
 namespace POWER {
     const float MECANUM = 1.0;
     const float SWORD   = 1.0;
-    const float SHOLDER = 1.0;
+    const float SHOT = 1.0;
+    const float RELOAD = 1.0;
 }
 
 //index_namespace_variable
@@ -84,26 +71,10 @@
     float target;
 }
 
-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;
-}
-
 //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);
@@ -121,18 +92,7 @@
 Accelerator v2;
 Accelerator v3;
 Accelerator v4;
-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);
 
 
@@ -154,130 +114,12 @@
     pc.baud(BAUD::PC);
     sbdbt.begin(BAUD::SBDBT);
     rs422.begin(BAUD::RS422);
-    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);
-    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;
-}
-
-float shoulderRightCalclation(){
-    if(shoulderRightInterruptMax==0&&sbdbt.sankaku==1) {
-        return 0.0;
-    }
-    if(shoulderRightInterruptMin==0&&sbdbt.batu==1) {
-        return 0.0;
-    }
-    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);
-}
-
-void swordCalculation(){
-    swordSwingCyclic.cyclic(sbdbt.maru);
-}
-
-//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(shoulderRightInterruptMax == 1){
-            sholderR = -2.0;
-        }else{
-            sholderR = 0.0;
-        }
-        if(shoulderLeftInterruptMax == 1){
-            sholderL = 2.0;
-        }else{
-            sholderL = 0.0;
-        }
-        rs422.put(5, cylinder, 0.0);
-        rs422.put(6, sholderR, sholderL);
-    }
-    shotEncoder.origin();
-}
-
-//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 shotCylinderCalculation(){
-    //cylinder ON/OFF
-    if(sbdbt.shikaku){
-        shotCylinderOn = 1;
-        shotCylinderOff = 0;
-    }else{
-        shotCylinderOn = 0;
-        shotCylinderOff = 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;
-        }
-    }
-    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());
-}
-
-//functionBoost
-void motorBoost(){
-    if(sbdbt.r2) {
-        mecanum.boost_forward();
-    }
-    if(sbdbt.l2) {
-        mecanum.boost_back();
-    }
-    /*
-    if(sbdbt.shikaku) {
-        mecanum.boost_left();
-    }
-    if(sbdbt.maru) {
-        mecanum.boost_right();
-    }
-    */
 }
 
 //index_mecanum
@@ -297,23 +139,17 @@
 //    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()
 {
     motorCalculation();
-    shotCylinderCalculation();
-    swordCalculation();
-    //servo_out();
-    //motorBoost();
     controlIndigator = sbdbt.get_pairingState();
     //led1 = sbdbt.get_pairingState();
-    shoulderInterruptIndicator();
-    if(sbdbt.up)shot::cylinderOriginFlag = 1;
     if(sbdbt.left)bno055.yaw_origin();
-    reloadCylinder.cyclic(sbdbt.down);
     
     static int counter;
-    int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
+    int id[nucleo_num] = {n0_id, n1_id, n2_id, n3_id};
 
     //sbdbtがpairingしている場合のみ動作
     if(sbdbt.get_pairingState()) {
@@ -328,18 +164,13 @@
                 counter ++;
                 break;
             case 2:
-                //rs422.put(id[counter], sbdbt.right_y, 0.0);
-                rs422.put(id[counter], limitf(shotCylinderPowerCalclation(),1.0,-1.0), 0.0);
+                rs422.put(id[counter], limitf(0.0*POWER::SHOT,1.0,-1.0), limitf(0.0*POWER::RELOAD,1.0,-1.0));
                 counter ++;
                 break;
             case 3:
-                rs422.put(id[counter], limitf((-1*shoulderRightCalclation()*POWER::SHOLDER),1.0,-1.0), limitf((shoulderLeftCalclation()*POWER::SHOLDER),1.0,-1.0));
+                rs422.put(id[counter], limitf(0.0*POWER::SWORD,1.0,-1.0), limitf(0.0,1.0,-1.0));
                 counter ++;
                 break;
-            case 4:
-                rs422.put(id[counter], limitf(((float)swordSwingCyclic.getState()*POWER::SWORD),1.0,-1.0),0.0);
-                counter = 0;
-                break;
             default:
                 break;
         }
@@ -361,10 +192,6 @@
                 rs422.put(id[counter],0.0,0.0);
                 counter ++;
                 break;
-            case 4:
-                rs422.put(id[counter],0.0,0.0);
-                counter = 0;
-                break;
             default:
                 break;
         }