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@28:72b9af7e1700, 2017-08-24 (annotated)

Committer:
echo_piyo
Date:
Thu Aug 24 17:50:01 2017 +0000
Revision:
28:72b9af7e1700
Parent:
27:2ffbc5587399
Child:
29:f380201ca30f
SecHack365????????????? ; ???????????

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