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
Diff: main.cpp
- Revision:
- 55:2dd2f161ebaf
- Parent:
- 54:a88208d9bc1d
- Child:
- 56:a7bd860b85b6
--- a/main.cpp Sat Sep 16 01:05:23 2017 +0000 +++ b/main.cpp Sat Sep 16 05:59:00 2017 +0000 @@ -55,6 +55,7 @@ #define pin_cylinder_reload p15 #define pin_sbdbt_pairing p12 #define pin_sbdbt_indicator p11 +#define cylinder_pos_max 4 DigitalOut led1(LED1); DigitalOut led2(LED2); @@ -84,7 +85,6 @@ Cyclic_IO cylinder_reload(pin_cylinder_reload); PwmOut servo(pin_servo); DigitalOut sbdbt_indigator(pin_sbdbt_indicator); -varEvent event; varEvent cylinder_event; void setup(); @@ -106,7 +106,7 @@ //cylinder float cylinder_pwm; int cylinder_pos_num = 0; -float cylinder_pos[3] = {0.0,90.0,325.0}; +float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,700.0}; int main() { @@ -117,8 +117,8 @@ led3 = interrupt_sholderright_max; led4 = interrupt_sholderright_min; - event.input(sbdbt.left); - pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall()); + pc.printf("cylinder pos : %f\r\n",enc_cylinder.deg()); + //pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall()); } } @@ -211,7 +211,6 @@ rs422.put(5, -0.8, 0.0); } } - void cylinder_cal() { cylinder.cyclic(sbdbt.shikaku); //cylinder ON/OFF @@ -219,7 +218,7 @@ cylinder_event.input(sbdbt.right); if(cylinder_event.getRise()) { //cylinder degset cylinder_pos_num++; - if(cylinder_pos_num >= 3) { + if(cylinder_pos_num >= cylinder_pos_max) { //cylinder_pos_num = 0; cylinder_origin_flag=1; } @@ -273,7 +272,7 @@ yaw = bno055.getYawRad(); target_yaw = yaw; yaw_pid.cal(target_yaw, yaw, output_period); - mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad()); + mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, sbdbt.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()); } void output() @@ -309,6 +308,7 @@ counter ++; break; case 2: + //rs422.put(id[counter], sbdbt.right_y, 0.0); rs422.put(id[counter], limitf((-1*enc_cylinder.duty_enableWidth(-5.0,5.0)),1.0,-1.0), 0.0); counter ++; break;