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@72:7d6177047823, 2017-09-27 (annotated)

Committer:
echo_piyo
Date:
Wed Sep 27 08:30:19 2017 +0000
Revision:
72:7d6177047823
Parent:
71:7d684ff43ddd
Child:
74:98bad67fde39
(?)??????????????; ????????????????

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