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:
- 0:bf96e953cdb8
- Child:
- 1:2d878962e6ea
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jun 26 09:59:14 2017 +0000 @@ -0,0 +1,92 @@ +#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() +{ + 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; + }; +} \ No newline at end of file