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@31:285c9898da03, 2017-08-29 (annotated)

Committer:
echo_piyo
Date:
Tue Aug 29 02:07:56 2017 +0000
Revision:
31:285c9898da03
Parent:
30:57061e222f10
Child:
32:f535ace7c529
(?)??????????;

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 31:285c9898da03 41 #define pin_interrupt_cylinder_min 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 31:285c9898da03 50 #define pin_interrupt_sholderright_max p23
echo_piyo 31:285c9898da03 51 #define pin_interrupt_sholderright_min p24
echo_piyo 31:285c9898da03 52 #define pin_interrupt_sholderleft_max p29
echo_piyo 31:285c9898da03 53 #define pin_interrupt_sholderleft_min p30
echo_piyo 0:bf96e953cdb8 54
echo_piyo 13:dfae731e239f 55 DigitalOut led1(LED1);
echo_piyo 15:0fdf483769bf 56 //DigitalOut led2(LED2);
echo_piyo 15:0fdf483769bf 57 DigitalOut led3(LED3);
echo_piyo 15:0fdf483769bf 58 //DigitalOut led4(LED4);
echo_piyo 31:285c9898da03 59 DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
echo_piyo 0:bf96e953cdb8 60 Serial pc(USBTX,USBRX);
echo_piyo 0:bf96e953cdb8 61 RS422 rs422(rs422_tx, rs422_rx);
echo_piyo 0:bf96e953cdb8 62 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
echo_piyo 0:bf96e953cdb8 63 Ticker output_timer;
echo_piyo 0:bf96e953cdb8 64 Mecanum mecanum;
echo_piyo 0:bf96e953cdb8 65 Bno055 bno055;
echo_piyo 2:d5b8f8e62923 66 Position_pid yaw_pid;
echo_piyo 4:a6cc2f03e69b 67 Accel v1;
echo_piyo 4:a6cc2f03e69b 68 Accel v2;
echo_piyo 4:a6cc2f03e69b 69 Accel v3;
echo_piyo 4:a6cc2f03e69b 70 Accel v4;
echo_piyo 15:0fdf483769bf 71 Cylinder cylinder(pin_cylinder_on,pin_cylinder_off);
echo_piyo 26:3280d0300b04 72 Cyclic sword;
echo_piyo 31:285c9898da03 73 Cyclic cyclic_cylinder_position;
echo_piyo 31:285c9898da03 74 DigitalIn interrupt_sholderright_min(pin_interrupt_sholderright_min);
echo_piyo 31:285c9898da03 75 DigitalIn interrupt_sholderright_max(pin_interrupt_sholderright_max);
echo_piyo 31:285c9898da03 76 DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min);
echo_piyo 31:285c9898da03 77 DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max);
echo_piyo 31:285c9898da03 78
echo_piyo 31:285c9898da03 79
echo_piyo 12:1fec80ae8a2c 80 //追加点
echo_piyo 13:dfae731e239f 81 Encoder enc_cylinder(encoder_A,encoder_B);
echo_piyo 13:dfae731e239f 82 //QEI wheel(encoder_A, encoder_B, NC, 624);
echo_piyo 28:72b9af7e1700 83 Servo servo_reload(pin_servo_reload);
echo_piyo 28:72b9af7e1700 84 DigitalIn interrupt_reload(pin_interrupt_reload);
echo_piyo 9:6486f4b3ac50 85
echo_piyo 0:bf96e953cdb8 86 void setup();
echo_piyo 0:bf96e953cdb8 87 void output();
echo_piyo 15:0fdf483769bf 88 void motor_cal();
echo_piyo 16:e49df474e4c6 89 void cylinder_cal();
echo_piyo 10:04f2a82cfd89 90 void boost();
echo_piyo 12:1fec80ae8a2c 91 void cylinder_origin();
echo_piyo 26:3280d0300b04 92 void sword_cal();
echo_piyo 31:285c9898da03 93 float shoulder_right_cal();
echo_piyo 31:285c9898da03 94 float shoulder_left_cal();
echo_piyo 30:57061e222f10 95 int reload_magazine_flag;
echo_piyo 2:d5b8f8e62923 96 float yaw, target_yaw;
echo_piyo 0:bf96e953cdb8 97
echo_piyo 31:285c9898da03 98 //riseEventそのうちClassにしたい
echo_piyo 31:285c9898da03 99 short state;
echo_piyo 31:285c9898da03 100 int riseEvent(int input)
echo_piyo 31:285c9898da03 101 {
echo_piyo 31:285c9898da03 102 state = ((state<<1)|input)&3;
echo_piyo 31:285c9898da03 103 if(state == 1) {
echo_piyo 31:285c9898da03 104 return 1;
echo_piyo 31:285c9898da03 105 } else {
echo_piyo 31:285c9898da03 106 return 0;
echo_piyo 31:285c9898da03 107 }
echo_piyo 31:285c9898da03 108 }
echo_piyo 31:285c9898da03 109
echo_piyo 0:bf96e953cdb8 110 int main()
echo_piyo 0:bf96e953cdb8 111 {
echo_piyo 0:bf96e953cdb8 112 setup();
echo_piyo 2:d5b8f8e62923 113 while(1) {
echo_piyo 31:285c9898da03 114 //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
echo_piyo 31:285c9898da03 115 //pc.printf("rise state : %d\r\n",riseEvent(sbdbt.right));
echo_piyo 0:bf96e953cdb8 116 }
echo_piyo 0:bf96e953cdb8 117 }
echo_piyo 0:bf96e953cdb8 118
echo_piyo 0:bf96e953cdb8 119 void setup()
echo_piyo 0:bf96e953cdb8 120 {
echo_piyo 1:2d878962e6ea 121 wait(1);
echo_piyo 19:76a387e4bcf6 122 bno055.begin();
echo_piyo 0:bf96e953cdb8 123 wait(1);
echo_piyo 19:76a387e4bcf6 124 bno055.firstRead();
echo_piyo 0:bf96e953cdb8 125 pc.baud(pc_baud);
echo_piyo 0:bf96e953cdb8 126 sbdbt.begin(sbdbt_baud);
echo_piyo 0:bf96e953cdb8 127 rs422.begin(rs422_baud);
echo_piyo 19:76a387e4bcf6 128 cylinder_origin();
echo_piyo 0:bf96e953cdb8 129 output_timer.attach(&output, output_period);
echo_piyo 2:d5b8f8e62923 130 yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
echo_piyo 10:04f2a82cfd89 131 mecanum.setupdeg(bno055.getYawRad()+180.0);
echo_piyo 4:a6cc2f03e69b 132 v1.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 133 v2.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 134 v3.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 135 v4.setup(acceleration,output_period);
echo_piyo 12:1fec80ae8a2c 136
echo_piyo 12:1fec80ae8a2c 137 //追加点
echo_piyo 31:285c9898da03 138 enc_cylinder.setup(100);
echo_piyo 13:dfae731e239f 139 enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
echo_piyo 12:1fec80ae8a2c 140 }
echo_piyo 12:1fec80ae8a2c 141
echo_piyo 15:0fdf483769bf 142 void output()
echo_piyo 15:0fdf483769bf 143 {
echo_piyo 15:0fdf483769bf 144 motor_cal();
echo_piyo 16:e49df474e4c6 145 cylinder_cal();
echo_piyo 26:3280d0300b04 146 sword_cal();
echo_piyo 18:ae4d97945b83 147 //boost();
echo_piyo 15:0fdf483769bf 148
echo_piyo 15:0fdf483769bf 149 static int counter;
echo_piyo 28:72b9af7e1700 150 int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
echo_piyo 15:0fdf483769bf 151
echo_piyo 15:0fdf483769bf 152 switch (counter) {
echo_piyo 15:0fdf483769bf 153 case 0:
echo_piyo 24:da0fc94add43 154 rs422.put(id[counter], v1.duty(mecanum.v1()*powerdown), v3.duty(mecanum.v3()*powerdown));
echo_piyo 15:0fdf483769bf 155 counter++;
echo_piyo 15:0fdf483769bf 156 break;
echo_piyo 15:0fdf483769bf 157 case 1:
echo_piyo 24:da0fc94add43 158 rs422.put(id[counter], v2.duty(mecanum.v2()*powerdown), v4.duty(mecanum.v4()*powerdown));
echo_piyo 15:0fdf483769bf 159 counter ++;
echo_piyo 15:0fdf483769bf 160 break;
echo_piyo 15:0fdf483769bf 161 case 2:
echo_piyo 19:76a387e4bcf6 162 rs422.put(id[counter], sbdbt.right_y, 0.0);
echo_piyo 17:5c4718c603dc 163 counter ++;
echo_piyo 17:5c4718c603dc 164 break;
echo_piyo 17:5c4718c603dc 165 case 3:
echo_piyo 31:285c9898da03 166 rs422.put(id[counter], shoulder_right_cal(),shoulder_right_cal());
echo_piyo 20:347daec6c9a3 167 counter ++;
echo_piyo 20:347daec6c9a3 168 break;
echo_piyo 20:347daec6c9a3 169 case 4:
echo_piyo 26:3280d0300b04 170 rs422.put(id[counter], ((float)sword.getState()*0.8),0.0);
echo_piyo 15:0fdf483769bf 171 counter = 0;
echo_piyo 15:0fdf483769bf 172 break;
echo_piyo 15:0fdf483769bf 173 default:
echo_piyo 15:0fdf483769bf 174 break;
echo_piyo 15:0fdf483769bf 175 };
echo_piyo 15:0fdf483769bf 176 }
echo_piyo 15:0fdf483769bf 177
echo_piyo 31:285c9898da03 178 float shoulder_right_cal(){
echo_piyo 31:285c9898da03 179 if(interrupt_sholderright_max==1&&sbdbt.batu==1){
echo_piyo 31:285c9898da03 180 return 0.0;
echo_piyo 31:285c9898da03 181 }
echo_piyo 31:285c9898da03 182 if(interrupt_sholderright_min==1&&sbdbt.sankaku==1){
echo_piyo 31:285c9898da03 183 return 0.0;
echo_piyo 31:285c9898da03 184 }
echo_piyo 31:285c9898da03 185 return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
echo_piyo 31:285c9898da03 186 }
echo_piyo 31:285c9898da03 187
echo_piyo 31:285c9898da03 188 float shoulder_left_cal(){
echo_piyo 31:285c9898da03 189 if(interrupt_sholderright_max==1&&sbdbt.up==1){
echo_piyo 31:285c9898da03 190 return 0.0;
echo_piyo 31:285c9898da03 191 }
echo_piyo 31:285c9898da03 192 if(interrupt_sholderright_min==1&&sbdbt.down==1){
echo_piyo 31:285c9898da03 193 return 0.0;
echo_piyo 31:285c9898da03 194 }
echo_piyo 31:285c9898da03 195 return (sbdbt.up*0.8-sbdbt.down*0.8);
echo_piyo 31:285c9898da03 196 }
echo_piyo 31:285c9898da03 197
echo_piyo 31:285c9898da03 198 void sword_cal()
echo_piyo 31:285c9898da03 199 {
echo_piyo 26:3280d0300b04 200 sword.cyclic(sbdbt.maru);
echo_piyo 17:5c4718c603dc 201 }
echo_piyo 15:0fdf483769bf 202
echo_piyo 31:285c9898da03 203 void cylinder_origin()
echo_piyo 31:285c9898da03 204 {
echo_piyo 31:285c9898da03 205 while(interrupt_cylinder_min == 1) {
echo_piyo 14:aac2f18f6779 206 led1 = 1;
echo_piyo 31:285c9898da03 207 rs422.put(5, -0.2, 0.0);
echo_piyo 12:1fec80ae8a2c 208 }
echo_piyo 14:aac2f18f6779 209 led1 = 0;
echo_piyo 31:285c9898da03 210 enc_cylinder.origin();
echo_piyo 0:bf96e953cdb8 211 }
echo_piyo 0:bf96e953cdb8 212
echo_piyo 28:72b9af7e1700 213 //追記(動作未確認)
echo_piyo 31:285c9898da03 214 float cylinder_pwm;
echo_piyo 31:285c9898da03 215 int cylinder_pos_num = 0;
echo_piyo 31:285c9898da03 216 float cylinder_pos[3] = {0.0,50.0,75.0};
echo_piyo 19:76a387e4bcf6 217 void cylinder_cal()
echo_piyo 31:285c9898da03 218 {
echo_piyo 31:285c9898da03 219 cylinder.cyclic(sbdbt.shikaku); //cylinder ON/OFF
echo_piyo 31:285c9898da03 220
echo_piyo 31:285c9898da03 221 if(riseEvent(sbdbt.right)){ //cylinder degset
echo_piyo 31:285c9898da03 222 cylinder_pos_num++;
echo_piyo 31:285c9898da03 223 if(cylinder_pos_num >= 3){
echo_piyo 31:285c9898da03 224 cylinder_pos_num = 0;
echo_piyo 31:285c9898da03 225 }
echo_piyo 31:285c9898da03 226 }
echo_piyo 31:285c9898da03 227 enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定
echo_piyo 31:285c9898da03 228
echo_piyo 31:285c9898da03 229 pc.printf("terget\t%f\tnow_pulse\t%lo\tnow_rad\t%f\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.pulse(),enc_cylinder.deg(),enc_cylinder.duty());
echo_piyo 28:72b9af7e1700 230
echo_piyo 31:285c9898da03 231 //リロード機構完成後
echo_piyo 31:285c9898da03 232 /*
echo_piyo 31:285c9898da03 233 if(cylinder.getInState() == 1){
echo_piyo 31:285c9898da03 234 if(interrupt_cylinder_min)servo_reload = 1.0;
echo_piyo 31:285c9898da03 235 }else{
echo_piyo 31:285c9898da03 236 servo_reload = 0.0;
echo_piyo 31:285c9898da03 237 reload_magazine_flag = 1;
echo_piyo 31:285c9898da03 238 }
echo_piyo 31:285c9898da03 239
echo_piyo 31:285c9898da03 240 if(reload_magazine_flag == 1){
echo_piyo 31:285c9898da03 241 rs422.put(n6_id, 0.8, 0.0);
echo_piyo 31:285c9898da03 242 if(interrupt_reload == 1)reload_magazine_flag = 0;
echo_piyo 31:285c9898da03 243 }
echo_piyo 31:285c9898da03 244 */
echo_piyo 19:76a387e4bcf6 245 }
echo_piyo 19:76a387e4bcf6 246
echo_piyo 31:285c9898da03 247 void boost()
echo_piyo 31:285c9898da03 248 {
echo_piyo 31:285c9898da03 249 if(sbdbt.r2) {
echo_piyo 10:04f2a82cfd89 250 mecanum.boost_forward();
echo_piyo 10:04f2a82cfd89 251 }
echo_piyo 31:285c9898da03 252 if(sbdbt.l2) {
echo_piyo 10:04f2a82cfd89 253 mecanum.boost_back();
echo_piyo 10:04f2a82cfd89 254 }
echo_piyo 26:3280d0300b04 255 /*
echo_piyo 10:04f2a82cfd89 256 if(sbdbt.shikaku) {
echo_piyo 10:04f2a82cfd89 257 mecanum.boost_left();
echo_piyo 10:04f2a82cfd89 258 }
echo_piyo 10:04f2a82cfd89 259 if(sbdbt.maru) {
echo_piyo 10:04f2a82cfd89 260 mecanum.boost_right();
echo_piyo 10:04f2a82cfd89 261 }
echo_piyo 26:3280d0300b04 262 */
echo_piyo 10:04f2a82cfd89 263 }
echo_piyo 10:04f2a82cfd89 264
echo_piyo 15:0fdf483769bf 265 void motor_cal()
echo_piyo 5:6efda58ff71b 266 {
echo_piyo 4:a6cc2f03e69b 267 yaw = bno055.getYawRad();
echo_piyo 4:a6cc2f03e69b 268 target_yaw = yaw;
echo_piyo 4:a6cc2f03e69b 269 yaw_pid.cal(target_yaw, yaw, output_period);
echo_piyo 4:a6cc2f03e69b 270 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad());
echo_piyo 5:6efda58ff71b 271 // 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 272 }