![](/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-06-29
- Revision:
- 1:2d878962e6ea
- Parent:
- 0:bf96e953cdb8
- Child:
- 2:d5b8f8e62923
File content as of revision 1:2d878962e6ea:
#include "mbed.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" #define pc_baud 460800 #define sbdbt_tx p13 #define sbdbt_rx p14 #define sbdbt_baud 115200 #define rs422_tx p28 #define rs422_rx p27 #define rs422_baud 115200 #define output_period 0.005 #define nucleo_num 3 #define pi 3.141592 #define n1_id 3 #define n2_id 4 #define n3_id 0 #define Kp 0.0 #define Ki 0.0 #define Kd 0.0 DigitalOut led(LED1); Serial pc(USBTX,USBRX); RS422 rs422(rs422_tx, rs422_rx); Sbdbt sbdbt(sbdbt_tx, sbdbt_rx); Ticker output_timer; Mecanum mecanum; Bno055 bno055; void setup(); void output(); void put_output(); float m1, m2, m3, m4; float a; int main() { setup(); while(1) { mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, 0, 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()); put_output(); } } void setup() { wait(1); bno055.begin(); wait(1); bno055.firstRead(); pc.baud(pc_baud); sbdbt.begin(sbdbt_baud); rs422.begin(rs422_baud); output_timer.attach(&output, output_period); mecanum.setupdeg(bno055.getYawRad()); } void put_output(){ m1 = mecanum.v1(); m2 = mecanum.v2(); m3 = mecanum.v3(); m4 = mecanum.v4(); } void output() { static int counter; int id[nucleo_num] = {n1_id, n2_id, n3_id}; switch (counter) { case 0: rs422.put(id[counter],m1 ,m3); counter++; break; case 1: rs422.put(id[counter],m2 ,m4); counter ++; break; case 2: rs422.put(id[counter], 0.0, 0.0); counter = 0; break; default: break; }; }