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@14:aac2f18f6779, 2017-08-04 (annotated)

Committer:
echo_piyo
Date:
Fri Aug 04 00:16:44 2017 +0000
Revision:
14:aac2f18f6779
Parent:
13:dfae731e239f
Child:
15:0fdf483769bf
???????????????????????????

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 14:aac2f18f6779 13 #include "cyclic.h"
echo_piyo 14:aac2f18f6779 14 #include "cyclic_IO.h"
echo_piyo 13:dfae731e239f 15 //#include "QEI.h"
echo_piyo 0:bf96e953cdb8 16
echo_piyo 0:bf96e953cdb8 17 #define pc_baud 460800
echo_piyo 0:bf96e953cdb8 18 #define sbdbt_tx p13
echo_piyo 0:bf96e953cdb8 19 #define sbdbt_rx p14
echo_piyo 0:bf96e953cdb8 20 #define sbdbt_baud 115200
echo_piyo 0:bf96e953cdb8 21 #define rs422_tx p28
echo_piyo 0:bf96e953cdb8 22 #define rs422_rx p27
echo_piyo 0:bf96e953cdb8 23 #define rs422_baud 115200
echo_piyo 3:9ef929639c4a 24 #define output_period 0.015
echo_piyo 0:bf96e953cdb8 25 #define nucleo_num 3
echo_piyo 0:bf96e953cdb8 26 #define pi 3.141592
echo_piyo 0:bf96e953cdb8 27 #define n1_id 3
echo_piyo 0:bf96e953cdb8 28 #define n2_id 4
echo_piyo 10:04f2a82cfd89 29 #define n3_id 5
echo_piyo 14:aac2f18f6779 30 #define n4_id 6
echo_piyo 14:aac2f18f6779 31 #define n5_id 7
echo_piyo 5:6efda58ff71b 32 #define yaw_Kp 0.01
echo_piyo 5:6efda58ff71b 33 #define yaw_Ki 0.01
echo_piyo 5:6efda58ff71b 34 #define yaw_Kd 0.01
echo_piyo 10:04f2a82cfd89 35 #define acceleration 25
echo_piyo 10:04f2a82cfd89 36 #define pin_cylinder_on p17
echo_piyo 10:04f2a82cfd89 37 #define pin_cylinder_off p18
echo_piyo 13:dfae731e239f 38 #define pin_interrupt p23
echo_piyo 12:1fec80ae8a2c 39 #define encoder_A p25
echo_piyo 12:1fec80ae8a2c 40 #define encoder_B p26
echo_piyo 10:04f2a82cfd89 41 #define enc_Kp 0.01
echo_piyo 10:04f2a82cfd89 42 #define enc_Ki 0.01
echo_piyo 10:04f2a82cfd89 43 #define enc_Kd 0.01
echo_piyo 0:bf96e953cdb8 44
echo_piyo 13:dfae731e239f 45 DigitalOut led1(LED1);
echo_piyo 13:dfae731e239f 46 DigitalIn interrupt(pin_interrupt);
echo_piyo 0:bf96e953cdb8 47 Serial pc(USBTX,USBRX);
echo_piyo 0:bf96e953cdb8 48 RS422 rs422(rs422_tx, rs422_rx);
echo_piyo 0:bf96e953cdb8 49 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
echo_piyo 0:bf96e953cdb8 50 Ticker output_timer;
echo_piyo 0:bf96e953cdb8 51 Mecanum mecanum;
echo_piyo 0:bf96e953cdb8 52 Bno055 bno055;
echo_piyo 2:d5b8f8e62923 53 Position_pid yaw_pid;
echo_piyo 4:a6cc2f03e69b 54 Accel v1;
echo_piyo 4:a6cc2f03e69b 55 Accel v2;
echo_piyo 4:a6cc2f03e69b 56 Accel v3;
echo_piyo 4:a6cc2f03e69b 57 Accel v4;
echo_piyo 13:dfae731e239f 58 Cyclic_IO led2(LED2);
echo_piyo 13:dfae731e239f 59 Cyclic_IO sylinder_on(pin_cylinder_on);
echo_piyo 13:dfae731e239f 60 Cyclic_IO sylinder_off(pin_cylinder_off);
echo_piyo 14:aac2f18f6779 61 Cyclic cylinder_cyclic;
echo_piyo 12:1fec80ae8a2c 62
echo_piyo 12:1fec80ae8a2c 63 //追加点
echo_piyo 13:dfae731e239f 64 Encoder enc_cylinder(encoder_A,encoder_B);
echo_piyo 13:dfae731e239f 65 //QEI wheel(encoder_A, encoder_B, NC, 624);
echo_piyo 9:6486f4b3ac50 66
echo_piyo 0:bf96e953cdb8 67 void setup();
echo_piyo 0:bf96e953cdb8 68 void output();
echo_piyo 0:bf96e953cdb8 69 void put_output();
echo_piyo 10:04f2a82cfd89 70 void cylinder_check();
echo_piyo 10:04f2a82cfd89 71 void boost();
echo_piyo 12:1fec80ae8a2c 72 void cylinder_origin();
echo_piyo 2:d5b8f8e62923 73 float yaw, target_yaw;
echo_piyo 0:bf96e953cdb8 74
echo_piyo 0:bf96e953cdb8 75 int main()
echo_piyo 0:bf96e953cdb8 76 {
echo_piyo 0:bf96e953cdb8 77 setup();
echo_piyo 2:d5b8f8e62923 78 while(1) {
echo_piyo 14:aac2f18f6779 79 //led1 = interrupt;
echo_piyo 13:dfae731e239f 80 //pc.printf("Pulses is: %i\r\n",wheel.getPulses());
echo_piyo 0:bf96e953cdb8 81 }
echo_piyo 0:bf96e953cdb8 82 }
echo_piyo 0:bf96e953cdb8 83
echo_piyo 0:bf96e953cdb8 84 void setup()
echo_piyo 0:bf96e953cdb8 85 {
echo_piyo 1:2d878962e6ea 86 wait(1);
echo_piyo 0:bf96e953cdb8 87 bno055.begin();
echo_piyo 0:bf96e953cdb8 88 wait(1);
echo_piyo 0:bf96e953cdb8 89 bno055.firstRead();
echo_piyo 0:bf96e953cdb8 90 pc.baud(pc_baud);
echo_piyo 0:bf96e953cdb8 91 sbdbt.begin(sbdbt_baud);
echo_piyo 0:bf96e953cdb8 92 rs422.begin(rs422_baud);
echo_piyo 14:aac2f18f6779 93 cylinder_origin();
echo_piyo 0:bf96e953cdb8 94 output_timer.attach(&output, output_period);
echo_piyo 2:d5b8f8e62923 95 yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
echo_piyo 10:04f2a82cfd89 96 mecanum.setupdeg(bno055.getYawRad()+180.0);
echo_piyo 4:a6cc2f03e69b 97 v1.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 98 v2.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 99 v3.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 100 v4.setup(acceleration,output_period);
echo_piyo 12:1fec80ae8a2c 101
echo_piyo 12:1fec80ae8a2c 102 //追加点
echo_piyo 13:dfae731e239f 103 enc_cylinder.setup(1200);
echo_piyo 13:dfae731e239f 104 enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
echo_piyo 12:1fec80ae8a2c 105 }
echo_piyo 12:1fec80ae8a2c 106
echo_piyo 12:1fec80ae8a2c 107 void cylinder_origin(){
echo_piyo 12:1fec80ae8a2c 108 while(interrupt){
echo_piyo 14:aac2f18f6779 109 led1 = 1;
echo_piyo 12:1fec80ae8a2c 110 rs422.put(5, -0.8, 0.0);
echo_piyo 12:1fec80ae8a2c 111 }
echo_piyo 14:aac2f18f6779 112 led1 = 0;
echo_piyo 0:bf96e953cdb8 113 }
echo_piyo 0:bf96e953cdb8 114
echo_piyo 10:04f2a82cfd89 115 void boost(){
echo_piyo 10:04f2a82cfd89 116 if(sbdbt.sankaku) {
echo_piyo 10:04f2a82cfd89 117 mecanum.boost_forward();
echo_piyo 10:04f2a82cfd89 118 }
echo_piyo 10:04f2a82cfd89 119 if(sbdbt.batu) {
echo_piyo 10:04f2a82cfd89 120 mecanum.boost_back();
echo_piyo 10:04f2a82cfd89 121 }
echo_piyo 10:04f2a82cfd89 122 if(sbdbt.shikaku) {
echo_piyo 10:04f2a82cfd89 123 mecanum.boost_left();
echo_piyo 10:04f2a82cfd89 124 }
echo_piyo 10:04f2a82cfd89 125 if(sbdbt.maru) {
echo_piyo 10:04f2a82cfd89 126 mecanum.boost_right();
echo_piyo 10:04f2a82cfd89 127 }
echo_piyo 10:04f2a82cfd89 128 }
echo_piyo 10:04f2a82cfd89 129
echo_piyo 10:04f2a82cfd89 130 void cylinder_check()
echo_piyo 10:04f2a82cfd89 131 {
echo_piyo 13:dfae731e239f 132 //printf("up\t%d\tstate\t%d\tdown\t%d\tstate\t%d\t\r\n",sbdbt.up,sylinder_on.getState(),sbdbt.down,sylinder_off.getState());
echo_piyo 13:dfae731e239f 133 /*
echo_piyo 13:dfae731e239f 134 if(sylinder_on?){
echo_piyo 13:dfae731e239f 135 sylinder_on.cyclic(sbdbt.up);
echo_piyo 13:dfae731e239f 136 }else if(){
echo_piyo 13:dfae731e239f 137 sylinder_off.cyclic(sbdbt.down);
echo_piyo 10:04f2a82cfd89 138 }
echo_piyo 13:dfae731e239f 139 led2.cyclic(sbdbt.left);
echo_piyo 13:dfae731e239f 140 */
echo_piyo 14:aac2f18f6779 141
echo_piyo 14:aac2f18f6779 142 cylinder_cyclic.cyclic(sbdbt.up);
echo_piyo 14:aac2f18f6779 143 if(cylinder_cyclic.getState()== 1){
echo_piyo 14:aac2f18f6779 144 led1 = 1;
echo_piyo 14:aac2f18f6779 145 }else{
echo_piyo 14:aac2f18f6779 146 led1 = 0;
echo_piyo 14:aac2f18f6779 147 }
echo_piyo 10:04f2a82cfd89 148 }
echo_piyo 10:04f2a82cfd89 149
echo_piyo 5:6efda58ff71b 150 void put_output()
echo_piyo 5:6efda58ff71b 151 {
echo_piyo 4:a6cc2f03e69b 152 yaw = bno055.getYawRad();
echo_piyo 4:a6cc2f03e69b 153 target_yaw = yaw;
echo_piyo 4:a6cc2f03e69b 154 yaw_pid.cal(target_yaw, yaw, output_period);
echo_piyo 4:a6cc2f03e69b 155 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad());
echo_piyo 5:6efda58ff71b 156 // 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 157 }
echo_piyo 0:bf96e953cdb8 158
echo_piyo 0:bf96e953cdb8 159 void output()
echo_piyo 0:bf96e953cdb8 160 {
echo_piyo 4:a6cc2f03e69b 161 put_output();
echo_piyo 12:1fec80ae8a2c 162 cylinder_check();
echo_piyo 10:04f2a82cfd89 163 boost();
echo_piyo 4:a6cc2f03e69b 164
echo_piyo 0:bf96e953cdb8 165 static int counter;
echo_piyo 0:bf96e953cdb8 166 int id[nucleo_num] = {n1_id, n2_id, n3_id};
echo_piyo 0:bf96e953cdb8 167
echo_piyo 0:bf96e953cdb8 168 switch (counter) {
echo_piyo 0:bf96e953cdb8 169 case 0:
echo_piyo 4:a6cc2f03e69b 170 rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
echo_piyo 0:bf96e953cdb8 171 counter++;
echo_piyo 0:bf96e953cdb8 172 break;
echo_piyo 0:bf96e953cdb8 173 case 1:
echo_piyo 4:a6cc2f03e69b 174 rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4()));
echo_piyo 0:bf96e953cdb8 175 counter ++;
echo_piyo 0:bf96e953cdb8 176 break;
echo_piyo 0:bf96e953cdb8 177 case 2:
echo_piyo 10:04f2a82cfd89 178 rs422.put(id[counter], sbdbt.right_y, 0.0);
echo_piyo 0:bf96e953cdb8 179 counter = 0;
echo_piyo 0:bf96e953cdb8 180 break;
echo_piyo 0:bf96e953cdb8 181 default:
echo_piyo 0:bf96e953cdb8 182 break;
echo_piyo 0:bf96e953cdb8 183 };
echo_piyo 14:aac2f18f6779 184 }