Dependencies:   PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter

Fork of nhk_2018_undercarry_test08 by ケンタ ミヤザキ

Committer:
kenken0721
Date:
Fri Apr 06 08:22:03 2018 +0000
Revision:
7:f1a924244b76
Parent:
6:ff7fd6556a81

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenken0721 0:f6828b914b1c 1 #include "mbed.h"
kenken0721 0:f6828b914b1c 2 #include "config.h"
kenken0721 0:f6828b914b1c 3 #include "moter.h"
kenken0721 0:f6828b914b1c 4 #include "PID.h"
kenken0721 1:f4dbd6b9cc27 5 #include "encoder.h"
kenken0721 4:eabb7a738ff8 6 #include "encoder2.h"
kenken0721 4:eabb7a738ff8 7 #include "Serialservo.h"
kenken0721 4:eabb7a738ff8 8 #include "XQ_servo.h"
kenken0721 0:f6828b914b1c 9
kenken0721 0:f6828b914b1c 10 Serial pc(USBTX, USBRX);
kenken0721 0:f6828b914b1c 11 Serial kbtpc(PC_12 ,PD_2);//serial5
kenken0721 0:f6828b914b1c 12 I2C master(sda, scl);
kenken0721 4:eabb7a738ff8 13 DigitalOut led2(LED2);//チェック用
kenken0721 0:f6828b914b1c 14 PID S_pid(SP, SI, SD, 0.0);
kenken0721 2:c5996dd62e9c 15 PID R_pid(RP, RI, RD, 0.0);
kenken0721 2:c5996dd62e9c 16 PID L_pid(LP, LI, LD, 0.0);
kenken0721 4:eabb7a738ff8 17
kenken0721 2:c5996dd62e9c 18 Encoder Enc_Carry_Y(PA_15,PA_14,PC_1);
kenken0721 2:c5996dd62e9c 19 Encoder Enc_Carry_X(PA_4,PB_0,PB_7);
kenken0721 4:eabb7a738ff8 20 Encoder2 Enc_Arm(PF_1,PA_0,PA_1);
kenken0721 4:eabb7a738ff8 21
kenken0721 3:aaa2fde4fafd 22 DigitalOut buzzer(PA_10);
kenken0721 3:aaa2fde4fafd 23 DigitalOut lamp(PC_3);
kenken0721 3:aaa2fde4fafd 24 Timer timer;
kenken0721 4:eabb7a738ff8 25 Serialservo servo1(PC_10, PC_11);//アームの肘
kenken0721 4:eabb7a738ff8 26 Serialservo servo2(PC_10, PC_11);//回転軸
kenken0721 4:eabb7a738ff8 27 XQ_servo servo_hand(PB_5);
kenken0721 6:ff7fd6556a81 28 DigitalOut throwing_relay(PC_9);
kenken0721 4:eabb7a738ff8 29 DigitalIn distance(PC_2);
kenken0721 4:eabb7a738ff8 30 DigitalIn redIn(PC_4);
kenken0721 4:eabb7a738ff8 31 DigitalIn blueIn(PB_2);
kenken0721 4:eabb7a738ff8 32 DigitalIn micro_switch(PC_0);
kenken0721 4:eabb7a738ff8 33 DigitalOut air(PB_4);
kenken0721 4:eabb7a738ff8 34
kenken0721 7:f1a924244b76 35 DigitalOut motor_dir1(PA_8);
kenken0721 4:eabb7a738ff8 36 DigitalOut motor_dir2(PA_9);
kenken0721 4:eabb7a738ff8 37 PwmOut motor_pwm(PB_3);
kenken0721 4:eabb7a738ff8 38
kenken0721 0:f6828b914b1c 39 PwmOut pwm_pins[] = {
kenken0721 0:f6828b914b1c 40 PwmOut( PC_8 ),
kenken0721 0:f6828b914b1c 41 PwmOut( PC_6 ),
kenken0721 0:f6828b914b1c 42 PwmOut( PB_14 ),
kenken0721 0:f6828b914b1c 43 PwmOut( PB_13 )
kenken0721 0:f6828b914b1c 44 };
kenken0721 0:f6828b914b1c 45
kenken0721 0:f6828b914b1c 46 DigitalOut dir_pins_1[] = {
kenken0721 0:f6828b914b1c 47 DigitalOut( PC_5 ),
kenken0721 0:f6828b914b1c 48 DigitalOut( PA_7 ),
kenken0721 2:c5996dd62e9c 49 DigitalOut( PB_6 ),
kenken0721 2:c5996dd62e9c 50 DigitalOut( PB_1)
kenken0721 0:f6828b914b1c 51 };
kenken0721 0:f6828b914b1c 52
kenken0721 0:f6828b914b1c 53 DigitalOut dir_pins_2[] = {
kenken0721 0:f6828b914b1c 54 DigitalOut( PB_12 ),
kenken0721 0:f6828b914b1c 55 DigitalOut( PA_6 ),
kenken0721 2:c5996dd62e9c 56 DigitalOut( PC_7 ),
kenken0721 2:c5996dd62e9c 57 DigitalOut( PB_15)
kenken0721 0:f6828b914b1c 58 };
kenken0721 0:f6828b914b1c 59
kenken0721 6:ff7fd6556a81 60
kenken0721 6:ff7fd6556a81 61 void linecheck(char *buff ,int data[2]);
kenken0721 6:ff7fd6556a81 62 void LineCheck(int dmode);
kenken0721 7:f1a924244b76 63 void Linecount(void);
kenken0721 6:ff7fd6556a81 64 void syokikanomai(void);
kenken0721 6:ff7fd6556a81 65 void syokikanomai2(void);
kenken0721 6:ff7fd6556a81 66 void touteki(float armpower, int pos, bool thro_mode);
kenken0721 7:f1a924244b76 67 void yellow_throwing();
kenken0721 6:ff7fd6556a81 68
kenken0721 7:f1a924244b76 69 bool yellow_throw_flag = false;
kenken0721 7:f1a924244b76 70 bool taiki_flag = false;
kenken0721 2:c5996dd62e9c 71 int crosscount = 0;
kenken0721 0:f6828b914b1c 72 bool mode = true;//trueでラジコン、falseでオート
kenken0721 0:f6828b914b1c 73 bool stopflag = true;//trueで機体停止
kenken0721 0:f6828b914b1c 74 double power = POWER;
kenken0721 0:f6828b914b1c 75 int kbtread = 0;
kenken0721 0:f6828b914b1c 76 int count = 0;
kenken0721 0:f6828b914b1c 77 int counter = 0;
kenken0721 2:c5996dd62e9c 78 int linedata_1[2] = {0};
kenken0721 2:c5996dd62e9c 79 int linedata_2[2] = {0};
kenken0721 2:c5996dd62e9c 80 int linedata_3[2] = {0};
kenken0721 0:f6828b914b1c 81 int prelinedata = 0;
kenken0721 0:f6828b914b1c 82 double output = 0.0;
kenken0721 1:f4dbd6b9cc27 83 char buff1[2];
kenken0721 1:f4dbd6b9cc27 84 char buff2[2];
kenken0721 1:f4dbd6b9cc27 85 char buff3[2];
kenken0721 1:f4dbd6b9cc27 86 float x_dist = 0.0;
kenken0721 1:f4dbd6b9cc27 87 float y_dist = 0.0;
kenken0721 2:c5996dd62e9c 88 float tar_x_dist = 0.0;
kenken0721 7:f1a924244b76 89 float tar_x_dist_2 = 0.0;
kenken0721 2:c5996dd62e9c 90 float tar_y_dist = 0.0;
kenken0721 2:c5996dd62e9c 91 bool ontheline = false;
kenken0721 2:c5996dd62e9c 92 int directmode = STRAIGHT;
kenken0721 3:aaa2fde4fafd 93 int state = START;
kenken0721 3:aaa2fde4fafd 94 int times = 0;
kenken0721 4:eabb7a738ff8 95 int redcount = 0;
kenken0721 4:eabb7a738ff8 96 int bluecount = 0;
kenken0721 4:eabb7a738ff8 97 int yellowcount = 0;
kenken0721 4:eabb7a738ff8 98 bool syokika_flag=false;
kenken0721 4:eabb7a738ff8 99 bool set_flag = false;
kenken0721 4:eabb7a738ff8 100 int encoder_count = 1000;
kenken0721 7:f1a924244b76 101 int linecountcheck = false;
kenken0721 4:eabb7a738ff8 102
kenken0721 3:aaa2fde4fafd 103 int main() {
kenken0721 3:aaa2fde4fafd 104 timer.reset();
kenken0721 3:aaa2fde4fafd 105 timer.stop();
kenken0721 6:ff7fd6556a81 106 throwing_relay=0;
kenken0721 4:eabb7a738ff8 107 lamp=1;
kenken0721 4:eabb7a738ff8 108 air=1.0;
kenken0721 4:eabb7a738ff8 109 buzzer = 0.0;
kenken0721 4:eabb7a738ff8 110 micro_switch.mode(PullUp);
kenken0721 4:eabb7a738ff8 111 motor_pwm=0;
kenken0721 6:ff7fd6556a81 112 motor_dir1=1;
kenken0721 4:eabb7a738ff8 113 motor_dir2=1;
kenken0721 6:ff7fd6556a81 114 servo1.init(0);//アーム
kenken0721 6:ff7fd6556a81 115 servo2.init(1);//回転軸
kenken0721 1:f4dbd6b9cc27 116 S_pid.init();
kenken0721 3:aaa2fde4fafd 117 R_pid.init();
kenken0721 3:aaa2fde4fafd 118 L_pid.init();
kenken0721 1:f4dbd6b9cc27 119 pwm_pins[0].period(0.00005);
kenken0721 1:f4dbd6b9cc27 120 pwm_pins[1].period(0.00005);
kenken0721 1:f4dbd6b9cc27 121 pwm_pins[2].period(0.00005);
kenken0721 1:f4dbd6b9cc27 122 pwm_pins[3].period(0.00005);
kenken0721 4:eabb7a738ff8 123 motor_pwm.period(0.00005);
kenken0721 6:ff7fd6556a81 124 //------通信スピード----------------------------------------------
kenken0721 3:aaa2fde4fafd 125 pc.baud(115200);
kenken0721 2:c5996dd62e9c 126 kbtpc.baud(9600);
kenken0721 2:c5996dd62e9c 127 master.frequency(100000);
kenken0721 6:ff7fd6556a81 128 lamp = 1.0;
kenken0721 6:ff7fd6556a81 129 //-------初期化-------------------------------------------------
kenken0721 4:eabb7a738ff8 130 syokikanomai();
kenken0721 4:eabb7a738ff8 131 buzzer = 0;
kenken0721 6:ff7fd6556a81 132 //-----------メインループ----------------------------------------
kenken0721 1:f4dbd6b9cc27 133 while (true) {
kenken0721 7:f1a924244b76 134 if(directmode == STRAIGHT){
kenken0721 7:f1a924244b76 135 master.read(addr1,buff1,2);
kenken0721 7:f1a924244b76 136 linecheck(buff1,linedata_1);
kenken0721 7:f1a924244b76 137 }else if(directmode == RIGHT){
kenken0721 7:f1a924244b76 138 master.read(addr2,buff2,2);
kenken0721 7:f1a924244b76 139 linecheck(buff2,linedata_2);
kenken0721 7:f1a924244b76 140 }else if(directmode == LEFT){
kenken0721 7:f1a924244b76 141 master.read(addr3,buff3,2);
kenken0721 7:f1a924244b76 142 linecheck(buff3,linedata_3);
kenken0721 7:f1a924244b76 143 }
kenken0721 7:f1a924244b76 144
kenken0721 2:c5996dd62e9c 145 x_dist = Enc_Carry_X.read_rotate();
kenken0721 2:c5996dd62e9c 146 y_dist = Enc_Carry_Y.read_rotate();
kenken0721 3:aaa2fde4fafd 147 kbtpc.putc(state);
kenken0721 1:f4dbd6b9cc27 148 if(kbtpc.readable()){
kenken0721 1:f4dbd6b9cc27 149 kbtread = kbtpc.getc();
kenken0721 1:f4dbd6b9cc27 150 }
kenken0721 1:f4dbd6b9cc27 151 if(kbtread == 255){
kenken0721 1:f4dbd6b9cc27 152 mode = true;
kenken0721 1:f4dbd6b9cc27 153 }else if(kbtread == 150){
kenken0721 1:f4dbd6b9cc27 154 mode = false;
kenken0721 1:f4dbd6b9cc27 155 }else if(kbtread == 100){
kenken0721 1:f4dbd6b9cc27 156 stopflag = true;
kenken0721 1:f4dbd6b9cc27 157 }else if(kbtread == 200){
kenken0721 1:f4dbd6b9cc27 158 stopflag = false;
kenken0721 2:c5996dd62e9c 159 }else if(kbtread == STRAIGHT){
kenken0721 2:c5996dd62e9c 160 directmode = STRAIGHT;
kenken0721 2:c5996dd62e9c 161 }else if(kbtread == RIGHT){
kenken0721 2:c5996dd62e9c 162 directmode = RIGHT;
kenken0721 2:c5996dd62e9c 163 }else if(kbtread == LEFT){
kenken0721 2:c5996dd62e9c 164 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 165 }else if(kbtread == START){
kenken0721 3:aaa2fde4fafd 166 state = START;
kenken0721 1:f4dbd6b9cc27 167 }
kenken0721 1:f4dbd6b9cc27 168 //--------------手動--------------------------------------
kenken0721 1:f4dbd6b9cc27 169 if(mode == true){
kenken0721 1:f4dbd6b9cc27 170 led2 = 1.0;
kenken0721 2:c5996dd62e9c 171 if(kbtread == STRAIGHT){
kenken0721 1:f4dbd6b9cc27 172 Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 173 }else if(kbtread == BACK){
kenken0721 1:f4dbd6b9cc27 174 Back(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 175 }else if(kbtread == RIGHT){
kenken0721 1:f4dbd6b9cc27 176 Right(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 177 }else if(kbtread == LEFT){
kenken0721 1:f4dbd6b9cc27 178 Left(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 179 }else{
kenken0721 1:f4dbd6b9cc27 180 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 181 }
kenken0721 1:f4dbd6b9cc27 182 //--------------自動---------------------------------
kenken0721 1:f4dbd6b9cc27 183 }else if(mode == false){
kenken0721 1:f4dbd6b9cc27 184 led2 = 0.0;
kenken0721 1:f4dbd6b9cc27 185 if(stopflag == true){
kenken0721 1:f4dbd6b9cc27 186 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 187 }else{
kenken0721 2:c5996dd62e9c 188 LineCheck(directmode);
kenken0721 3:aaa2fde4fafd 189 switch(state){
kenken0721 4:eabb7a738ff8 190 //------------------初期位置から待機位置1------------------------------------------
kenken0721 3:aaa2fde4fafd 191 case START:
kenken0721 3:aaa2fde4fafd 192 if(crosscount == 0){
kenken0721 3:aaa2fde4fafd 193 directmode = STRAIGHT;
kenken0721 6:ff7fd6556a81 194 }else if(crosscount == 1){
kenken0721 6:ff7fd6556a81 195 if(directmode == STRAIGHT){
kenken0721 6:ff7fd6556a81 196 directmode = LEFT;
kenken0721 6:ff7fd6556a81 197 prelinedata = 0;
kenken0721 6:ff7fd6556a81 198 }else if(directmode == LEFT){
kenken0721 6:ff7fd6556a81 199 directmode = LEFT;
kenken0721 7:f1a924244b76 200 Linecount();
kenken0721 6:ff7fd6556a81 201 }
kenken0721 3:aaa2fde4fafd 202 }else if(crosscount == 2){
kenken0721 3:aaa2fde4fafd 203 directmode = STOP;
kenken0721 3:aaa2fde4fafd 204 state = STAND_BY_1;
kenken0721 3:aaa2fde4fafd 205 crosscount = 0;
kenken0721 3:aaa2fde4fafd 206 prelinedata = 0;
kenken0721 3:aaa2fde4fafd 207 }
kenken0721 3:aaa2fde4fafd 208 break;
kenken0721 4:eabb7a738ff8 209 //------------------待機位置1----------------------------------------------------
kenken0721 3:aaa2fde4fafd 210 case STAND_BY_1:
kenken0721 3:aaa2fde4fafd 211 crosscount = 0;
kenken0721 3:aaa2fde4fafd 212 directmode = STOP;
kenken0721 4:eabb7a738ff8 213 if(distance == 1.0){
kenken0721 4:eabb7a738ff8 214 if(redIn == 1.0){
kenken0721 4:eabb7a738ff8 215 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 216 redcount++;
kenken0721 4:eabb7a738ff8 217 }else if(blueIn == 1.0){
kenken0721 4:eabb7a738ff8 218 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 219 bluecount++;
kenken0721 4:eabb7a738ff8 220 }else{
kenken0721 4:eabb7a738ff8 221 buzzer = 0.0;
kenken0721 4:eabb7a738ff8 222 }
kenken0721 4:eabb7a738ff8 223 if(redcount >= 100 ){
kenken0721 4:eabb7a738ff8 224 servo_hand.move(hand_tsukami_degree);
kenken0721 4:eabb7a738ff8 225 servo1.move(hiji_tsukami_degree);
kenken0721 6:ff7fd6556a81 226 servo1.move(hiji_tsukami_degree);
kenken0721 4:eabb7a738ff8 227 }else if(bluecount >= 100){
kenken0721 4:eabb7a738ff8 228 servo_hand.move(hand_tsukami_degree);
kenken0721 4:eabb7a738ff8 229 servo1.move(hiji_tsukami_degree);
kenken0721 6:ff7fd6556a81 230 servo1.move(hiji_tsukami_degree);
kenken0721 4:eabb7a738ff8 231 }
kenken0721 4:eabb7a738ff8 232 }else if(distance == 0.0 && redcount >= 100){
kenken0721 4:eabb7a738ff8 233 servo2.move(koshi_nage_degree);
kenken0721 6:ff7fd6556a81 234 servo2.move(koshi_nage_degree);
kenken0721 3:aaa2fde4fafd 235 state = RED_RECEIVE;
kenken0721 4:eabb7a738ff8 236 directmode = STOP;
kenken0721 4:eabb7a738ff8 237 redcount = 0;
kenken0721 4:eabb7a738ff8 238 crosscount = 0;
kenken0721 4:eabb7a738ff8 239 }else if(distance == 0.0 && bluecount >= 100){
kenken0721 7:f1a924244b76 240 servo2.move(koshi_nage_degree);
kenken0721 7:f1a924244b76 241 servo2.move(koshi_nage_degree);
kenken0721 4:eabb7a738ff8 242 state = BLUE_RECEIVE_1;
kenken0721 4:eabb7a738ff8 243 directmode = STOP;
kenken0721 4:eabb7a738ff8 244 bluecount = 0;
kenken0721 4:eabb7a738ff8 245 crosscount = 0;
kenken0721 4:eabb7a738ff8 246 }
kenken0721 3:aaa2fde4fafd 247 break;
kenken0721 4:eabb7a738ff8 248 //------------------待機位置1から投てき位置1----------------------------------------
kenken0721 3:aaa2fde4fafd 249 case RED_RECEIVE:
kenken0721 3:aaa2fde4fafd 250 if(crosscount == 0){
kenken0721 3:aaa2fde4fafd 251 directmode = LEFT;
kenken0721 4:eabb7a738ff8 252 }else if(crosscount == 1){
kenken0721 3:aaa2fde4fafd 253 directmode = STOP;
kenken0721 3:aaa2fde4fafd 254 prelinedata = 0;
kenken0721 4:eabb7a738ff8 255 touteki(0.8,190,0);
kenken0721 7:f1a924244b76 256 //syokikanomai2();
kenken0721 4:eabb7a738ff8 257 if(set_flag == false){
kenken0721 3:aaa2fde4fafd 258 directmode = RIGHT;
kenken0721 7:f1a924244b76 259 Linecount();
kenken0721 3:aaa2fde4fafd 260 }
kenken0721 3:aaa2fde4fafd 261 }else if(crosscount == 2){
kenken0721 3:aaa2fde4fafd 262 directmode = STOP;
kenken0721 4:eabb7a738ff8 263 syokikanomai2();
kenken0721 4:eabb7a738ff8 264 if(set_flag == true){
kenken0721 4:eabb7a738ff8 265 state = STAND_BY_1;
kenken0721 7:f1a924244b76 266 //taiki_flag = false;
kenken0721 4:eabb7a738ff8 267 crosscount = 0;
kenken0721 4:eabb7a738ff8 268 }
kenken0721 3:aaa2fde4fafd 269 }
kenken0721 3:aaa2fde4fafd 270 break;
kenken0721 4:eabb7a738ff8 271 //------------------待機位置1から投てき位置2----------------------------------------
kenken0721 3:aaa2fde4fafd 272 case BLUE_RECEIVE_1:
kenken0721 3:aaa2fde4fafd 273 if(crosscount == 0){
kenken0721 3:aaa2fde4fafd 274 directmode = RIGHT;
kenken0721 3:aaa2fde4fafd 275 }else if(crosscount == 1){
kenken0721 3:aaa2fde4fafd 276 directmode = STRAIGHT;
kenken0721 3:aaa2fde4fafd 277 }else if(crosscount == 2){
kenken0721 3:aaa2fde4fafd 278 directmode = LEFT;
kenken0721 7:f1a924244b76 279 Linecount();
kenken0721 3:aaa2fde4fafd 280 }else if(crosscount == 3){
kenken0721 3:aaa2fde4fafd 281 directmode = STOP;
kenken0721 3:aaa2fde4fafd 282 prelinedata = 0;
kenken0721 4:eabb7a738ff8 283 state = BLUE_RECEIVE_2;
kenken0721 3:aaa2fde4fafd 284 crosscount = 0;
kenken0721 3:aaa2fde4fafd 285 }
kenken0721 3:aaa2fde4fafd 286 break;
kenken0721 4:eabb7a738ff8 287 //------------------待機位置2----------------------------------------------------
kenken0721 3:aaa2fde4fafd 288 case STAND_BY_2:
kenken0721 3:aaa2fde4fafd 289 crosscount = 0;
kenken0721 4:eabb7a738ff8 290 directmode = STOP;
kenken0721 4:eabb7a738ff8 291 if(distance == 1.0){
kenken0721 4:eabb7a738ff8 292 if(blueIn == 1.0){
kenken0721 4:eabb7a738ff8 293 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 294 bluecount++;
kenken0721 7:f1a924244b76 295 }else if(micro_switch == 0.0){
kenken0721 4:eabb7a738ff8 296 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 297 yellowcount++;
kenken0721 4:eabb7a738ff8 298 }else{
kenken0721 4:eabb7a738ff8 299 buzzer = 0.0;
kenken0721 4:eabb7a738ff8 300 }
kenken0721 5:c5a2a5cf600d 301 if(bluecount >= 100){
kenken0721 5:c5a2a5cf600d 302 servo_hand.move(hand_tsukami_degree);
kenken0721 5:c5a2a5cf600d 303 servo1.move(hiji_tsukami_degree);
kenken0721 6:ff7fd6556a81 304 servo1.move(hiji_tsukami_degree);
kenken0721 4:eabb7a738ff8 305 }else if(yellowcount >= 500){
kenken0721 5:c5a2a5cf600d 306 air = 0.0;
kenken0721 4:eabb7a738ff8 307 }
kenken0721 5:c5a2a5cf600d 308 }else if(distance == 0.0 && bluecount >= 100){
kenken0721 5:c5a2a5cf600d 309 servo2.move(koshi_nage_degree);
kenken0721 6:ff7fd6556a81 310 servo2.move(koshi_nage_degree);
kenken0721 4:eabb7a738ff8 311 state = BLUE_RECEIVE_2;
kenken0721 4:eabb7a738ff8 312 directmode = STOP;
kenken0721 4:eabb7a738ff8 313 bluecount = 0;
kenken0721 4:eabb7a738ff8 314 crosscount = 0;
kenken0721 7:f1a924244b76 315 }else if(distance == 0.0 && yellowcount >= 500){
kenken0721 7:f1a924244b76 316 state = YELLOW_RECEIVE;
kenken0721 7:f1a924244b76 317 directmode = STOP;
kenken0721 7:f1a924244b76 318 yellowcount = 0;
kenken0721 7:f1a924244b76 319 crosscount = 0;
kenken0721 4:eabb7a738ff8 320 }
kenken0721 3:aaa2fde4fafd 321 break;
kenken0721 4:eabb7a738ff8 322 //------------------投てき位置2---------------------------------------------------
kenken0721 3:aaa2fde4fafd 323 case BLUE_RECEIVE_2:
kenken0721 3:aaa2fde4fafd 324 if(crosscount == 0){
kenken0721 3:aaa2fde4fafd 325 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 326 }else if(crosscount == 1){
kenken0721 5:c5a2a5cf600d 327 directmode = STOP;
kenken0721 5:c5a2a5cf600d 328 prelinedata = 0;
kenken0721 5:c5a2a5cf600d 329 touteki(1.0,190,1);
kenken0721 7:f1a924244b76 330 //syokikanomai2();
kenken0721 5:c5a2a5cf600d 331 if(set_flag == false){
kenken0721 5:c5a2a5cf600d 332 directmode = RIGHT;
kenken0721 7:f1a924244b76 333 Linecount();
kenken0721 3:aaa2fde4fafd 334 }
kenken0721 3:aaa2fde4fafd 335 }else if(crosscount == 2){
kenken0721 3:aaa2fde4fafd 336 directmode = STOP;
kenken0721 5:c5a2a5cf600d 337 syokikanomai2();
kenken0721 5:c5a2a5cf600d 338 if(set_flag == true){
kenken0721 5:c5a2a5cf600d 339 state = STAND_BY_2;
kenken0721 7:f1a924244b76 340 //taiki_flag = false;
kenken0721 5:c5a2a5cf600d 341 crosscount = 0;
kenken0721 5:c5a2a5cf600d 342 }
kenken0721 7:f1a924244b76 343 }
kenken0721 3:aaa2fde4fafd 344 break;
kenken0721 4:eabb7a738ff8 345 //------------------投てき位置3---------------------------------------------------
kenken0721 3:aaa2fde4fafd 346 case YELLOW_RECEIVE:
kenken0721 7:f1a924244b76 347 if(crosscount != 5){
kenken0721 3:aaa2fde4fafd 348 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 349 }else{
kenken0721 3:aaa2fde4fafd 350 directmode = STOP;
kenken0721 7:f1a924244b76 351 if(yellow_throw_flag == false){
kenken0721 7:f1a924244b76 352 yellow_throwing();
kenken0721 7:f1a924244b76 353 }else{
kenken0721 7:f1a924244b76 354 touteki(1.0,190,1.0);
kenken0721 7:f1a924244b76 355 syokikanomai2();
kenken0721 7:f1a924244b76 356 }
kenken0721 3:aaa2fde4fafd 357 }
kenken0721 3:aaa2fde4fafd 358 break;
kenken0721 3:aaa2fde4fafd 359 default:
kenken0721 3:aaa2fde4fafd 360 break;
kenken0721 3:aaa2fde4fafd 361 }
kenken0721 4:eabb7a738ff8 362 //--------------進行方向---------------------------------------------------------
kenken0721 3:aaa2fde4fafd 363 switch(directmode){
kenken0721 2:c5996dd62e9c 364 case STOP:
kenken0721 2:c5996dd62e9c 365 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 366 break;
kenken0721 2:c5996dd62e9c 367 case STRAIGHT:
kenken0721 2:c5996dd62e9c 368 output = S_pid.compute((double)linedata_1[0]);
kenken0721 2:c5996dd62e9c 369 if(output >= 0){
kenken0721 7:f1a924244b76 370 Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 371 }else {
kenken0721 7:f1a924244b76 372 Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 373 }
kenken0721 2:c5996dd62e9c 374 break;
kenken0721 2:c5996dd62e9c 375 case RIGHT:
kenken0721 2:c5996dd62e9c 376 output = R_pid.compute((double)linedata_2[0]);
kenken0721 1:f4dbd6b9cc27 377 if(output >= 0){
kenken0721 7:f1a924244b76 378 Right(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 379 }else {
kenken0721 7:f1a924244b76 380 Right(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 381 }
kenken0721 2:c5996dd62e9c 382 break;
kenken0721 2:c5996dd62e9c 383 case LEFT:
kenken0721 2:c5996dd62e9c 384 output = L_pid.compute((double)linedata_3[0]);
kenken0721 2:c5996dd62e9c 385 if(output >= 0){
kenken0721 7:f1a924244b76 386 Left(power, power + output, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 387 }else {
kenken0721 7:f1a924244b76 388 Left(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 389 }
kenken0721 2:c5996dd62e9c 390 break;
kenken0721 2:c5996dd62e9c 391 default:
kenken0721 2:c5996dd62e9c 392 break;
kenken0721 1:f4dbd6b9cc27 393 }
kenken0721 1:f4dbd6b9cc27 394 }
kenken0721 1:f4dbd6b9cc27 395 }
kenken0721 4:eabb7a738ff8 396 pc.printf("xdist = %f \r",x_dist);
kenken0721 4:eabb7a738ff8 397 pc.printf("ydist = %f \r",y_dist);
kenken0721 1:f4dbd6b9cc27 398 pc.printf("output = %lf \n" ,output);
kenken0721 1:f4dbd6b9cc27 399 }
kenken0721 1:f4dbd6b9cc27 400 }
kenken0721 1:f4dbd6b9cc27 401
kenken0721 7:f1a924244b76 402 //---------------初期化----------------------------------------------------------
kenken0721 7:f1a924244b76 403
kenken0721 7:f1a924244b76 404 void yellow_throwing(){
kenken0721 7:f1a924244b76 405 if(yellow_throw_flag == false){
kenken0721 7:f1a924244b76 406 timer.start();
kenken0721 7:f1a924244b76 407 }
kenken0721 7:f1a924244b76 408 int times = timer.read();
kenken0721 7:f1a924244b76 409 if(times >= 0.5 && times < 1.5){
kenken0721 7:f1a924244b76 410 servo_hand.move(hand_tsukami_degree);
kenken0721 7:f1a924244b76 411 }else if(times >=1.5 && times <2.5){
kenken0721 7:f1a924244b76 412 servo2.move(koshi_nage_degree);
kenken0721 7:f1a924244b76 413 }else if(times >= 2.5){
kenken0721 7:f1a924244b76 414 yellow_throw_flag = true;
kenken0721 7:f1a924244b76 415 timer.stop();
kenken0721 7:f1a924244b76 416 timer.reset();
kenken0721 7:f1a924244b76 417 }
kenken0721 7:f1a924244b76 418 }
kenken0721 7:f1a924244b76 419
kenken0721 4:eabb7a738ff8 420 void syokikanomai(void){
kenken0721 4:eabb7a738ff8 421 syokika_flag = false;
kenken0721 4:eabb7a738ff8 422 while (set_flag==false) {
kenken0721 4:eabb7a738ff8 423 encoder_count= Enc_Arm.read_rotate();
kenken0721 4:eabb7a738ff8 424 servo_hand.move(hand_uke_degree);
kenken0721 4:eabb7a738ff8 425 servo1.move(hiji_nage_degree);//退避
kenken0721 4:eabb7a738ff8 426 servo2.move(koshi_uke_degree);
kenken0721 6:ff7fd6556a81 427 servo1.move(hiji_nage_degree);//退避
kenken0721 6:ff7fd6556a81 428 servo2.move(koshi_uke_degree);
kenken0721 4:eabb7a738ff8 429 motor_dir1=0;
kenken0721 4:eabb7a738ff8 430 motor_dir2=1;
kenken0721 4:eabb7a738ff8 431 motor_pwm=0.15;
kenken0721 7:f1a924244b76 432 if(Enc_Arm.read_z() == 1){
kenken0721 4:eabb7a738ff8 433 syokika_flag=true;
kenken0721 4:eabb7a738ff8 434 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 435 }
kenken0721 4:eabb7a738ff8 436
kenken0721 4:eabb7a738ff8 437 if (syokika_flag==true) {
kenken0721 7:f1a924244b76 438 if(encoder_count == 170){
kenken0721 4:eabb7a738ff8 439 motor_pwm=0;
kenken0721 4:eabb7a738ff8 440 motor_dir1=1;
kenken0721 4:eabb7a738ff8 441 motor_dir2=1;
kenken0721 4:eabb7a738ff8 442 set_flag=true; //投てき後,falseにする.
kenken0721 4:eabb7a738ff8 443 }
kenken0721 4:eabb7a738ff8 444 }
kenken0721 4:eabb7a738ff8 445 }
kenken0721 4:eabb7a738ff8 446 servo1.move(hiji_uke_degree);
kenken0721 4:eabb7a738ff8 447 servo1.move(hiji_uke_degree);
kenken0721 4:eabb7a738ff8 448 }
kenken0721 4:eabb7a738ff8 449
kenken0721 4:eabb7a738ff8 450 void syokikanomai2(void){
kenken0721 4:eabb7a738ff8 451 if(set_flag==false) {
kenken0721 7:f1a924244b76 452 encoder_count= Enc_Arm.read_rotate();
kenken0721 7:f1a924244b76 453 if(yellow_throw_flag == true){
kenken0721 7:f1a924244b76 454 servo_hand.move(hand_tsukami_degree);
kenken0721 7:f1a924244b76 455 }else{
kenken0721 7:f1a924244b76 456 servo_hand.move(hand_uke_degree);
kenken0721 7:f1a924244b76 457 }
kenken0721 4:eabb7a738ff8 458 servo1.move(hiji_nage_degree);//退避
kenken0721 4:eabb7a738ff8 459 servo2.move(koshi_uke_degree);
kenken0721 6:ff7fd6556a81 460 servo1.move(hiji_nage_degree);//退避
kenken0721 6:ff7fd6556a81 461 servo2.move(koshi_uke_degree);
kenken0721 4:eabb7a738ff8 462 motor_dir1=0;
kenken0721 4:eabb7a738ff8 463 motor_dir2=1;
kenken0721 4:eabb7a738ff8 464 motor_pwm=0.15;
kenken0721 4:eabb7a738ff8 465 if(Enc_Arm.read_z() == 1) {
kenken0721 4:eabb7a738ff8 466 syokika_flag = true;
kenken0721 4:eabb7a738ff8 467 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 468 }
kenken0721 4:eabb7a738ff8 469 if (syokika_flag==true) {
kenken0721 7:f1a924244b76 470 if(encoder_count == 170){
kenken0721 4:eabb7a738ff8 471 motor_pwm=0;
kenken0721 4:eabb7a738ff8 472 motor_dir1=1;
kenken0721 4:eabb7a738ff8 473 motor_dir2=1;
kenken0721 7:f1a924244b76 474 servo_hand.move(hand_uke_degree);
kenken0721 4:eabb7a738ff8 475 servo1.move(hiji_uke_degree);
kenken0721 4:eabb7a738ff8 476 servo1.move(hiji_uke_degree);
kenken0721 4:eabb7a738ff8 477 set_flag = true; //投てき後,falseにする.
kenken0721 7:f1a924244b76 478 taiki_flag = true;
kenken0721 7:f1a924244b76 479 yellow_throw_flag = false;
kenken0721 4:eabb7a738ff8 480 }
kenken0721 4:eabb7a738ff8 481 }
kenken0721 4:eabb7a738ff8 482 }
kenken0721 4:eabb7a738ff8 483 }
kenken0721 4:eabb7a738ff8 484
kenken0721 6:ff7fd6556a81 485 void touteki(float armpower ,int pos, bool throwing_mode){
kenken0721 7:f1a924244b76 486 led2 = 1.0;
kenken0721 4:eabb7a738ff8 487 if(set_flag == true){
kenken0721 4:eabb7a738ff8 488 timer.start();
kenken0721 4:eabb7a738ff8 489 }
kenken0721 4:eabb7a738ff8 490 int times = timer.read();
kenken0721 7:f1a924244b76 491 if(times >= 2.0 && times < 3.0){
kenken0721 4:eabb7a738ff8 492 servo_hand.move(hand_uke_degree);
kenken0721 7:f1a924244b76 493 led2 = 0.0;
kenken0721 6:ff7fd6556a81 494 }
kenken0721 7:f1a924244b76 495 if(times >= 3.0 && times < 4.0){
kenken0721 7:f1a924244b76 496 servo1.move(hiji_nage_degree);
kenken0721 4:eabb7a738ff8 497 servo1.move(hiji_nage_degree);
kenken0721 4:eabb7a738ff8 498 }
kenken0721 7:f1a924244b76 499 if(times >= 4.0 && times < 6.0){
kenken0721 4:eabb7a738ff8 500 lamp = 0.0;
kenken0721 4:eabb7a738ff8 501 while(Enc_Arm.read_rotate() < pos) {
kenken0721 4:eabb7a738ff8 502 motor_dir1 = 0;
kenken0721 4:eabb7a738ff8 503 motor_dir2 = 1;
kenken0721 4:eabb7a738ff8 504 motor_pwm = 0.15;
kenken0721 4:eabb7a738ff8 505 }
kenken0721 4:eabb7a738ff8 506 }
kenken0721 4:eabb7a738ff8 507 motor_dir1=1;
kenken0721 4:eabb7a738ff8 508 motor_dir2=1;
kenken0721 4:eabb7a738ff8 509 motor_pwm=0;
kenken0721 7:f1a924244b76 510
kenken0721 4:eabb7a738ff8 511 //----投てき-------------------------
kenken0721 6:ff7fd6556a81 512 if(throwing_mode == 0.0){
kenken0721 7:f1a924244b76 513 if(times >= 6.0 && times < 7.0){
kenken0721 5:c5a2a5cf600d 514 motor_dir1 = 0;
kenken0721 5:c5a2a5cf600d 515 motor_dir2 = 1;
kenken0721 5:c5a2a5cf600d 516 motor_pwm = armpower;
kenken0721 5:c5a2a5cf600d 517 }
kenken0721 6:ff7fd6556a81 518 }else if(throwing_mode == 1.0){
kenken0721 7:f1a924244b76 519 if(times >= 6.0 && times < 6.5){
kenken0721 6:ff7fd6556a81 520 throwing_relay = 1.0;
kenken0721 5:c5a2a5cf600d 521 }else{
kenken0721 6:ff7fd6556a81 522 throwing_relay = 0.0;
kenken0721 5:c5a2a5cf600d 523 }
kenken0721 4:eabb7a738ff8 524 }
kenken0721 7:f1a924244b76 525 if(times >= 7.0 && times <8.0){
kenken0721 4:eabb7a738ff8 526 motor_pwm = 0.0;
kenken0721 4:eabb7a738ff8 527 motor_dir1 = 0;
kenken0721 4:eabb7a738ff8 528 motor_dir2 = 0;
kenken0721 4:eabb7a738ff8 529 buzzer = 0;
kenken0721 4:eabb7a738ff8 530 lamp = 1.0;
kenken0721 4:eabb7a738ff8 531 set_flag = false;
kenken0721 4:eabb7a738ff8 532 timer.reset();
kenken0721 4:eabb7a738ff8 533 timer.stop();
kenken0721 4:eabb7a738ff8 534 }
kenken0721 4:eabb7a738ff8 535 }
kenken0721 4:eabb7a738ff8 536
kenken0721 7:f1a924244b76 537 void Linecount(void){
kenken0721 7:f1a924244b76 538 if(directmode == RIGHT){
kenken0721 7:f1a924244b76 539 if(linecountcheck == false){
kenken0721 7:f1a924244b76 540 tar_x_dist_2 = x_dist;
kenken0721 7:f1a924244b76 541 linecountcheck = true;
kenken0721 7:f1a924244b76 542 }else{
kenken0721 7:f1a924244b76 543 if((x_dist - tar_x_dist_2) > X_STOP_DIST_3){
kenken0721 7:f1a924244b76 544 crosscount++;
kenken0721 7:f1a924244b76 545 linecountcheck = false;
kenken0721 7:f1a924244b76 546 }
kenken0721 7:f1a924244b76 547 }
kenken0721 7:f1a924244b76 548 }else if(directmode == LEFT){
kenken0721 7:f1a924244b76 549 if(linecountcheck == false){
kenken0721 7:f1a924244b76 550 tar_x_dist_2 = x_dist;
kenken0721 7:f1a924244b76 551 linecountcheck = true;
kenken0721 7:f1a924244b76 552 }else{
kenken0721 7:f1a924244b76 553 if((tar_x_dist_2 - x_dist) >= X_STOP_DIST_2){
kenken0721 7:f1a924244b76 554 crosscount++;
kenken0721 7:f1a924244b76 555 linecountcheck = false;
kenken0721 7:f1a924244b76 556 }
kenken0721 7:f1a924244b76 557 }
kenken0721 7:f1a924244b76 558 }
kenken0721 7:f1a924244b76 559 }
kenken0721 7:f1a924244b76 560
kenken0721 2:c5996dd62e9c 561 void LineCheck(int dmode){
kenken0721 2:c5996dd62e9c 562 if(dmode == STRAIGHT){
kenken0721 2:c5996dd62e9c 563 if(linedata_1[1] == 1){
kenken0721 2:c5996dd62e9c 564 ontheline = true;
kenken0721 2:c5996dd62e9c 565 tar_y_dist = y_dist;
kenken0721 2:c5996dd62e9c 566 }
kenken0721 2:c5996dd62e9c 567 if(ontheline == true){
kenken0721 3:aaa2fde4fafd 568 if((y_dist - tar_y_dist) >= Y_STOP_DIST){
kenken0721 2:c5996dd62e9c 569 ontheline = false;
kenken0721 2:c5996dd62e9c 570 crosscount++;
kenken0721 2:c5996dd62e9c 571 }
kenken0721 2:c5996dd62e9c 572 }
kenken0721 2:c5996dd62e9c 573 }else if(dmode == RIGHT){
kenken0721 3:aaa2fde4fafd 574 if(linedata_2[1] == 1){
kenken0721 3:aaa2fde4fafd 575 ontheline = true;
kenken0721 3:aaa2fde4fafd 576 tar_x_dist = x_dist;
kenken0721 3:aaa2fde4fafd 577 }
kenken0721 2:c5996dd62e9c 578 if(ontheline == true){
kenken0721 3:aaa2fde4fafd 579 if((x_dist - tar_x_dist) >= X_STOP_DIST){
kenken0721 2:c5996dd62e9c 580 ontheline = false;
kenken0721 2:c5996dd62e9c 581 crosscount++;
kenken0721 2:c5996dd62e9c 582 }
kenken0721 2:c5996dd62e9c 583 }
kenken0721 2:c5996dd62e9c 584 }else if(dmode == LEFT){
kenken0721 3:aaa2fde4fafd 585 if(linedata_3[1] == 1){
kenken0721 3:aaa2fde4fafd 586 ontheline = true;
kenken0721 3:aaa2fde4fafd 587 tar_x_dist = x_dist;
kenken0721 3:aaa2fde4fafd 588 }
kenken0721 2:c5996dd62e9c 589 if(ontheline == true){
kenken0721 3:aaa2fde4fafd 590 if((tar_x_dist - x_dist) >= X_STOP_DIST){
kenken0721 2:c5996dd62e9c 591 ontheline = false;
kenken0721 2:c5996dd62e9c 592 crosscount++;
kenken0721 2:c5996dd62e9c 593 }
kenken0721 2:c5996dd62e9c 594 }
kenken0721 2:c5996dd62e9c 595 }
kenken0721 2:c5996dd62e9c 596 }
kenken0721 0:f6828b914b1c 597
kenken0721 2:c5996dd62e9c 598 void linecheck(char *buff, int data[2]){
kenken0721 0:f6828b914b1c 599 if(buff[0] == 24){
kenken0721 2:c5996dd62e9c 600 data[0] = 0;
kenken0721 2:c5996dd62e9c 601 data[1] = 0;
kenken0721 0:f6828b914b1c 602 }else if(buff[0] == 56){
kenken0721 2:c5996dd62e9c 603 data[0] = 1;
kenken0721 2:c5996dd62e9c 604 data[1] = 0;
kenken0721 0:f6828b914b1c 605 }else if(buff[0] == 48){
kenken0721 2:c5996dd62e9c 606 data[0] = 2;
kenken0721 2:c5996dd62e9c 607 data[1] = 0;
kenken0721 0:f6828b914b1c 608 }else if(buff[0] == 112){
kenken0721 2:c5996dd62e9c 609 data[0] = 3;
kenken0721 2:c5996dd62e9c 610 data[1] = 0;
kenken0721 0:f6828b914b1c 611 }else if(buff[0] == 96){
kenken0721 2:c5996dd62e9c 612 data[0] = 4;
kenken0721 2:c5996dd62e9c 613 data[1] = 0;
kenken0721 0:f6828b914b1c 614 }else if(buff[0] == 224){
kenken0721 2:c5996dd62e9c 615 data[0] = 5;
kenken0721 2:c5996dd62e9c 616 data[1] = 0;
kenken0721 0:f6828b914b1c 617 }else if(buff[0] == 192){
kenken0721 2:c5996dd62e9c 618 data[0] = 6;
kenken0721 2:c5996dd62e9c 619 data[1] = 0;
kenken0721 0:f6828b914b1c 620 }else if(buff[0] == 128){
kenken0721 2:c5996dd62e9c 621 data[0] = 7;
kenken0721 2:c5996dd62e9c 622 data[1] = 0;
kenken0721 0:f6828b914b1c 623 }else if(buff[0] == 28){
kenken0721 2:c5996dd62e9c 624 data[0] = -1;
kenken0721 2:c5996dd62e9c 625 data[1] = 0;
kenken0721 0:f6828b914b1c 626 }else if(buff[0] == 12){
kenken0721 2:c5996dd62e9c 627 data[0] = -2;
kenken0721 2:c5996dd62e9c 628 data[1] = 0;
kenken0721 0:f6828b914b1c 629 }else if(buff[0] == 14){
kenken0721 2:c5996dd62e9c 630 data[0] = -3;
kenken0721 2:c5996dd62e9c 631 data[1] = 0;
kenken0721 0:f6828b914b1c 632 }else if(buff[0] == 6){
kenken0721 2:c5996dd62e9c 633 data[0] = -4;
kenken0721 2:c5996dd62e9c 634 data[1] = 0;
kenken0721 0:f6828b914b1c 635 }else if(buff[0] == 7){
kenken0721 2:c5996dd62e9c 636 data[0] = -5;
kenken0721 2:c5996dd62e9c 637 data[1] = 0;
kenken0721 0:f6828b914b1c 638 }else if(buff[0] == 3){
kenken0721 2:c5996dd62e9c 639 data[0] = -6;
kenken0721 2:c5996dd62e9c 640 data[1] = 0;
kenken0721 0:f6828b914b1c 641 }else if(buff[0] == 1){
kenken0721 2:c5996dd62e9c 642 data[0] = -7;
kenken0721 2:c5996dd62e9c 643 data[1] = 0;
kenken0721 2:c5996dd62e9c 644 }else if(buff[0] == 255){
kenken0721 2:c5996dd62e9c 645 data[0] = prelinedata;
kenken0721 2:c5996dd62e9c 646 data[1] = 1;
kenken0721 2:c5996dd62e9c 647 }else if(buff[0] == 0){
kenken0721 2:c5996dd62e9c 648 data[0] = prelinedata;
kenken0721 2:c5996dd62e9c 649 data[1] = 0;
kenken0721 0:f6828b914b1c 650 }else{
kenken0721 2:c5996dd62e9c 651 data[0] = prelinedata;
kenken0721 2:c5996dd62e9c 652 data[1] = 0;
kenken0721 0:f6828b914b1c 653 }
kenken0721 2:c5996dd62e9c 654 prelinedata = data[0];
kenken0721 3:aaa2fde4fafd 655 }