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@71:7d684ff43ddd, 2017-09-26 (annotated)

Committer:
echo_piyo
Date:
Tue Sep 26 08:39:55 2017 +0000
Revision:
71:7d684ff43ddd
Parent:
69:b2c8a0c5662c
Child:
72:7d6177047823
(?)cylinder_power_cal()???????????????????????????

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