![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ライブラリ化を行った後
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
Diff: main.cpp
- Revision:
- 72:7d6177047823
- Parent:
- 71:7d684ff43ddd
- Child:
- 74:98bad67fde39
- Child:
- 77:17469192b53a
diff -r 7d684ff43ddd -r 7d6177047823 main.cpp --- 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: