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@51:70d45b959d6b, 2017-09-13 (annotated)

Committer:
echo_piyo
Date:
Wed Sep 13 02:58:53 2017 +0000
Revision:
51:70d45b959d6b
Parent:
50:e4e1f38d1bd5
Child:
52:f5ae47e683fa
(??)RiseEvent???????varEvent???????????????

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