kusano kiyoshige / Mbed 2 deprecated 17robo_fuzi

ライブラリ化を行った後

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

main.cpp@10:04f2a82cfd89, 2017-07-16 (annotated)

Committer:
echo_piyo
Date:
Sun Jul 16 04:06:49 2017 +0000
Revision:
10:04f2a82cfd89
Parent:
9:6486f4b3ac50
Child:
12:1fec80ae8a2c
(??)????????????encorder.h?Speed_pid??Position_pid?????Main?????setup????

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 10:04f2a82cfd89 12 #include "encorder.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 10:04f2a82cfd89 26 #define n3_id 5
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 10:04f2a82cfd89 30 #define acceleration 25
echo_piyo 10:04f2a82cfd89 31 #define pin_cylinder_on p17
echo_piyo 10:04f2a82cfd89 32 #define pin_cylinder_off p18
echo_piyo 10:04f2a82cfd89 33 #define interrupt p16
echo_piyo 10:04f2a82cfd89 34 #define encoder_cylinder_A p25
echo_piyo 10:04f2a82cfd89 35 #define encoder_cylinder_B p26
echo_piyo 10:04f2a82cfd89 36 #define enc_Kp 0.01
echo_piyo 10:04f2a82cfd89 37 #define enc_Ki 0.01
echo_piyo 10:04f2a82cfd89 38 #define enc_Kd 0.01
echo_piyo 0:bf96e953cdb8 39
echo_piyo 0:bf96e953cdb8 40 DigitalOut led(LED1);
echo_piyo 10:04f2a82cfd89 41 DigitalOut sylinder_on(pin_cylinder_on);
echo_piyo 10:04f2a82cfd89 42 DigitalOut sylinder_off(pin_cylinder_off);
echo_piyo 0:bf96e953cdb8 43 Serial pc(USBTX,USBRX);
echo_piyo 0:bf96e953cdb8 44 RS422 rs422(rs422_tx, rs422_rx);
echo_piyo 0:bf96e953cdb8 45 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
echo_piyo 0:bf96e953cdb8 46 Ticker output_timer;
echo_piyo 0:bf96e953cdb8 47 Mecanum mecanum;
echo_piyo 0:bf96e953cdb8 48 Bno055 bno055;
echo_piyo 2:d5b8f8e62923 49 Position_pid yaw_pid;
echo_piyo 4:a6cc2f03e69b 50 Accel v1;
echo_piyo 4:a6cc2f03e69b 51 Accel v2;
echo_piyo 4:a6cc2f03e69b 52 Accel v3;
echo_piyo 4:a6cc2f03e69b 53 Accel v4;
echo_piyo 10:04f2a82cfd89 54 Encoder enc_cylinder(encoder_cylinder_A,encoder_cylinder_B);
echo_piyo 9:6486f4b3ac50 55
echo_piyo 0:bf96e953cdb8 56 void setup();
echo_piyo 0:bf96e953cdb8 57 void output();
echo_piyo 0:bf96e953cdb8 58 void put_output();
echo_piyo 10:04f2a82cfd89 59 void cylinder_check();
echo_piyo 10:04f2a82cfd89 60 void boost();
echo_piyo 2:d5b8f8e62923 61 float yaw, target_yaw;
echo_piyo 0:bf96e953cdb8 62
echo_piyo 0:bf96e953cdb8 63 int main()
echo_piyo 0:bf96e953cdb8 64 {
echo_piyo 0:bf96e953cdb8 65 setup();
echo_piyo 2:d5b8f8e62923 66 while(1) {
echo_piyo 0:bf96e953cdb8 67 }
echo_piyo 0:bf96e953cdb8 68 }
echo_piyo 0:bf96e953cdb8 69
echo_piyo 0:bf96e953cdb8 70 void setup()
echo_piyo 0:bf96e953cdb8 71 {
echo_piyo 1:2d878962e6ea 72 wait(1);
echo_piyo 0:bf96e953cdb8 73 bno055.begin();
echo_piyo 0:bf96e953cdb8 74 wait(1);
echo_piyo 0:bf96e953cdb8 75 bno055.firstRead();
echo_piyo 0:bf96e953cdb8 76 pc.baud(pc_baud);
echo_piyo 0:bf96e953cdb8 77 sbdbt.begin(sbdbt_baud);
echo_piyo 0:bf96e953cdb8 78 rs422.begin(rs422_baud);
echo_piyo 0:bf96e953cdb8 79 output_timer.attach(&output, output_period);
echo_piyo 2:d5b8f8e62923 80 yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
echo_piyo 10:04f2a82cfd89 81 mecanum.setupdeg(bno055.getYawRad()+180.0);
echo_piyo 4:a6cc2f03e69b 82 v1.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 83 v2.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 84 v3.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 85 v4.setup(acceleration,output_period);
echo_piyo 10:04f2a82cfd89 86 enc_cylinder.setup(1200);
echo_piyo 10:04f2a82cfd89 87 enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
echo_piyo 0:bf96e953cdb8 88 }
echo_piyo 0:bf96e953cdb8 89
echo_piyo 10:04f2a82cfd89 90 void boost(){
echo_piyo 10:04f2a82cfd89 91 if(sbdbt.sankaku) {
echo_piyo 10:04f2a82cfd89 92 mecanum.boost_forward();
echo_piyo 10:04f2a82cfd89 93 }
echo_piyo 10:04f2a82cfd89 94 if(sbdbt.batu) {
echo_piyo 10:04f2a82cfd89 95 mecanum.boost_back();
echo_piyo 10:04f2a82cfd89 96 }
echo_piyo 10:04f2a82cfd89 97 if(sbdbt.shikaku) {
echo_piyo 10:04f2a82cfd89 98 mecanum.boost_left();
echo_piyo 10:04f2a82cfd89 99 }
echo_piyo 10:04f2a82cfd89 100 if(sbdbt.maru) {
echo_piyo 10:04f2a82cfd89 101 mecanum.boost_right();
echo_piyo 10:04f2a82cfd89 102 }
echo_piyo 10:04f2a82cfd89 103 }
echo_piyo 10:04f2a82cfd89 104
echo_piyo 10:04f2a82cfd89 105 void cylinder_check()
echo_piyo 10:04f2a82cfd89 106 {
echo_piyo 10:04f2a82cfd89 107 if(sbdbt.sankaku) {
echo_piyo 10:04f2a82cfd89 108 sylinder_on = 1;
echo_piyo 10:04f2a82cfd89 109 } else {
echo_piyo 10:04f2a82cfd89 110 sylinder_on = 0;
echo_piyo 10:04f2a82cfd89 111 }
echo_piyo 10:04f2a82cfd89 112
echo_piyo 10:04f2a82cfd89 113 if(sbdbt.shikaku) {
echo_piyo 10:04f2a82cfd89 114 sylinder_off = 1;
echo_piyo 10:04f2a82cfd89 115 } else {
echo_piyo 10:04f2a82cfd89 116 sylinder_off = 0;
echo_piyo 10:04f2a82cfd89 117 }
echo_piyo 10:04f2a82cfd89 118 }
echo_piyo 10:04f2a82cfd89 119
echo_piyo 10:04f2a82cfd89 120
echo_piyo 10:04f2a82cfd89 121
echo_piyo 5:6efda58ff71b 122 void put_output()
echo_piyo 5:6efda58ff71b 123 {
echo_piyo 4:a6cc2f03e69b 124 yaw = bno055.getYawRad();
echo_piyo 4:a6cc2f03e69b 125 target_yaw = yaw;
echo_piyo 4:a6cc2f03e69b 126 yaw_pid.cal(target_yaw, yaw, output_period);
echo_piyo 4:a6cc2f03e69b 127 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad());
echo_piyo 5:6efda58ff71b 128 // 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 129 }
echo_piyo 0:bf96e953cdb8 130
echo_piyo 0:bf96e953cdb8 131 void output()
echo_piyo 0:bf96e953cdb8 132 {
echo_piyo 4:a6cc2f03e69b 133 put_output();
echo_piyo 10:04f2a82cfd89 134 //cylinder_check();
echo_piyo 10:04f2a82cfd89 135 boost();
echo_piyo 4:a6cc2f03e69b 136
echo_piyo 0:bf96e953cdb8 137 static int counter;
echo_piyo 0:bf96e953cdb8 138 int id[nucleo_num] = {n1_id, n2_id, n3_id};
echo_piyo 0:bf96e953cdb8 139
echo_piyo 5:6efda58ff71b 140
echo_piyo 0:bf96e953cdb8 141 switch (counter) {
echo_piyo 0:bf96e953cdb8 142 case 0:
echo_piyo 4:a6cc2f03e69b 143 rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
echo_piyo 0:bf96e953cdb8 144 counter++;
echo_piyo 0:bf96e953cdb8 145 break;
echo_piyo 0:bf96e953cdb8 146 case 1:
echo_piyo 4:a6cc2f03e69b 147 rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4()));
echo_piyo 0:bf96e953cdb8 148 counter ++;
echo_piyo 0:bf96e953cdb8 149 break;
echo_piyo 0:bf96e953cdb8 150 case 2:
echo_piyo 10:04f2a82cfd89 151 rs422.put(id[counter], sbdbt.right_y, 0.0);
echo_piyo 0:bf96e953cdb8 152 counter = 0;
echo_piyo 0:bf96e953cdb8 153 break;
echo_piyo 0:bf96e953cdb8 154 default:
echo_piyo 0:bf96e953cdb8 155 break;
echo_piyo 0:bf96e953cdb8 156 };
echo_piyo 5:6efda58ff71b 157 }
echo_piyo 5:6efda58ff71b 158