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@37:fa738e34c4d3, 2017-09-06 (annotated)

Committer:
echo_piyo
Date:
Wed Sep 06 02:57:38 2017 +0000
Revision:
37:fa738e34c4d3
Parent:
36:dca1081c19b3
Child:
38:b071512af5ca
(?)??Pin?21,22??11,12???????Pin?21?????

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 0:bf96e953cdb8 16
echo_piyo 0:bf96e953cdb8 17 #define pc_baud 460800
echo_piyo 0:bf96e953cdb8 18 #define sbdbt_tx p13
echo_piyo 0:bf96e953cdb8 19 #define sbdbt_rx p14
echo_piyo 0:bf96e953cdb8 20 #define sbdbt_baud 115200
echo_piyo 0:bf96e953cdb8 21 #define rs422_tx p28
echo_piyo 0:bf96e953cdb8 22 #define rs422_rx p27
echo_piyo 0:bf96e953cdb8 23 #define rs422_baud 115200
echo_piyo 3:9ef929639c4a 24 #define output_period 0.015
echo_piyo 28:72b9af7e1700 25 #define nucleo_num 6
echo_piyo 0:bf96e953cdb8 26 #define pi 3.141592
echo_piyo 0:bf96e953cdb8 27 #define n1_id 3
echo_piyo 0:bf96e953cdb8 28 #define n2_id 4
echo_piyo 10:04f2a82cfd89 29 #define n3_id 5
echo_piyo 14:aac2f18f6779 30 #define n4_id 6
echo_piyo 14:aac2f18f6779 31 #define n5_id 7
echo_piyo 28:72b9af7e1700 32 #define n6_id 8
echo_piyo 5:6efda58ff71b 33 #define yaw_Kp 0.01
echo_piyo 5:6efda58ff71b 34 #define yaw_Ki 0.01
echo_piyo 5:6efda58ff71b 35 #define yaw_Kd 0.01
echo_piyo 10:04f2a82cfd89 36 #define acceleration 25
echo_piyo 10:04f2a82cfd89 37 #define pin_cylinder_on p17
echo_piyo 10:04f2a82cfd89 38 #define pin_cylinder_off p18
echo_piyo 31:285c9898da03 39 #define pin_interrupt_cylinder_min p23
echo_piyo 12:1fec80ae8a2c 40 #define encoder_A p25
echo_piyo 12:1fec80ae8a2c 41 #define encoder_B p26
echo_piyo 33:64fd1bd83bac 42 #define enc_Kp 0.005
echo_piyo 33:64fd1bd83bac 43 #define enc_Ki 0.0001
echo_piyo 33:64fd1bd83bac 44 #define enc_Kd 0.0003
echo_piyo 34:02d605c68bf3 45 #define mecanum_power 1.0
echo_piyo 33:64fd1bd83bac 46 #define sword_power 0.8
echo_piyo 33:64fd1bd83bac 47 #define sholder_power 0.8
echo_piyo 37:fa738e34c4d3 48 #define pin_interrupt_sholderright_max p11 //p21
echo_piyo 37:fa738e34c4d3 49 #define pin_interrupt_sholderright_min p12 //p22
echo_piyo 33:64fd1bd83bac 50 #define pin_interrupt_sholderleft_max p7
echo_piyo 33:64fd1bd83bac 51 #define pin_interrupt_sholderleft_min p8
echo_piyo 37:fa738e34c4d3 52 #define pin_servo p21
echo_piyo 35:5e1ad00f26fb 53 #define servo_reload_time 1.0
echo_piyo 35:5e1ad00f26fb 54 //#define pin_servo_reload p29 //
echo_piyo 35:5e1ad00f26fb 55 //#define pin_interrupt_reload p30 //
echo_piyo 0:bf96e953cdb8 56
echo_piyo 13:dfae731e239f 57 DigitalOut led1(LED1);
echo_piyo 15:0fdf483769bf 58 //DigitalOut led2(LED2);
echo_piyo 15:0fdf483769bf 59 DigitalOut led3(LED3);
echo_piyo 15:0fdf483769bf 60 //DigitalOut led4(LED4);
echo_piyo 31:285c9898da03 61 DigitalIn interrupt_cylinder_min(pin_interrupt_cylinder_min);
echo_piyo 0:bf96e953cdb8 62 Serial pc(USBTX,USBRX);
echo_piyo 0:bf96e953cdb8 63 RS422 rs422(rs422_tx, rs422_rx);
echo_piyo 0:bf96e953cdb8 64 Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
echo_piyo 0:bf96e953cdb8 65 Ticker output_timer;
echo_piyo 0:bf96e953cdb8 66 Mecanum mecanum;
echo_piyo 0:bf96e953cdb8 67 Bno055 bno055;
echo_piyo 2:d5b8f8e62923 68 Position_pid yaw_pid;
echo_piyo 4:a6cc2f03e69b 69 Accel v1;
echo_piyo 4:a6cc2f03e69b 70 Accel v2;
echo_piyo 4:a6cc2f03e69b 71 Accel v3;
echo_piyo 4:a6cc2f03e69b 72 Accel v4;
echo_piyo 15:0fdf483769bf 73 Cylinder cylinder(pin_cylinder_on,pin_cylinder_off);
echo_piyo 26:3280d0300b04 74 Cyclic sword;
echo_piyo 31:285c9898da03 75 Cyclic cyclic_cylinder_position;
echo_piyo 31:285c9898da03 76 DigitalIn interrupt_sholderright_min(pin_interrupt_sholderright_min);
echo_piyo 31:285c9898da03 77 DigitalIn interrupt_sholderright_max(pin_interrupt_sholderright_max);
echo_piyo 31:285c9898da03 78 DigitalIn interrupt_sholderleft_min(pin_interrupt_sholderleft_min);
echo_piyo 31:285c9898da03 79 DigitalIn interrupt_sholderleft_max(pin_interrupt_sholderleft_max);
echo_piyo 35:5e1ad00f26fb 80 Encoder enc_cylinder(encoder_A,encoder_B);
echo_piyo 31:285c9898da03 81
echo_piyo 35:5e1ad00f26fb 82 //追加
echo_piyo 35:5e1ad00f26fb 83 PwmOut servo(pin_servo);
echo_piyo 35:5e1ad00f26fb 84 Timeout timer_servo;
echo_piyo 33:64fd1bd83bac 85 //Servo servo_reload(pin_servo_reload);
echo_piyo 33:64fd1bd83bac 86 //DigitalIn interrupt_reload(pin_interrupt_reload);
echo_piyo 9:6486f4b3ac50 87
echo_piyo 0:bf96e953cdb8 88 void setup();
echo_piyo 0:bf96e953cdb8 89 void output();
echo_piyo 15:0fdf483769bf 90 void motor_cal();
echo_piyo 16:e49df474e4c6 91 void cylinder_cal();
echo_piyo 10:04f2a82cfd89 92 void boost();
echo_piyo 12:1fec80ae8a2c 93 void cylinder_origin();
echo_piyo 26:3280d0300b04 94 void sword_cal();
echo_piyo 35:5e1ad00f26fb 95 void cylinder_reload();
echo_piyo 35:5e1ad00f26fb 96 void servo_origin();
echo_piyo 31:285c9898da03 97 float shoulder_right_cal();
echo_piyo 31:285c9898da03 98 float shoulder_left_cal();
echo_piyo 30:57061e222f10 99 int reload_magazine_flag;
echo_piyo 2:d5b8f8e62923 100 float yaw, target_yaw;
echo_piyo 0:bf96e953cdb8 101
echo_piyo 31:285c9898da03 102 //riseEventそのうちClassにしたい
echo_piyo 31:285c9898da03 103 short state;
echo_piyo 31:285c9898da03 104 int riseEvent(int input)
echo_piyo 31:285c9898da03 105 {
echo_piyo 31:285c9898da03 106 state = ((state<<1)|input)&3;
echo_piyo 31:285c9898da03 107 if(state == 1) {
echo_piyo 31:285c9898da03 108 return 1;
echo_piyo 31:285c9898da03 109 } else {
echo_piyo 31:285c9898da03 110 return 0;
echo_piyo 31:285c9898da03 111 }
echo_piyo 31:285c9898da03 112 }
echo_piyo 31:285c9898da03 113
echo_piyo 0:bf96e953cdb8 114 int main()
echo_piyo 0:bf96e953cdb8 115 {
echo_piyo 0:bf96e953cdb8 116 setup();
echo_piyo 2:d5b8f8e62923 117 while(1) {
echo_piyo 31:285c9898da03 118 //pc.printf("Pulses is: %lo\tdeg :%f\r\n",enc_cylinder.pulse(),enc_cylinder.deg());
echo_piyo 31:285c9898da03 119 //pc.printf("rise state : %d\r\n",riseEvent(sbdbt.right));
echo_piyo 0:bf96e953cdb8 120 }
echo_piyo 0:bf96e953cdb8 121 }
echo_piyo 0:bf96e953cdb8 122
echo_piyo 0:bf96e953cdb8 123 void setup()
echo_piyo 0:bf96e953cdb8 124 {
echo_piyo 35:5e1ad00f26fb 125 wait(0.5);
echo_piyo 19:76a387e4bcf6 126 bno055.begin();
echo_piyo 35:5e1ad00f26fb 127 wait(0.5);
echo_piyo 19:76a387e4bcf6 128 bno055.firstRead();
echo_piyo 0:bf96e953cdb8 129 pc.baud(pc_baud);
echo_piyo 0:bf96e953cdb8 130 sbdbt.begin(sbdbt_baud);
echo_piyo 0:bf96e953cdb8 131 rs422.begin(rs422_baud);
echo_piyo 19:76a387e4bcf6 132 cylinder_origin();
echo_piyo 0:bf96e953cdb8 133 output_timer.attach(&output, output_period);
echo_piyo 2:d5b8f8e62923 134 yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
echo_piyo 10:04f2a82cfd89 135 mecanum.setupdeg(bno055.getYawRad()+180.0);
echo_piyo 4:a6cc2f03e69b 136 v1.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 137 v2.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 138 v3.setup(acceleration,output_period);
echo_piyo 4:a6cc2f03e69b 139 v4.setup(acceleration,output_period);
echo_piyo 31:285c9898da03 140 enc_cylinder.setup(100);
echo_piyo 13:dfae731e239f 141 enc_cylinder.set_parameter(enc_Kp,enc_Ki,enc_Kd);
echo_piyo 35:5e1ad00f26fb 142
echo_piyo 35:5e1ad00f26fb 143 //追加
echo_piyo 35:5e1ad00f26fb 144 servo.period(0.020);
echo_piyo 12:1fec80ae8a2c 145 }
echo_piyo 12:1fec80ae8a2c 146
echo_piyo 36:dca1081c19b3 147 //Sword
echo_piyo 35:5e1ad00f26fb 148 float shoulder_right_cal()
echo_piyo 15:0fdf483769bf 149 {
echo_piyo 35:5e1ad00f26fb 150 if(interrupt_sholderright_max==0&&sbdbt.sankaku) {
echo_piyo 31:285c9898da03 151 return 0.0;
echo_piyo 31:285c9898da03 152 }
echo_piyo 35:5e1ad00f26fb 153 if(interrupt_sholderright_min==0&&sbdbt.batu) {
echo_piyo 33:64fd1bd83bac 154 return 0.0;
echo_piyo 31:285c9898da03 155 }
echo_piyo 31:285c9898da03 156 return (-sbdbt.sankaku*0.8+sbdbt.batu*0.8);
echo_piyo 31:285c9898da03 157 }
echo_piyo 31:285c9898da03 158
echo_piyo 35:5e1ad00f26fb 159 float shoulder_left_cal()
echo_piyo 35:5e1ad00f26fb 160 {
echo_piyo 35:5e1ad00f26fb 161 if(interrupt_sholderleft_max==0&&sbdbt.up==1) {
echo_piyo 31:285c9898da03 162 return 0.0;
echo_piyo 31:285c9898da03 163 }
echo_piyo 35:5e1ad00f26fb 164 if(interrupt_sholderleft_min==0&&sbdbt.down==1) {
echo_piyo 35:5e1ad00f26fb 165 return 0.0;
echo_piyo 31:285c9898da03 166 }
echo_piyo 36:dca1081c19b3 167 return (sbdbt.sankaku*0.8-sbdbt.batu*0.8);
echo_piyo 31:285c9898da03 168 }
echo_piyo 31:285c9898da03 169 void sword_cal()
echo_piyo 31:285c9898da03 170 {
echo_piyo 26:3280d0300b04 171 sword.cyclic(sbdbt.maru);
echo_piyo 17:5c4718c603dc 172 }
echo_piyo 15:0fdf483769bf 173
echo_piyo 36:dca1081c19b3 174 //cylinder
echo_piyo 31:285c9898da03 175 void cylinder_origin()
echo_piyo 31:285c9898da03 176 {
echo_piyo 31:285c9898da03 177 while(interrupt_cylinder_min == 1) {
echo_piyo 14:aac2f18f6779 178 led1 = 1;
echo_piyo 31:285c9898da03 179 rs422.put(5, -0.2, 0.0);
echo_piyo 12:1fec80ae8a2c 180 }
echo_piyo 14:aac2f18f6779 181 led1 = 0;
echo_piyo 31:285c9898da03 182 enc_cylinder.origin();
echo_piyo 0:bf96e953cdb8 183 }
echo_piyo 28:72b9af7e1700 184 //追記(動作未確認)
echo_piyo 31:285c9898da03 185 float cylinder_pwm;
echo_piyo 31:285c9898da03 186 int cylinder_pos_num = 0;
echo_piyo 32:f535ace7c529 187 float cylinder_pos[3] = {0.0,180.0,360.0};
echo_piyo 19:76a387e4bcf6 188 void cylinder_cal()
echo_piyo 31:285c9898da03 189 {
echo_piyo 31:285c9898da03 190 cylinder.cyclic(sbdbt.shikaku); //cylinder ON/OFF
echo_piyo 35:5e1ad00f26fb 191
echo_piyo 35:5e1ad00f26fb 192 if(riseEvent(sbdbt.right)) { //cylinder degset
echo_piyo 31:285c9898da03 193 cylinder_pos_num++;
echo_piyo 35:5e1ad00f26fb 194 if(cylinder_pos_num >= 3) {
echo_piyo 31:285c9898da03 195 cylinder_pos_num = 0;
echo_piyo 31:285c9898da03 196 }
echo_piyo 31:285c9898da03 197 }
echo_piyo 31:285c9898da03 198 enc_cylinder.cal((float)cylinder_pos[cylinder_pos_num],output_period); //コントローラで数値設定
echo_piyo 35:5e1ad00f26fb 199
echo_piyo 33:64fd1bd83bac 200 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 28:72b9af7e1700 201
echo_piyo 31:285c9898da03 202 //リロード機構完成後
echo_piyo 31:285c9898da03 203 /*
echo_piyo 31:285c9898da03 204 if(cylinder.getInState() == 1){
echo_piyo 31:285c9898da03 205 if(interrupt_cylinder_min)servo_reload = 1.0;
echo_piyo 31:285c9898da03 206 }else{
echo_piyo 31:285c9898da03 207 servo_reload = 0.0;
echo_piyo 31:285c9898da03 208 reload_magazine_flag = 1;
echo_piyo 31:285c9898da03 209 }
echo_piyo 31:285c9898da03 210
echo_piyo 31:285c9898da03 211 if(reload_magazine_flag == 1){
echo_piyo 31:285c9898da03 212 rs422.put(n6_id, 0.8, 0.0);
echo_piyo 31:285c9898da03 213 if(interrupt_reload == 1)reload_magazine_flag = 0;
echo_piyo 31:285c9898da03 214 }
echo_piyo 31:285c9898da03 215 */
echo_piyo 19:76a387e4bcf6 216 }
echo_piyo 35:5e1ad00f26fb 217 //追記(動作未確認)
echo_piyo 35:5e1ad00f26fb 218 //SBDBTか何かでcylinder_reload_enableを1にすりゃ動く
echo_piyo 35:5e1ad00f26fb 219 int cylinder_reload_enable = 0;
echo_piyo 35:5e1ad00f26fb 220 void cylinder_reload()
echo_piyo 35:5e1ad00f26fb 221 {
echo_piyo 35:5e1ad00f26fb 222 if(cylinder_reload_enable == 1) {
echo_piyo 35:5e1ad00f26fb 223 cylinder_pos_num = 0;
echo_piyo 35:5e1ad00f26fb 224 if(interrupt_cylinder_min == 0) {
echo_piyo 35:5e1ad00f26fb 225 servo.pulsewidth(0.0022);
echo_piyo 35:5e1ad00f26fb 226 cylinder_reload_enable = 0;
echo_piyo 35:5e1ad00f26fb 227 timer_servo.attach(&servo_origin, servo_reload_time);
echo_piyo 35:5e1ad00f26fb 228 }
echo_piyo 35:5e1ad00f26fb 229 }
echo_piyo 35:5e1ad00f26fb 230 }
echo_piyo 35:5e1ad00f26fb 231 void servo_origin()
echo_piyo 35:5e1ad00f26fb 232 {
echo_piyo 35:5e1ad00f26fb 233 servo.pulsewidth(0.0010);
echo_piyo 35:5e1ad00f26fb 234 timer_servo.detach();
echo_piyo 35:5e1ad00f26fb 235 }
echo_piyo 35:5e1ad00f26fb 236
echo_piyo 36:dca1081c19b3 237
echo_piyo 36:dca1081c19b3 238 //boost
echo_piyo 31:285c9898da03 239 void boost()
echo_piyo 31:285c9898da03 240 {
echo_piyo 31:285c9898da03 241 if(sbdbt.r2) {
echo_piyo 10:04f2a82cfd89 242 mecanum.boost_forward();
echo_piyo 10:04f2a82cfd89 243 }
echo_piyo 31:285c9898da03 244 if(sbdbt.l2) {
echo_piyo 10:04f2a82cfd89 245 mecanum.boost_back();
echo_piyo 10:04f2a82cfd89 246 }
echo_piyo 26:3280d0300b04 247 /*
echo_piyo 10:04f2a82cfd89 248 if(sbdbt.shikaku) {
echo_piyo 10:04f2a82cfd89 249 mecanum.boost_left();
echo_piyo 10:04f2a82cfd89 250 }
echo_piyo 10:04f2a82cfd89 251 if(sbdbt.maru) {
echo_piyo 10:04f2a82cfd89 252 mecanum.boost_right();
echo_piyo 10:04f2a82cfd89 253 }
echo_piyo 26:3280d0300b04 254 */
echo_piyo 10:04f2a82cfd89 255 }
echo_piyo 10:04f2a82cfd89 256
echo_piyo 36:dca1081c19b3 257 //mecanum
echo_piyo 15:0fdf483769bf 258 void motor_cal()
echo_piyo 5:6efda58ff71b 259 {
echo_piyo 4:a6cc2f03e69b 260 yaw = bno055.getYawRad();
echo_piyo 4:a6cc2f03e69b 261 target_yaw = yaw;
echo_piyo 4:a6cc2f03e69b 262 yaw_pid.cal(target_yaw, yaw, output_period);
echo_piyo 4:a6cc2f03e69b 263 mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), bno055.getYawRad());
echo_piyo 5:6efda58ff71b 264 // 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 265 }
echo_piyo 35:5e1ad00f26fb 266 void output()
echo_piyo 35:5e1ad00f26fb 267 {
echo_piyo 35:5e1ad00f26fb 268 motor_cal();
echo_piyo 35:5e1ad00f26fb 269 cylinder_cal();
echo_piyo 35:5e1ad00f26fb 270 sword_cal();
echo_piyo 35:5e1ad00f26fb 271 //boost();
echo_piyo 35:5e1ad00f26fb 272
echo_piyo 35:5e1ad00f26fb 273 //インタラプタが反応したとき原点を取る
echo_piyo 35:5e1ad00f26fb 274 /*
echo_piyo 35:5e1ad00f26fb 275 if(interrupt_cylinder_min==0) {
echo_piyo 35:5e1ad00f26fb 276 enc_cylinder.origin();
echo_piyo 35:5e1ad00f26fb 277 }
echo_piyo 35:5e1ad00f26fb 278 */
echo_piyo 35:5e1ad00f26fb 279
echo_piyo 35:5e1ad00f26fb 280 if(sbdbt.left) {
echo_piyo 35:5e1ad00f26fb 281 cylinder_origin();
echo_piyo 35:5e1ad00f26fb 282 }
echo_piyo 35:5e1ad00f26fb 283
echo_piyo 35:5e1ad00f26fb 284 if(sbdbt.down){
echo_piyo 35:5e1ad00f26fb 285 cylinder_reload_enable = 1;
echo_piyo 35:5e1ad00f26fb 286 }
echo_piyo 35:5e1ad00f26fb 287
echo_piyo 35:5e1ad00f26fb 288 static int counter;
echo_piyo 35:5e1ad00f26fb 289 int id[nucleo_num] = {n1_id, n2_id, n3_id, n4_id, n5_id, n6_id};
echo_piyo 35:5e1ad00f26fb 290 switch (counter) {
echo_piyo 35:5e1ad00f26fb 291 case 0:
echo_piyo 35:5e1ad00f26fb 292 rs422.put(id[counter], v1.duty(mecanum.v1()*mecanum_power), v3.duty(mecanum.v3()*mecanum_power));
echo_piyo 35:5e1ad00f26fb 293 counter++;
echo_piyo 35:5e1ad00f26fb 294 break;
echo_piyo 35:5e1ad00f26fb 295 case 1:
echo_piyo 35:5e1ad00f26fb 296 rs422.put(id[counter], v2.duty(mecanum.v2()*mecanum_power), v4.duty(mecanum.v4()*mecanum_power));
echo_piyo 35:5e1ad00f26fb 297 counter ++;
echo_piyo 35:5e1ad00f26fb 298 break;
echo_piyo 35:5e1ad00f26fb 299 case 2:
echo_piyo 35:5e1ad00f26fb 300 rs422.put(id[counter], enc_cylinder.duty()*-1, 0.0);
echo_piyo 35:5e1ad00f26fb 301 counter ++;
echo_piyo 35:5e1ad00f26fb 302 break;
echo_piyo 35:5e1ad00f26fb 303 case 3:
echo_piyo 36:dca1081c19b3 304 rs422.put(id[counter], shoulder_right_cal()*sholder_power,-1*shoulder_left_cal()*sholder_power);
echo_piyo 35:5e1ad00f26fb 305 counter ++;
echo_piyo 35:5e1ad00f26fb 306 break;
echo_piyo 35:5e1ad00f26fb 307 case 4:
echo_piyo 35:5e1ad00f26fb 308 rs422.put(id[counter], ((float)sword.getState()*sword_power),0.0);
echo_piyo 35:5e1ad00f26fb 309 counter = 0;
echo_piyo 35:5e1ad00f26fb 310 break;
echo_piyo 35:5e1ad00f26fb 311 default:
echo_piyo 35:5e1ad00f26fb 312 break;
echo_piyo 35:5e1ad00f26fb 313 };
echo_piyo 14:aac2f18f6779 314 }