ライブラリ化を行った後

Dependencies:   QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo

Fork of 17robo_Practice1 by kusano kiyoshige

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 //index_include
00002 #include "mbed.h"
00003 #include "math.h"
00004 #include "bit_test.h"
00005 #include "rs422_put.h"
00006 #include "sbdbt.h"
00007 #include "mecanum.h"
00008 #include "bno055_lib.h"
00009 #include "bno055_use.h"
00010 #include "pid.h"
00011 #include "limit.h"
00012 #include "accelerator.h"
00013 #include "pid_encoder.h"
00014 #include "cyclic_var.h"
00015 #include "cyclic_io.h"
00016 #include "cylinder.h"
00017 #include "event_var.h"
00018 
00019 //index_define
00020 #define SBDBT_TX    p13
00021 #define SBDBT_RX    p14
00022 #define SBDBT_STO p12
00023 #define RS422_TX    p28
00024 #define RS422_RX    p27
00025 #define CONTROL_INDICATOR p11
00026 
00027 #define SHOT_CYLINDER_ON    p18
00028 #define SHOT_CYLINDER_OFF   p17
00029 #define SHOT_INTERRUPT      p29
00030 #define SHOT_ENCODER_A      p21
00031 #define SHOT_ENCODER_B      p22
00032 #define RELOAD_CYLINDER     p15
00033 
00034 #define SHOULDER_RIGHT_INTERRUPT_MAX  p19
00035 #define SHOULDER_RIGHT_INTERRUPT_MIN  p20
00036 #define SHOULDER_LEFT_INTERRUPT_MAX   p7
00037 #define SHOULDER_LEFT_INTERRUPT_MIN   p8
00038 
00039 #define nucleo_num  6
00040 #define n1_id       3
00041 #define n2_id       4
00042 #define n3_id       5
00043 #define n4_id       6
00044 #define n5_id       7
00045 #define n6_id       8
00046 
00047 //index_namespace_const_variable
00048 namespace BAUD {
00049     const int PC    = 460800;
00050     const int SBDBT = 115200;
00051     const int RS422 = 115200;
00052 }
00053 
00054 namespace CYCLE {
00055     const float OUTPUT  = 0.015;
00056 }
00057 
00058 namespace MECANUM {
00059     const float ACCELERATION = 15;
00060 }
00061 
00062 namespace YAW {
00063     const float KP = 0.01;
00064     const float KI = 0.01;
00065     const float KD = 0.01;
00066 }
00067 
00068 namespace SHOT {
00069     const float ENCORDER_KP = 0.0400;
00070     const float ENCORDER_KI = 0.0001;
00071     const float ENCORDER_KD = 0.0003;
00072     const int CYLINDER_POSITION_MAX = 6;
00073 }
00074 
00075 namespace POWER {
00076     const float MECANUM = 1.0;
00077     const float SWORD   = 1.0;
00078     const float SHOLDER = 1.0;
00079 }
00080 
00081 //index_namespace_variable
00082 namespace yaw{
00083     float now;
00084     float target;
00085 }
00086 
00087 namespace shot{
00088     int   cylinderOriginFlag = 0;
00089     int   cylinderPosNum = 0;
00090     float cylinderPos[SHOT::CYLINDER_POSITION_MAX] = {0.0,90.0,325.0,450.0,580.0,700.0};
00091     float cylinderPwm = 0;
00092     float cylinderPower = 0.0;
00093 }
00094 
00095 //index_Function
00096 void    setup();
00097 void    output();
00098 void    motorCalculation();
00099 //void  motorBoost();
00100 float   shotCylinderPowerCalclation();
00101 void    shotCylinderCalculation();
00102 void    firstMotion();
00103 void    swordCalculation();
00104 float   shoulderRightCalclation();
00105 float   shoulderLeftCalclation();
00106 void    shoulderInterruptIndicator();
00107 
00108 //index_setupPin
00109 DigitalOut  led1(LED1);
00110 DigitalOut  led2(LED2);
00111 DigitalOut  led3(LED3);
00112 DigitalOut  led4(LED4);
00113 Serial      pc(USBTX,USBRX);
00114 Rs422       rs422(RS422_TX, RS422_RX);
00115 Sbdbt       sbdbt(SBDBT_TX, SBDBT_RX, SBDBT_STO);
00116 Ticker      outputTimer;
00117 Mecanum     mecanum;
00118 Bno055      bno055;
00119 PositionPid yawPid;
00120 Accelerator v1;
00121 Accelerator v2;
00122 Accelerator v3;
00123 Accelerator v4;
00124 eventVar    shotCylinderEvent;
00125 CyclicVar   shotPositionCyclic;
00126 DigitalIn   shotInterrupt(SHOT_INTERRUPT);
00127 DigitalOut  shotCylinderOn(SHOT_CYLINDER_ON);
00128 DigitalOut  shotCylinderOff(SHOT_CYLINDER_OFF);
00129 Encoder     shotEncoder(SHOT_ENCODER_A,SHOT_ENCODER_B);
00130 DigitalIn   shoulderRightInterruptMin(SHOULDER_RIGHT_INTERRUPT_MIN);
00131 DigitalIn   shoulderRightInterruptMax(SHOULDER_RIGHT_INTERRUPT_MAX);
00132 DigitalIn   shoulderLeftInterruptMin(SHOULDER_LEFT_INTERRUPT_MIN);
00133 DigitalIn   shoulderLeftInterruptMax(SHOULDER_LEFT_INTERRUPT_MAX);
00134 CyclicVar   swordSwingCyclic;
00135 CyclicIo    reloadCylinder(RELOAD_CYLINDER);
00136 DigitalOut  controlIndigator(CONTROL_INDICATOR);
00137 
00138 int main()
00139 {
00140     setup();
00141     while(1) {
00142         //pc.printf("cylinder pos : %f\r\n",shotEncoder.deg());
00143         //pc.printf("riseState %d  :  fallState %d\r\n",event.getRise(),event.getFall());
00144     }
00145 }
00146 
00147 void setup()
00148 {
00149     wait(1.0);
00150     bno055.begin();
00151     wait(1.0);
00152     bno055.firstRead();
00153     pc.baud(BAUD::PC);
00154     sbdbt.begin(BAUD::SBDBT);
00155     rs422.begin(BAUD::RS422);
00156     firstMotion();
00157     shot::cylinderPosNum = 1;                   //セットアップタイムでの初期装填のため
00158     outputTimer.attach(&output, CYCLE::OUTPUT);
00159     yawPid.setup(YAW::KP, YAW::KI, YAW::KD, CYCLE::OUTPUT);
00160     mecanum.setupdeg(bno055.getYawRad());       //基盤が前後逆の場合+180
00161     v1.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
00162     v2.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
00163     v3.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
00164     v4.setup(MECANUM::ACCELERATION, CYCLE::OUTPUT);
00165     shotEncoder.setPpr(100);
00166     shotEncoder.setup(SHOT::ENCORDER_KP, SHOT::ENCORDER_KI, SHOT::ENCORDER_KD, CYCLE::OUTPUT);
00167 }
00168 
00169 //index_sword
00170 void shoulderInterruptIndicator(){
00171     led1 = shoulderLeftInterruptMax;
00172     led2 = shoulderLeftInterruptMin;
00173     led3 = shoulderRightInterruptMax;
00174     led4 = shoulderRightInterruptMin;
00175 }
00176 
00177 float shoulderRightCalclation(){
00178     if(shoulderRightInterruptMax==0&&sbdbt.sankaku==1) {
00179         return 0.0;
00180     }
00181     if(shoulderRightInterruptMin==0&&sbdbt.batu==1) {
00182         return 0.0;
00183     }
00184     return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
00185 }
00186 
00187 float shoulderLeftCalclation(){
00188     if(shoulderLeftInterruptMax==0&&sbdbt.sankaku==1) {
00189         return 0.0;
00190     }
00191     if(shoulderLeftInterruptMin==0&&sbdbt.batu==1) {
00192         return 0.0;
00193     }
00194     return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
00195 }
00196 
00197 void swordCalculation(){
00198     swordSwingCyclic.cyclic(sbdbt.maru);
00199 }
00200 
00201 //index_shot
00202 void firstMotion(){
00203     float cylinder = 0.0;
00204     float sholderL = 0.0;
00205     float sholderR = 0.0;
00206     while(shotInterrupt == 1 || shoulderLeftInterruptMax == 1 || shoulderRightInterruptMax == 1){
00207         if(shotInterrupt == 1){
00208             cylinder = 2.0;
00209         }else{
00210             cylinder = 0.0;
00211         }
00212         if(shoulderRightInterruptMax == 1){
00213             sholderR = -2.0;
00214         }else{
00215             sholderR = 0.0;
00216         }
00217         if(shoulderLeftInterruptMax == 1){
00218             sholderL = 2.0;
00219         }else{
00220             sholderL = 0.0;
00221         }
00222         rs422.put(5, cylinder, 0.0);
00223         rs422.put(6, sholderR, sholderL);
00224     }
00225     shotEncoder.origin();
00226 }
00227 
00228 //cylinderOriginFlagを1にすることで動作する
00229 float shotCylinderPowerCalclation(){
00230     if(shotInterrupt == 0&&shot::cylinderOriginFlag == 1) {
00231         shot::cylinderOriginFlag = 0;
00232         shotEncoder.origin();
00233         shot::cylinderPosNum = 0;
00234         shot::cylinderPower = -0.0;
00235         reloadCylinder.cyclicOff();
00236         return 0.0;
00237     }else if(shot::cylinderOriginFlag == 1){
00238         return -0.8;
00239     }else{
00240         return -1*shotEncoder.duty_enableWidth(-10.0,10.0);   
00241     }
00242 }
00243 
00244 void shotCylinderCalculation(){
00245     //cylinder ON/OFF
00246     if(sbdbt.shikaku){
00247         shotCylinderOn = 1;
00248         shotCylinderOff = 0;
00249     }else{
00250         shotCylinderOn = 0;
00251         shotCylinderOff = 1;    
00252     }
00253     shotCylinderEvent.input(sbdbt.right);
00254     if(shotCylinderEvent.getRise()) {        //cylinder degset
00255         shot::cylinderPosNum++;
00256         if(shot::cylinderPosNum >= SHOT::CYLINDER_POSITION_MAX) {
00257             //shot::cylinderPosNum = 0;
00258             shot::cylinderOriginFlag=1;
00259         }
00260     }
00261     shotEncoder.cal((float)shot::cylinderPos[shot::cylinderPosNum],CYCLE::OUTPUT); //set cylinder_tergetPos
00262     //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());
00263 }
00264 
00265 //functionBoost
00266 void motorBoost(){
00267     if(sbdbt.r2) {
00268         mecanum.boost_forward();
00269     }
00270     if(sbdbt.l2) {
00271         mecanum.boost_back();
00272     }
00273     /*
00274     if(sbdbt.shikaku) {
00275         mecanum.boost_left();
00276     }
00277     if(sbdbt.maru) {
00278         mecanum.boost_right();
00279     }
00280     */
00281 }
00282 
00283 //index_mecanum
00284 void motorCalculation(){
00285     static float out_right_x;
00286     yaw::now = bno055.getYawRad();
00287     yaw::target = yaw::now;
00288     yawPid.calculate(yaw::target, yaw::now); 
00289     
00290     //後進時に方向が逆転するため
00291     if(sbdbt.right_y < 0.0){
00292         out_right_x = -1.0*sbdbt.right_x;
00293     }else{
00294         out_right_x = sbdbt.right_x;
00295     }
00296     mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, bno055.getYawRad());
00297 //    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());
00298 }
00299 
00300 //index_output
00301 void output()
00302 {
00303     motorCalculation();
00304     shotCylinderCalculation();
00305     swordCalculation();
00306     //servo_out();
00307     //motorBoost();
00308     controlIndigator = sbdbt.get_pairingState();
00309     //led1 = sbdbt.get_pairingState();
00310     shoulderInterruptIndicator();
00311     if(sbdbt.up)shot::cylinderOriginFlag = 1;
00312     if(sbdbt.left)bno055.yaw_origin();
00313     reloadCylinder.cyclic(sbdbt.down);
00314     
00315     static int counter;
00316     int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
00317 
00318     //sbdbtがpairingしている場合のみ動作
00319     if(sbdbt.get_pairingState()) {
00320         switch (counter) {
00321             case 0:
00322                 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)));
00323                 counter++;
00324                 break;
00325             case 1:
00326                 //.duty(<cal>*<powerControle>+(<motorBoost>*0.5))
00327                 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)));
00328                 counter ++;
00329                 break;
00330             case 2:
00331                 //rs422.put(id[counter], sbdbt.right_y, 0.0);
00332                 rs422.put(id[counter], limitf(shotCylinderPowerCalclation(),1.0,-1.0), 0.0);
00333                 counter ++;
00334                 break;
00335             case 3:
00336                 rs422.put(id[counter], limitf((-1*shoulderRightCalclation()*POWER::SHOLDER),1.0,-1.0), limitf((shoulderLeftCalclation()*POWER::SHOLDER),1.0,-1.0));
00337                 counter ++;
00338                 break;
00339             case 4:
00340                 rs422.put(id[counter], limitf(((float)swordSwingCyclic.getState()*POWER::SWORD),1.0,-1.0),0.0);
00341                 counter = 0;
00342                 break;
00343             default:
00344                 break;
00345         }
00346     }else{
00347         switch (counter) {
00348             case 0:
00349                 rs422.put(id[counter],0.0,0.0);
00350                 counter++;
00351                 break;
00352             case 1:
00353                 rs422.put(id[counter],0.0,0.0);
00354                 counter ++;
00355                 break;
00356             case 2:
00357                 rs422.put(id[counter],0.0,0.0);
00358                 counter ++;
00359                 break;
00360             case 3:
00361                 rs422.put(id[counter],0.0,0.0);
00362                 counter ++;
00363                 break;
00364             case 4:
00365                 rs422.put(id[counter],0.0,0.0);
00366                 counter = 0;
00367                 break;
00368             default:
00369                 break;
00370         }
00371     }
00372 }