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@69:b2c8a0c5662c, 2017-09-24 (annotated)

Committer:
echo_piyo
Date:
Sun Sep 24 08:32:07 2017 +0000
Revision:
69:b2c8a0c5662c
Parent:
68:18cd96c13fa1
Child:
71:7d684ff43ddd
(?)9?14??????????????; ???????????????????????; ?????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 56:a7bd860b85b6 1 //index_include
echo_piyo 0:bf96e953cdb8 2 #include "mbed.h"
echo_piyo 4:a6cc2f03e69b 3 #include "math.h"
echo_piyo 68:18cd96c13fa1 4 #include "bit_test.h"
echo_piyo 66:1664ee92539d 5 #include "rs422_put.h"
echo_piyo 0:bf96e953cdb8 6 #include "sbdbt.h"
echo_piyo 0:bf96e953cdb8 7 #include "mecanum.h"
echo_piyo 0:bf96e953cdb8 8 #include "bno055_lib.h"
echo_piyo 0:bf96e953cdb8 9 #include "bno055_use.h"
echo_piyo 0:bf96e953cdb8 10 #include "pid.h"
echo_piyo 0:bf96e953cdb8 11 #include "limit.h"
echo_piyo 4:a6cc2f03e69b 12 #include "accelerator.h"
echo_piyo 66:1664ee92539d 13 #include "pid_encoder.h"
echo_piyo 66:1664ee92539d 14 #include "cyclic_var.h"
echo_piyo 66:1664ee92539d 15 #include "cyclic_io.h"
echo_piyo 15:0fdf483769bf 16 #include "cylinder.h"
echo_piyo 66:1664ee92539d 17 #include "event_var.h"
echo_piyo 68:18cd96c13fa1 18 #include "servo.h"
echo_piyo 0:bf96e953cdb8 19
echo_piyo 69:b2c8a0c5662c 20
echo_piyo 56:a7bd860b85b6 21 //index_define
echo_piyo 0:bf96e953cdb8 22 #define pc_baud 460800
echo_piyo 0:bf96e953cdb8 23 #define sbdbt_tx p13
echo_piyo 0:bf96e953cdb8 24 #define sbdbt_rx p14
echo_piyo 0:bf96e953cdb8 25 #define sbdbt_baud 115200
echo_piyo 0:bf96e953cdb8 26 #define rs422_tx p28
echo_piyo 0:bf96e953cdb8 27 #define rs422_rx p27
echo_piyo 0:bf96e953cdb8 28 #define rs422_baud 115200
echo_piyo 3:9ef929639c4a 29 #define output_period 0.015
echo_piyo 28:72b9af7e1700 30 #define nucleo_num 6
echo_piyo 0:bf96e953cdb8 31 #define pi 3.141592
echo_piyo 0:bf96e953cdb8 32 #define n1_id 3
echo_piyo 0:bf96e953cdb8 33 #define n2_id 4
echo_piyo 10:04f2a82cfd89 34 #define n3_id 5
echo_piyo 14:aac2f18f6779 35 #define n4_id 6
echo_piyo 14:aac2f18f6779 36 #define n5_id 7
echo_piyo 28:72b9af7e1700 37 #define n6_id 8
echo_piyo 5:6efda58ff71b 38 #define yaw_Kp 0.01
echo_piyo 5:6efda58ff71b 39 #define yaw_Ki 0.01
echo_piyo 5:6efda58ff71b 40 #define yaw_Kd 0.01
echo_piyo 54:a88208d9bc1d 41 #define acceleration 15 //25
echo_piyo 59:68a73b9d27f5 42 #define pin_cylinder_on p18
echo_piyo 59:68a73b9d27f5 43 #define pin_cylinder_off p17
echo_piyo 64:41dcec6c20bc 44 #define pin_interrupt_cylinder_min p29
echo_piyo 64:41dcec6c20bc 45 #define encoder_A p21
echo_piyo 64:41dcec6c20bc 46 #define encoder_B p22
echo_piyo 40:2d6888448ab2 47 #define enc_Kp 0.0400
echo_piyo 33:64fd1bd83bac 48 #define enc_Ki 0.0001
echo_piyo 33:64fd1bd83bac 49 #define enc_Kd 0.0003
echo_piyo 34:02d605c68bf3 50 #define mecanum_power 1.0
echo_piyo 52:f5ae47e683fa 51 #define sword_power 1.0
echo_piyo 52:f5ae47e683fa 52 #define sholder_power 1.0
echo_piyo 47:6ea046767494 53 #define pin_interrupt_sholderright_max p19 //p21
echo_piyo 47:6ea046767494 54 #define pin_interrupt_sholderright_min p20 //p22
echo_piyo 33:64fd1bd83bac 55 #define pin_interrupt_sholderleft_max p7
echo_piyo 33:64fd1bd83bac 56 #define pin_interrupt_sholderleft_min p8
echo_piyo 64:41dcec6c20bc 57 //#define pin_servo p21
echo_piyo 35:5e1ad00f26fb 58 #define servo_reload_time 1.0
echo_piyo 43:605e5b0b9106 59 #define pin_cylinder_reload p15
echo_piyo 45:a32e8091901b 60 #define pin_sbdbt_pairing p12
echo_piyo 45:a32e8091901b 61 #define pin_sbdbt_indicator p11
echo_piyo 56:a7bd860b85b6 62 #define cylinder_pos_max 6
echo_piyo 0:bf96e953cdb8 63
echo_piyo 57:68df771fd8a1 64
echo_piyo 56:a7bd860b85b6 65 //index_setupPin
echo_piyo 45:a32e8091901b 66 DigitalOut led1(LED1);
echo_piyo 48:96b5f5ebdfb0 67 DigitalOut led2(LED2);
echo_piyo 48:96b5f5ebdfb0 68 DigitalOut led3(LED3);
echo_piyo 48:96b5f5ebdfb0 69 DigitalOut led4(LED4);
echo_piyo 31:285c9898da03 70 DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
echo_piyo 0:bf96e953cdb8 71 Serial pc(USBTX,USBRX);
echo_piyo 66:1664ee92539d 72 Rs422 rs422(rs422_tx, rs422_rx);
echo_piyo 45:a32e8091901b 73 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx, pin_sbdbt_pairing);
echo_piyo 0:bf96e953cdb8 74 Ticker output_timer;
echo_piyo 0:bf96e953cdb8 75 Mecanum mecanum;
echo_piyo 0:bf96e953cdb8 76 Bno055 bno055;
echo_piyo 66:1664ee92539d 77 PositionPid yaw_pid;
echo_piyo 66:1664ee92539d 78 Accelerator v1;
echo_piyo 66:1664ee92539d 79 Accelerator v2;
echo_piyo 66:1664ee92539d 80 Accelerator v3;
echo_piyo 66:1664ee92539d 81 Accelerator v4;
echo_piyo 58:6cd46488f59b 82 DigitalOut cylinder_on(pin_cylinder_on);
echo_piyo 58:6cd46488f59b 83 DigitalOut cylinder_off(pin_cylinder_off);
echo_piyo 66:1664ee92539d 84 CyclicVar sword;
echo_piyo 66:1664ee92539d 85 CyclicVar cyclic_cylinder_position;
echo_piyo 31:285c9898da03 86 DigitalIn interrupt_sholderright_min(pin_interrupt_sholderright_min);
echo_piyo 31:285c9898da03 87 DigitalIn interrupt_sholderright_max(pin_interrupt_sholderright_max);
echo_piyo 31:285c9898da03 88 DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min);
echo_piyo 31:285c9898da03 89 DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max);
echo_piyo 35:5e1ad00f26fb 90 Encoder enc_cylinder(encoder_A,encoder_B);
echo_piyo 66:1664ee92539d 91 CyclicVar cyclic_servo;
echo_piyo 66:1664ee92539d 92 CyclicIo cylinder_reload(pin_cylinder_reload);
echo_piyo 64:41dcec6c20bc 93 //PwmOut servo(pin_servo);
echo_piyo 45:a32e8091901b 94 DigitalOut sbdbt_indigator(pin_sbdbt_indicator);
echo_piyo 66:1664ee92539d 95 eventVar cylinder_event;
echo_piyo 9:6486f4b3ac50 96
echo_piyo 67:b094d88583be 97
echo_piyo 56:a7bd860b85b6 98 //index_setupFunction
echo_piyo 0:bf96e953cdb8 99 void setup();
echo_piyo 0:bf96e953cdb8 100 void output();
echo_piyo 15:0fdf483769bf 101 void motor_cal();
echo_piyo 16:e49df474e4c6 102 void cylinder_cal();
echo_piyo 10:04f2a82cfd89 103 void boost();
echo_piyo 12:1fec80ae8a2c 104 void cylinder_origin();
echo_piyo 53:701dc7886fff 105 void firstmotion();
echo_piyo 26:3280d0300b04 106 void sword_cal();
echo_piyo 35:5e1ad00f26fb 107 void servo_origin();
echo_piyo 31:285c9898da03 108 float shoulder_right_cal();
echo_piyo 31:285c9898da03 109 float shoulder_left_cal();
echo_piyo 42:63aedf71f4d1 110
echo_piyo 56:a7bd860b85b6 111 //index_setupVariable
echo_piyo 39:6735743ac0f1 112 //output
echo_piyo 2:d5b8f8e62923 113 float yaw, target_yaw;
echo_piyo 39:6735743ac0f1 114 //cylinder_origin
echo_piyo 38:b071512af5ca 115 int cylinder_origin_flag = 0;
echo_piyo 39:6735743ac0f1 116 //cylinder
echo_piyo 39:6735743ac0f1 117 float cylinder_pwm;
echo_piyo 39:6735743ac0f1 118 int cylinder_pos_num = 0;
echo_piyo 56:a7bd860b85b6 119 float cylinder_pos[cylinder_pos_max] = {0.0,90.0,325.0,450.0,580.0,700.0};
echo_piyo 0:bf96e953cdb8 120
echo_piyo 0:bf96e953cdb8 121 int main()
echo_piyo 0:bf96e953cdb8 122 {
echo_piyo 0:bf96e953cdb8 123 setup();
echo_piyo 2:d5b8f8e62923 124 while(1) {
echo_piyo 48:96b5f5ebdfb0 125 led1 = interrupt_sholderleft_max;
echo_piyo 48:96b5f5ebdfb0 126 led2 = interrupt_sholderleft_min;
echo_piyo 48:96b5f5ebdfb0 127 led3 = interrupt_sholderright_max;
echo_piyo 48:96b5f5ebdfb0 128 led4 = interrupt_sholderright_min;
echo_piyo 50:e4e1f38d1bd5 129
echo_piyo 64:41dcec6c20bc 130 //pc.printf("cylinder pos : %f\r\n",enc_cylinder.deg());
echo_piyo 55:2dd2f161ebaf 131 //pc.printf("riseState %d : fallState %d\r\n",event.getRise(),event.getFall());
echo_piyo 0:bf96e953cdb8 132 }
echo_piyo 0:bf96e953cdb8 133 }
echo_piyo 0:bf96e953cdb8 134
echo_piyo 0:bf96e953cdb8 135 void setup()
echo_piyo 0:bf96e953cdb8 136 {
echo_piyo 40:2d6888448ab2 137 wait(2.0);
echo_piyo 65:5e4c2e5494ae 138 bno055.begin();
echo_piyo 40:2d6888448ab2 139 wait(1.0);
echo_piyo 65:5e4c2e5494ae 140 bno055.firstRead();
echo_piyo 0:bf96e953cdb8 141 pc.baud(pc_baud);
echo_piyo 0:bf96e953cdb8 142 sbdbt.begin(sbdbt_baud);
echo_piyo 0:bf96e953cdb8 143 rs422.begin(rs422_baud);
echo_piyo 53:701dc7886fff 144 firstmotion();
echo_piyo 49:b041c815c063 145 cylinder_pos_num = 2; //セットアップタイムでの初期装填のため
echo_piyo 0:bf96e953cdb8 146 output_timer.attach(&output, output_period);
echo_piyo 66:1664ee92539d 147 yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd,output_period);
echo_piyo 39:6735743ac0f1 148 mecanum.setupdeg(bno055.getYawRad()); //基盤が前後逆の場合+180
echo_piyo 4:a6cc2f03e69b 149 v1.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 150 v2.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 151 v3.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 152 v4.setup(acceleration,output_period);
echo_piyo 66:1664ee92539d 153 enc_cylinder.setPpr(100);
echo_piyo 66:1664ee92539d 154 enc_cylinder.setup(enc_Kp,enc_Ki,enc_Kd,output_period);
echo_piyo 64:41dcec6c20bc 155 //servo.period(0.020);
echo_piyo 12:1fec80ae8a2c 156 }
echo_piyo 12:1fec80ae8a2c 157
echo_piyo 36:dca1081c19b3 158 //Sword
echo_piyo 35:5e1ad00f26fb 159 float shoulder_right_cal()
echo_piyo 15:0fdf483769bf 160 {
echo_piyo 40:2d6888448ab2 161 if(interrupt_sholderright_max==0&&sbdbt.sankaku==1) {
echo_piyo 31:285c9898da03 162 return 0.0;
echo_piyo 31:285c9898da03 163 }
echo_piyo 40:2d6888448ab2 164 if(interrupt_sholderright_min==0&&sbdbt.batu==1) {
echo_piyo 33:64fd1bd83bac 165 return 0.0;
echo_piyo 31:285c9898da03 166 }
echo_piyo 31:285c9898da03 167 return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
echo_piyo 31:285c9898da03 168 }
echo_piyo 31:285c9898da03 169
echo_piyo 35:5e1ad00f26fb 170 float shoulder_left_cal()
echo_piyo 35:5e1ad00f26fb 171 {
echo_piyo 40:2d6888448ab2 172 if(interrupt_sholderleft_max==0&&sbdbt.sankaku==1) {
echo_piyo 31:285c9898da03 173 return 0.0;
echo_piyo 31:285c9898da03 174 }
echo_piyo 40:2d6888448ab2 175 if(interrupt_sholderleft_min==0&&sbdbt.batu==1) {
echo_piyo 35:5e1ad00f26fb 176 return 0.0;
echo_piyo 31:285c9898da03 177 }
echo_piyo 40:2d6888448ab2 178 return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
echo_piyo 31:285c9898da03 179 }
echo_piyo 31:285c9898da03 180 void sword_cal()
echo_piyo 31:285c9898da03 181 {
echo_piyo 26:3280d0300b04 182 sword.cyclic(sbdbt.maru);
echo_piyo 17:5c4718c603dc 183 }
echo_piyo 15:0fdf483769bf 184
echo_piyo 36:dca1081c19b3 185 //cylinder
echo_piyo 53:701dc7886fff 186 void firstmotion()
echo_piyo 31:285c9898da03 187 {
echo_piyo 53:701dc7886fff 188 float cylinder = 0.0;
echo_piyo 53:701dc7886fff 189 float sholderL = 0.0;
echo_piyo 53:701dc7886fff 190 float sholderR = 0.0;
echo_piyo 65:5e4c2e5494ae 191 while(interrupt_cylinder_min == 1 || interrupt_sholderleft_max == 1 || interrupt_sholderright_max == 1){
echo_piyo 53:701dc7886fff 192 if(interrupt_cylinder_min == 1){
echo_piyo 53:701dc7886fff 193 cylinder = 2.0;
echo_piyo 53:701dc7886fff 194 }else{
echo_piyo 53:701dc7886fff 195 cylinder = 0.0;
echo_piyo 53:701dc7886fff 196 }
echo_piyo 53:701dc7886fff 197 if(interrupt_sholderright_max == 1){
echo_piyo 53:701dc7886fff 198 sholderR = -2.0;
echo_piyo 53:701dc7886fff 199 }else{
echo_piyo 53:701dc7886fff 200 sholderR = 0.0;
echo_piyo 53:701dc7886fff 201 }
echo_piyo 53:701dc7886fff 202 if(interrupt_sholderleft_max == 1){
echo_piyo 53:701dc7886fff 203 sholderL = 2.0;
echo_piyo 53:701dc7886fff 204 }else{
echo_piyo 53:701dc7886fff 205 sholderL = 0.0;
echo_piyo 53:701dc7886fff 206 }
echo_piyo 53:701dc7886fff 207 rs422.put(5, cylinder, 0.0);
echo_piyo 53:701dc7886fff 208 rs422.put(6, sholderR, sholderL);
echo_piyo 12:1fec80ae8a2c 209 }
echo_piyo 31:285c9898da03 210 enc_cylinder.origin();
echo_piyo 0:bf96e953cdb8 211 }
echo_piyo 38:b071512af5ca 212
echo_piyo 42:63aedf71f4d1 213 //cylinder_origin_flagを1にすることで動作する
echo_piyo 38:b071512af5ca 214 void cylinder_origin()
echo_piyo 38:b071512af5ca 215 {
echo_piyo 45:a32e8091901b 216 if(interrupt_cylinder_min == 0&&cylinder_origin_flag == 1) {
echo_piyo 39:6735743ac0f1 217 cylinder_origin_flag = 0;
echo_piyo 39:6735743ac0f1 218 enc_cylinder.origin();
echo_piyo 39:6735743ac0f1 219 cylinder_pos_num = 0;
echo_piyo 45:a32e8091901b 220 } else if(cylinder_origin_flag == 1) {
echo_piyo 42:63aedf71f4d1 221 rs422.put(5, -0.8, 0.0);
echo_piyo 38:b071512af5ca 222 }
echo_piyo 38:b071512af5ca 223 }
echo_piyo 19:76a387e4bcf6 224 void cylinder_cal()
echo_piyo 31:285c9898da03 225 {
echo_piyo 58:6cd46488f59b 226 //cylinder ON/OFF
echo_piyo 58:6cd46488f59b 227 if(sbdbt.shikaku){
echo_piyo 58:6cd46488f59b 228 cylinder_on = 1;
echo_piyo 58:6cd46488f59b 229 cylinder_off = 0;
echo_piyo 58:6cd46488f59b 230 }else{
echo_piyo 58:6cd46488f59b 231 cylinder_on = 0;
echo_piyo 58:6cd46488f59b 232 cylinder_off = 1;
echo_piyo 58:6cd46488f59b 233 }
echo_piyo 51:70d45b959d6b 234
echo_piyo 51:70d45b959d6b 235 cylinder_event.input(sbdbt.right);
echo_piyo 51:70d45b959d6b 236 if(cylinder_event.getRise()) { //cylinder degset
echo_piyo 31:285c9898da03 237 cylinder_pos_num++;
echo_piyo 55:2dd2f161ebaf 238 if(cylinder_pos_num >= cylinder_pos_max) {
echo_piyo 42:63aedf71f4d1 239 //cylinder_pos_num = 0;
echo_piyo 42:63aedf71f4d1 240 cylinder_origin_flag=1;
echo_piyo 31:285c9898da03 241 }
echo_piyo 31:285c9898da03 242 }
echo_piyo 50:e4e1f38d1bd5 243 enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //set cylinder_tergetPos
echo_piyo 35:5e1ad00f26fb 244
echo_piyo 64:41dcec6c20bc 245 pc.printf("terget\t%f\tnow_deg\t%f\tnow_pulse\t%d\tpwm\t%f\r\n",cylinder_pos[cylinder_pos_num],enc_cylinder.deg(),enc_cylinder.pulse(),enc_cylinder.duty());
echo_piyo 19:76a387e4bcf6 246 }
echo_piyo 38:b071512af5ca 247
echo_piyo 64:41dcec6c20bc 248 //function
echo_piyo 64:41dcec6c20bc 249 /*void servo_max()
echo_piyo 45:a32e8091901b 250 {
echo_piyo 45:a32e8091901b 251 servo.pulsewidth(0.0022);
echo_piyo 35:5e1ad00f26fb 252 }
echo_piyo 38:b071512af5ca 253 void servo_min()
echo_piyo 35:5e1ad00f26fb 254 {
echo_piyo 35:5e1ad00f26fb 255 servo.pulsewidth(0.0010);
echo_piyo 38:b071512af5ca 256 }
echo_piyo 45:a32e8091901b 257 void servo_out()
echo_piyo 45:a32e8091901b 258 {
echo_piyo 39:6735743ac0f1 259 cyclic_servo.cyclic(sbdbt.down); //setServoControl
echo_piyo 45:a32e8091901b 260 if(cyclic_servo.getState()==0) {
echo_piyo 39:6735743ac0f1 261 servo_min();
echo_piyo 45:a32e8091901b 262 } else if(cyclic_servo.getState()==1) {
echo_piyo 39:6735743ac0f1 263 servo_max();
echo_piyo 38:b071512af5ca 264 }
echo_piyo 64:41dcec6c20bc 265 }*/
echo_piyo 35:5e1ad00f26fb 266
echo_piyo 36:dca1081c19b3 267
echo_piyo 64:41dcec6c20bc 268 //functionBoost
echo_piyo 31:285c9898da03 269 void boost()
echo_piyo 31:285c9898da03 270 {
echo_piyo 31:285c9898da03 271 if(sbdbt.r2) {
echo_piyo 10:04f2a82cfd89 272 mecanum.boost_forward();
echo_piyo 10:04f2a82cfd89 273 }
echo_piyo 31:285c9898da03 274 if(sbdbt.l2) {
echo_piyo 10:04f2a82cfd89 275 mecanum.boost_back();
echo_piyo 10:04f2a82cfd89 276 }
echo_piyo 26:3280d0300b04 277 /*
echo_piyo 10:04f2a82cfd89 278 if(sbdbt.shikaku) {
echo_piyo 10:04f2a82cfd89 279 mecanum.boost_left();
echo_piyo 10:04f2a82cfd89 280 }
echo_piyo 10:04f2a82cfd89 281 if(sbdbt.maru) {
echo_piyo 10:04f2a82cfd89 282 mecanum.boost_right();
echo_piyo 10:04f2a82cfd89 283 }
echo_piyo 26:3280d0300b04 284 */
echo_piyo 10:04f2a82cfd89 285 }
echo_piyo 10:04f2a82cfd89 286
echo_piyo 64:41dcec6c20bc 287 //functionMecanum
echo_piyo 15:0fdf483769bf 288 void motor_cal()
echo_piyo 5:6efda58ff71b 289 {
echo_piyo 56:a7bd860b85b6 290 static float out_right_x;
echo_piyo 4:a6cc2f03e69b 291 yaw = bno055.getYawRad();
echo_piyo 4:a6cc2f03e69b 292 target_yaw = yaw;
echo_piyo 66:1664ee92539d 293 yaw_pid.calculate(target_yaw, yaw);
echo_piyo 56:a7bd860b85b6 294
echo_piyo 56:a7bd860b85b6 295 //後進時に方向が逆転するため
echo_piyo 56:a7bd860b85b6 296 if(sbdbt.right_y < 0.0){
echo_piyo 56:a7bd860b85b6 297 out_right_x = -1.0*sbdbt.right_x;
echo_piyo 56:a7bd860b85b6 298 }else{
echo_piyo 56:a7bd860b85b6 299 out_right_x = sbdbt.right_x;
echo_piyo 56:a7bd860b85b6 300 }
echo_piyo 56:a7bd860b85b6 301 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, out_right_x, bno055.getYawRad());
echo_piyo 5:6efda58ff71b 302 // 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 35:5e1ad00f26fb 303 }
echo_piyo 35:5e1ad00f26fb 304 void output()
echo_piyo 35:5e1ad00f26fb 305 {
echo_piyo 35:5e1ad00f26fb 306 motor_cal();
echo_piyo 35:5e1ad00f26fb 307 cylinder_cal();
echo_piyo 35:5e1ad00f26fb 308 sword_cal();
echo_piyo 64:41dcec6c20bc 309 //servo_out();
echo_piyo 38:b071512af5ca 310 cylinder_origin();
echo_piyo 35:5e1ad00f26fb 311 //boost();
echo_piyo 56:a7bd860b85b6 312 sbdbt_indigator = sbdbt.get_pairingState();
echo_piyo 48:96b5f5ebdfb0 313 //led1 = sbdbt.get_pairingState();
echo_piyo 49:b041c815c063 314 if(sbdbt.up)cylinder_origin_flag = 1;
echo_piyo 49:b041c815c063 315 if(sbdbt.left)bno055.yaw_origin();
echo_piyo 49:b041c815c063 316 cylinder_reload.cyclic(sbdbt.down);
echo_piyo 45:a32e8091901b 317
echo_piyo 35:5e1ad00f26fb 318 static int counter;
echo_piyo 53:701dc7886fff 319 static float mecanumV1, mecanumV2, mecanumV3, mecanumV4;
echo_piyo 35:5e1ad00f26fb 320 int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
echo_piyo 45:a32e8091901b 321
echo_piyo 45:a32e8091901b 322 //sbdbtがpairingしている場合のみ動作
echo_piyo 45:a32e8091901b 323 if(sbdbt.get_pairingState()) {
echo_piyo 45:a32e8091901b 324 switch (counter) {
echo_piyo 45:a32e8091901b 325 case 0:
echo_piyo 53:701dc7886fff 326 rs422.put(id[counter], v1.duty(limitf(((mecanum.v1()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)), v3.duty(limitf(((mecanum.v3()*mecanum_power)+(sbdbt.right_y*0.8)),1.0,-1.0)));
echo_piyo 45:a32e8091901b 327 counter++;
echo_piyo 45:a32e8091901b 328 break;
echo_piyo 45:a32e8091901b 329 case 1:
echo_piyo 45:a32e8091901b 330 //.duty(<cal>*<powerControle>+(<boost>*0.5))
echo_piyo 53:701dc7886fff 331 rs422.put(id[counter], v2.duty(limitf(((mecanum.v2()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)), v4.duty(limitf(((mecanum.v4()*mecanum_power)-(sbdbt.right_y*0.8)),1.0,-1.0)));
echo_piyo 45:a32e8091901b 332 counter ++;
echo_piyo 45:a32e8091901b 333 break;
echo_piyo 45:a32e8091901b 334 case 2:
echo_piyo 55:2dd2f161ebaf 335 //rs422.put(id[counter], sbdbt.right_y, 0.0);
echo_piyo 65:5e4c2e5494ae 336 rs422.put(id[counter], limitf((-1*enc_cylinder.duty_enableWidth(-10.0,10.0)),1.0,-1.0), 0.0);
echo_piyo 45:a32e8091901b 337 counter ++;
echo_piyo 45:a32e8091901b 338 break;
echo_piyo 45:a32e8091901b 339 case 3:
echo_piyo 53:701dc7886fff 340 rs422.put(id[counter], limitf((-1*shoulder_right_cal()*sholder_power),1.0,-1.0), limitf((shoulder_left_cal()*sholder_power),1.0,-1.0));
echo_piyo 45:a32e8091901b 341 counter ++;
echo_piyo 45:a32e8091901b 342 break;
echo_piyo 45:a32e8091901b 343 case 4:
echo_piyo 53:701dc7886fff 344 rs422.put(id[counter], limitf(((float)sword.getState()*sword_power),1.0,-1.0),0.0);
echo_piyo 45:a32e8091901b 345 counter = 0;
echo_piyo 45:a32e8091901b 346 break;
echo_piyo 45:a32e8091901b 347 default:
echo_piyo 45:a32e8091901b 348 break;
echo_piyo 48:96b5f5ebdfb0 349 }
echo_piyo 45:a32e8091901b 350 }else{
echo_piyo 48:96b5f5ebdfb0 351 switch (counter) {
echo_piyo 48:96b5f5ebdfb0 352 case 0:
echo_piyo 48:96b5f5ebdfb0 353 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 354 counter++;
echo_piyo 48:96b5f5ebdfb0 355 break;
echo_piyo 48:96b5f5ebdfb0 356 case 1:
echo_piyo 48:96b5f5ebdfb0 357 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 358 counter ++;
echo_piyo 48:96b5f5ebdfb0 359 break;
echo_piyo 48:96b5f5ebdfb0 360 case 2:
echo_piyo 48:96b5f5ebdfb0 361 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 362 counter ++;
echo_piyo 48:96b5f5ebdfb0 363 break;
echo_piyo 48:96b5f5ebdfb0 364 case 3:
echo_piyo 48:96b5f5ebdfb0 365 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 366 counter ++;
echo_piyo 48:96b5f5ebdfb0 367 break;
echo_piyo 48:96b5f5ebdfb0 368 case 4:
echo_piyo 48:96b5f5ebdfb0 369 rs422.put(id[counter],0.0,0.0);
echo_piyo 48:96b5f5ebdfb0 370 counter = 0;
echo_piyo 48:96b5f5ebdfb0 371 break;
echo_piyo 48:96b5f5ebdfb0 372 default:
echo_piyo 48:96b5f5ebdfb0 373 break;
echo_piyo 45:a32e8091901b 374 }
echo_piyo 45:a32e8091901b 375 }
echo_piyo 14:aac2f18f6779 376 }