![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sampleProgram
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_fuzi by
main.cpp
- Committer:
- echo_piyo
- Date:
- 2017-09-27
- Revision:
- 72:7d6177047823
- Parent:
- 71:7d684ff43ddd
- Child:
- 74:98bad67fde39
File content as of revision 72:7d6177047823:
//index_include #include "mbed.h" #include "math.h" #include "bit_test.h" #include "rs422_put.h" #include "sbdbt.h" #include "mecanum.h" #include "bno055_lib.h" #include "bno055_use.h" #include "pid.h" #include "limit.h" #include "accelerator.h" #include "pid_encoder.h" #include "cyclic_var.h" #include "cyclic_io.h" #include "cylinder.h" #include "event_var.h" //index_define #define SBDBT_TX p13 #define SBDBT_RX p14 #define SBDBT_STO p12 #define RS422_TX p28 #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; 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; } namespace POWER { const float MECANUM = 1.0; const float SWORD = 1.0; const float SHOLDER = 1.0; } //index_namespace_variable namespace yaw{ float now; 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); 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; 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); int main() { setup(); while(1) { //pc.printf("cylinder pos : %f\r\n",shotEncoder.deg()); //pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall()); } } void setup() { wait(1.0); bno055.begin(); wait(1.0); bno055.firstRead(); 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 void motorCalculation(){ static float out_right_x; yaw::now = bno055.getYawRad(); yaw::target = yaw::now; yawPid.calculate(yaw::target, yaw::now); //後進時に方向が逆転するため 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()); } //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}; //sbdbtがpairingしている場合のみ動作 if(sbdbt.get_pairingState()) { switch (counter) { case 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>+(<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(shotCylinderPowerCalclation(),1.0,-1.0), 0.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)); 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; } }else{ switch (counter) { case 0: rs422.put(id[counter],0.0,0.0); counter++; break; case 1: rs422.put(id[counter],0.0,0.0); counter ++; break; case 2: rs422.put(id[counter],0.0,0.0); counter ++; break; case 3: 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; } } }