kusano kiyoshige / Mbed 2 deprecated 17robo_tokyo_kaede

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 kusano kiyoshige

main.cpp@0:bf96e953cdb8, 2017-06-26 (annotated)

Committer:
echo_piyo
Date:
Mon Jun 26 09:59:14 2017 +0000
Revision:
0:bf96e953cdb8
Child:
1:2d878962e6ea
????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 0:bf96e953cdb8 1 #include "mbed.h"
echo_piyo 0:bf96e953cdb8 2 #include "bit_test.h"
echo_piyo 0:bf96e953cdb8 3 #include "RS422_put.h"
echo_piyo 0:bf96e953cdb8 4 #include "sbdbt.h"
echo_piyo 0:bf96e953cdb8 5 #include "mecanum.h"
echo_piyo 0:bf96e953cdb8 6 #include "bno055_lib.h"
echo_piyo 0:bf96e953cdb8 7 #include "bno055_use.h"
echo_piyo 0:bf96e953cdb8 8 #include "pid.h"
echo_piyo 0:bf96e953cdb8 9 #include "limit.h"
echo_piyo 0:bf96e953cdb8 10
echo_piyo 0:bf96e953cdb8 11 #define pc_baud 460800
echo_piyo 0:bf96e953cdb8 12 #define sbdbt_tx p13
echo_piyo 0:bf96e953cdb8 13 #define sbdbt_rx p14
echo_piyo 0:bf96e953cdb8 14 #define sbdbt_baud 115200
echo_piyo 0:bf96e953cdb8 15 #define rs422_tx p28
echo_piyo 0:bf96e953cdb8 16 #define rs422_rx p27
echo_piyo 0:bf96e953cdb8 17 #define rs422_baud 115200
echo_piyo 0:bf96e953cdb8 18 #define output_period 0.005
echo_piyo 0:bf96e953cdb8 19 #define nucleo_num 3
echo_piyo 0:bf96e953cdb8 20 #define pi 3.141592
echo_piyo 0:bf96e953cdb8 21 #define n1_id 3
echo_piyo 0:bf96e953cdb8 22 #define n2_id 4
echo_piyo 0:bf96e953cdb8 23 #define n3_id 0
echo_piyo 0:bf96e953cdb8 24 #define Kp 0.0
echo_piyo 0:bf96e953cdb8 25 #define Ki 0.0
echo_piyo 0:bf96e953cdb8 26 #define Kd 0.0
echo_piyo 0:bf96e953cdb8 27
echo_piyo 0:bf96e953cdb8 28 DigitalOut led(LED1);
echo_piyo 0:bf96e953cdb8 29 Serial pc(USBTX,USBRX);
echo_piyo 0:bf96e953cdb8 30 RS422 rs422(rs422_tx, rs422_rx);
echo_piyo 0:bf96e953cdb8 31 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
echo_piyo 0:bf96e953cdb8 32 Ticker output_timer;
echo_piyo 0:bf96e953cdb8 33 Mecanum mecanum;
echo_piyo 0:bf96e953cdb8 34 Bno055 bno055;
echo_piyo 0:bf96e953cdb8 35
echo_piyo 0:bf96e953cdb8 36 void setup();
echo_piyo 0:bf96e953cdb8 37 void output();
echo_piyo 0:bf96e953cdb8 38 void put_output();
echo_piyo 0:bf96e953cdb8 39 float m1, m2, m3, m4;
echo_piyo 0:bf96e953cdb8 40 float a;
echo_piyo 0:bf96e953cdb8 41
echo_piyo 0:bf96e953cdb8 42 int main()
echo_piyo 0:bf96e953cdb8 43 {
echo_piyo 0:bf96e953cdb8 44 setup();
echo_piyo 0:bf96e953cdb8 45 while(1) {
echo_piyo 0:bf96e953cdb8 46 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, 0, bno055.getYawRad());
echo_piyo 0:bf96e953cdb8 47 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());
echo_piyo 0:bf96e953cdb8 48 put_output();
echo_piyo 0:bf96e953cdb8 49 }
echo_piyo 0:bf96e953cdb8 50 }
echo_piyo 0:bf96e953cdb8 51
echo_piyo 0:bf96e953cdb8 52 void setup()
echo_piyo 0:bf96e953cdb8 53 {
echo_piyo 0:bf96e953cdb8 54 bno055.begin();
echo_piyo 0:bf96e953cdb8 55 wait(1);
echo_piyo 0:bf96e953cdb8 56 bno055.firstRead();
echo_piyo 0:bf96e953cdb8 57 pc.baud(pc_baud);
echo_piyo 0:bf96e953cdb8 58 sbdbt.begin(sbdbt_baud);
echo_piyo 0:bf96e953cdb8 59 rs422.begin(rs422_baud);
echo_piyo 0:bf96e953cdb8 60 output_timer.attach(&output, output_period);
echo_piyo 0:bf96e953cdb8 61 mecanum.setupdeg(bno055.getYawRad());
echo_piyo 0:bf96e953cdb8 62 }
echo_piyo 0:bf96e953cdb8 63
echo_piyo 0:bf96e953cdb8 64 void put_output(){
echo_piyo 0:bf96e953cdb8 65 m1 = mecanum.v1();
echo_piyo 0:bf96e953cdb8 66 m2 = mecanum.v2();
echo_piyo 0:bf96e953cdb8 67 m3 = mecanum.v3();
echo_piyo 0:bf96e953cdb8 68 m4 = mecanum.v4();
echo_piyo 0:bf96e953cdb8 69 }
echo_piyo 0:bf96e953cdb8 70
echo_piyo 0:bf96e953cdb8 71 void output()
echo_piyo 0:bf96e953cdb8 72 {
echo_piyo 0:bf96e953cdb8 73 static int counter;
echo_piyo 0:bf96e953cdb8 74 int id[nucleo_num] = {n1_id, n2_id, n3_id};
echo_piyo 0:bf96e953cdb8 75
echo_piyo 0:bf96e953cdb8 76 switch (counter) {
echo_piyo 0:bf96e953cdb8 77 case 0:
echo_piyo 0:bf96e953cdb8 78 rs422.put(id[counter],m1 ,m3);
echo_piyo 0:bf96e953cdb8 79 counter++;
echo_piyo 0:bf96e953cdb8 80 break;
echo_piyo 0:bf96e953cdb8 81 case 1:
echo_piyo 0:bf96e953cdb8 82 rs422.put(id[counter],m2 ,m4);
echo_piyo 0:bf96e953cdb8 83 counter ++;
echo_piyo 0:bf96e953cdb8 84 break;
echo_piyo 0:bf96e953cdb8 85 case 2:
echo_piyo 0:bf96e953cdb8 86 rs422.put(id[counter], 0.0, 0.0);
echo_piyo 0:bf96e953cdb8 87 counter = 0;
echo_piyo 0:bf96e953cdb8 88 break;
echo_piyo 0:bf96e953cdb8 89 default:
echo_piyo 0:bf96e953cdb8 90 break;
echo_piyo 0:bf96e953cdb8 91 };
echo_piyo 0:bf96e953cdb8 92 }