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@7:c4ae1d001d09, 2017-07-01 (annotated)

Committer:
echo_piyo
Date:
Sat Jul 01 10:12:46 2017 +0000
Revision:
7:c4ae1d001d09
Parent:
5:6efda58ff71b 
rotaryencoder?rad???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 0:bf96e953cdb8 1 #include "mbed.h"
echo_piyo 4:a6cc2f03e69b 2 #include "math.h"
echo_piyo 0:bf96e953cdb8 3 #include "bit_test.h"
echo_piyo 0:bf96e953cdb8 4 #include "RS422_put.h"
echo_piyo 0:bf96e953cdb8 5 #include "sbdbt.h"
echo_piyo 0:bf96e953cdb8 6 #include "mecanum.h"
echo_piyo 0:bf96e953cdb8 7 #include "bno055_lib.h"
echo_piyo 0:bf96e953cdb8 8 #include "bno055_use.h"
echo_piyo 0:bf96e953cdb8 9 #include "pid.h"
echo_piyo 0:bf96e953cdb8 10 #include "limit.h"
echo_piyo 4:a6cc2f03e69b 11 #include "accelerator.h"
echo_piyo 7:c4ae1d001d09 12 #include "encoder.h"
echo_piyo 0:bf96e953cdb8 13
echo_piyo 0:bf96e953cdb8 14 #define pc_baud 460800
echo_piyo 0:bf96e953cdb8 15 #define sbdbt_tx p13
echo_piyo 0:bf96e953cdb8 16 #define sbdbt_rx p14
echo_piyo 0:bf96e953cdb8 17 #define sbdbt_baud 115200
echo_piyo 0:bf96e953cdb8 18 #define rs422_tx p28
echo_piyo 0:bf96e953cdb8 19 #define rs422_rx p27
echo_piyo 0:bf96e953cdb8 20 #define rs422_baud 115200
echo_piyo 3:9ef929639c4a 21 #define output_period 0.015
echo_piyo 0:bf96e953cdb8 22 #define nucleo_num 3
echo_piyo 0:bf96e953cdb8 23 #define pi 3.141592
echo_piyo 0:bf96e953cdb8 24 #define n1_id 3
echo_piyo 0:bf96e953cdb8 25 #define n2_id 4
echo_piyo 0:bf96e953cdb8 26 #define n3_id 0
echo_piyo 5:6efda58ff71b 27 #define yaw_Kp 0.01
echo_piyo 5:6efda58ff71b 28 #define yaw_Ki 0.01
echo_piyo 5:6efda58ff71b 29 #define yaw_Kd 0.01
echo_piyo 4:a6cc2f03e69b 30 #define acceleration 5
echo_piyo 7:c4ae1d001d09 31 #define encoderA p21
echo_piyo 7:c4ae1d001d09 32 #define encoderB p22
echo_piyo 0:bf96e953cdb8 33
echo_piyo 0:bf96e953cdb8 34 DigitalOut led(LED1);
echo_piyo 0:bf96e953cdb8 35 Serial pc(USBTX,USBRX);
echo_piyo 0:bf96e953cdb8 36 RS422 rs422(rs422_tx, rs422_rx);
echo_piyo 0:bf96e953cdb8 37 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
echo_piyo 0:bf96e953cdb8 38 Ticker output_timer;
echo_piyo 0:bf96e953cdb8 39 Mecanum mecanum;
echo_piyo 0:bf96e953cdb8 40 Bno055 bno055;
echo_piyo 2:d5b8f8e62923 41 Position_pid yaw_pid;
echo_piyo 4:a6cc2f03e69b 42 Accel v1;
echo_piyo 4:a6cc2f03e69b 43 Accel v2;
echo_piyo 4:a6cc2f03e69b 44 Accel v3;
echo_piyo 4:a6cc2f03e69b 45 Accel v4;
echo_piyo 7:c4ae1d001d09 46 Encoder encoder(encoderA, encoderB);
echo_piyo 0:bf96e953cdb8 47
echo_piyo 0:bf96e953cdb8 48 void setup();
echo_piyo 0:bf96e953cdb8 49 void output();
echo_piyo 0:bf96e953cdb8 50 void put_output();
echo_piyo 2:d5b8f8e62923 51 float yaw, target_yaw;
echo_piyo 7:c4ae1d001d09 52 long pulse;
echo_piyo 0:bf96e953cdb8 53
echo_piyo 0:bf96e953cdb8 54 int main()
echo_piyo 0:bf96e953cdb8 55 {
echo_piyo 0:bf96e953cdb8 56 setup();
echo_piyo 2:d5b8f8e62923 57 while(1) {
echo_piyo 0:bf96e953cdb8 58 }
echo_piyo 0:bf96e953cdb8 59 }
echo_piyo 0:bf96e953cdb8 60
echo_piyo 0:bf96e953cdb8 61 void setup()
echo_piyo 0:bf96e953cdb8 62 {
echo_piyo 1:2d878962e6ea 63 wait(1);
echo_piyo 0:bf96e953cdb8 64 bno055.begin();
echo_piyo 0:bf96e953cdb8 65 wait(1);
echo_piyo 0:bf96e953cdb8 66 bno055.firstRead();
echo_piyo 0:bf96e953cdb8 67 pc.baud(pc_baud);
echo_piyo 0:bf96e953cdb8 68 sbdbt.begin(sbdbt_baud);
echo_piyo 0:bf96e953cdb8 69 rs422.begin(rs422_baud);
echo_piyo 0:bf96e953cdb8 70 output_timer.attach(&output, output_period);
echo_piyo 2:d5b8f8e62923 71 yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
echo_piyo 0:bf96e953cdb8 72 mecanum.setupdeg(bno055.getYawRad());
echo_piyo 4:a6cc2f03e69b 73 v1.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 74 v2.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 75 v3.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 76 v4.setup(acceleration,output_period);
echo_piyo 7:c4ae1d001d09 77 encoder.setup(360);
echo_piyo 0:bf96e953cdb8 78 }
echo_piyo 0:bf96e953cdb8 79
echo_piyo 5:6efda58ff71b 80 void put_output()
echo_piyo 5:6efda58ff71b 81 {
echo_piyo 7:c4ae1d001d09 82 pulse = encoder.pulse();
echo_piyo 7:c4ae1d001d09 83 pc.printf("pulse %ld",pulse);
echo_piyo 4:a6cc2f03e69b 84 yaw = bno055.getYawRad();
echo_piyo 4:a6cc2f03e69b 85 target_yaw = yaw;
echo_piyo 4:a6cc2f03e69b 86 yaw_pid.cal(target_yaw, yaw, output_period);
echo_piyo 4:a6cc2f03e69b 87 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad());
echo_piyo 5:6efda58ff71b 88 // 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 89 }
echo_piyo 0:bf96e953cdb8 90
echo_piyo 0:bf96e953cdb8 91 void output()
echo_piyo 0:bf96e953cdb8 92 {
echo_piyo 4:a6cc2f03e69b 93 put_output();
echo_piyo 4:a6cc2f03e69b 94
echo_piyo 0:bf96e953cdb8 95 static int counter;
echo_piyo 0:bf96e953cdb8 96 int id[nucleo_num] = {n1_id, n2_id, n3_id};
echo_piyo 0:bf96e953cdb8 97
echo_piyo 5:6efda58ff71b 98 if(sbdbt.batu) {
echo_piyo 5:6efda58ff71b 99 mecanum.boost();
echo_piyo 5:6efda58ff71b 100 }
echo_piyo 5:6efda58ff71b 101
echo_piyo 0:bf96e953cdb8 102 switch (counter) {
echo_piyo 0:bf96e953cdb8 103 case 0:
echo_piyo 4:a6cc2f03e69b 104 rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
echo_piyo 0:bf96e953cdb8 105 counter++;
echo_piyo 0:bf96e953cdb8 106 break;
echo_piyo 0:bf96e953cdb8 107 case 1:
echo_piyo 4:a6cc2f03e69b 108 rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4()));
echo_piyo 0:bf96e953cdb8 109 counter ++;
echo_piyo 0:bf96e953cdb8 110 break;
echo_piyo 0:bf96e953cdb8 111 case 2:
echo_piyo 0:bf96e953cdb8 112 rs422.put(id[counter], 0.0, 0.0);
echo_piyo 0:bf96e953cdb8 113 counter = 0;
echo_piyo 0:bf96e953cdb8 114 break;
echo_piyo 0:bf96e953cdb8 115 default:
echo_piyo 0:bf96e953cdb8 116 break;
echo_piyo 0:bf96e953cdb8 117 };
echo_piyo 5:6efda58ff71b 118
echo_piyo 5:6efda58ff71b 119 }
echo_piyo 5:6efda58ff71b 120