![](/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
Diff: main.cpp
- Revision:
- 17:5c4718c603dc
- Parent:
- 16:e49df474e4c6
- Child:
- 18:ae4d97945b83
diff -r e49df474e4c6 -r 5c4718c603dc main.cpp --- a/main.cpp Fri Aug 04 01:45:44 2017 +0000 +++ b/main.cpp Fri Aug 04 02:14:02 2017 +0000 @@ -23,7 +23,7 @@ #define rs422_rx p27 #define rs422_baud 115200 #define output_period 0.015 -#define nucleo_num 3 +#define nucleo_num 4 #define pi 3.141592 #define n1_id 3 #define n2_id 4 @@ -60,6 +60,7 @@ Accel v3; Accel v4; Cylinder cylinder(pin_cylinder_on,pin_cylinder_off); +Cyclic sword_left; //追加点 Encoder enc_cylinder(encoder_A,encoder_B); @@ -71,13 +72,14 @@ void cylinder_cal(); void boost(); void cylinder_origin(); +void sword_left_cal(); float yaw, target_yaw; int main() { setup(); while(1) { - pc.printf("Pulses is: %i\r\n",sbdbt.down); + //pc.printf("Pulses is: %i\r\n",sbdbt.down); } } @@ -108,10 +110,11 @@ { motor_cal(); cylinder_cal(); + sword_left_cal(); boost(); static int counter; - int id[nucleo_num] = {n1_id, n2_id, n3_id}; + int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id}; switch (counter) { case 0: @@ -123,7 +126,11 @@ counter ++; break; case 2: - rs422.put(id[counter], sbdbt.right_y, 0.0); + rs422.put(id[counter], 0.0, 0.0); + counter ++; + break; + case 3: + rs422.put(id[counter], sbdbt.right_y, ((float)sword_left.getState())); counter = 0; break; default: @@ -131,6 +138,9 @@ }; } +void sword_left_cal(){ + sword_left.cyclic(sbdbt.maru); +} void cylinder_origin(){ while(interrupt){