ケンタ ミヤザキ / Mbed 2 deprecated nhk_2018_undercarry_test07

Dependencies:   PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter

Fork of nhk_2018_undercarry_test04 by ケンタ ミヤザキ

main.cpp@5:c5a2a5cf600d, 2018-04-01 (annotated)

Committer:
kenken0721
Date:
Sun Apr 01 04:11:13 2018 +0000
Revision:
5:c5a2a5cf600d
Parent:
4:eabb7a738ff8 
?

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 4:eabb7a738ff8 28 DigitalOut dpin(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 5:c5a2a5cf600d 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 2:c5996dd62e9c 39 void linecheck(char *buff ,int data[2]);
kenken0721 2:c5996dd62e9c 40 void LineCheck(int dmode);
kenken0721 4:eabb7a738ff8 41 void syokikanomai(void);
kenken0721 4:eabb7a738ff8 42 void syokikanomai2(void);
kenken0721 4:eabb7a738ff8 43 void pre_touteki(void);
kenken0721 4:eabb7a738ff8 44 void touteki(float armpower, int pos, bool thro_mode);
kenken0721 0:f6828b914b1c 45
kenken0721 0:f6828b914b1c 46 PwmOut pwm_pins[] = {
kenken0721 0:f6828b914b1c 47 PwmOut( PC_8 ),
kenken0721 0:f6828b914b1c 48 PwmOut( PC_6 ),
kenken0721 0:f6828b914b1c 49 PwmOut( PB_14 ),
kenken0721 0:f6828b914b1c 50 PwmOut( PB_13 )
kenken0721 0:f6828b914b1c 51 };
kenken0721 0:f6828b914b1c 52
kenken0721 0:f6828b914b1c 53 DigitalOut dir_pins_1[] = {
kenken0721 0:f6828b914b1c 54 DigitalOut( PC_5 ),
kenken0721 0:f6828b914b1c 55 DigitalOut( PA_7 ),
kenken0721 2:c5996dd62e9c 56 DigitalOut( PB_6 ),
kenken0721 2:c5996dd62e9c 57 DigitalOut( PB_1)
kenken0721 0:f6828b914b1c 58 };
kenken0721 0:f6828b914b1c 59
kenken0721 0:f6828b914b1c 60 DigitalOut dir_pins_2[] = {
kenken0721 0:f6828b914b1c 61 DigitalOut( PB_12 ),
kenken0721 0:f6828b914b1c 62 DigitalOut( PA_6 ),
kenken0721 2:c5996dd62e9c 63 DigitalOut( PC_7 ),
kenken0721 2:c5996dd62e9c 64 DigitalOut( PB_15)
kenken0721 0:f6828b914b1c 65 };
kenken0721 0:f6828b914b1c 66
kenken0721 2:c5996dd62e9c 67 int crosscount = 0;
kenken0721 0:f6828b914b1c 68 bool mode = true;//trueでラジコン、falseでオート
kenken0721 0:f6828b914b1c 69 bool stopflag = true;//trueで機体停止
kenken0721 0:f6828b914b1c 70 double power = POWER;
kenken0721 2:c5996dd62e9c 71 double rpower = POWER;
kenken0721 2:c5996dd62e9c 72 double lpower = POWER;
kenken0721 0:f6828b914b1c 73 int kbtread = 0;
kenken0721 0:f6828b914b1c 74 int count = 0;
kenken0721 0:f6828b914b1c 75 int counter = 0;
kenken0721 2:c5996dd62e9c 76 int linedata_1[2] = {0};
kenken0721 2:c5996dd62e9c 77 int linedata_2[2] = {0};
kenken0721 2:c5996dd62e9c 78 int linedata_3[2] = {0};
kenken0721 0:f6828b914b1c 79 int prelinedata = 0;
kenken0721 0:f6828b914b1c 80 double output = 0.0;
kenken0721 1:f4dbd6b9cc27 81 char buff1[2];
kenken0721 1:f4dbd6b9cc27 82 char buff2[2];
kenken0721 1:f4dbd6b9cc27 83 char buff3[2];
kenken0721 1:f4dbd6b9cc27 84 float x_dist = 0.0;
kenken0721 1:f4dbd6b9cc27 85 float y_dist = 0.0;
kenken0721 2:c5996dd62e9c 86 float tar_x_dist = 0.0;
kenken0721 2:c5996dd62e9c 87 float tar_y_dist = 0.0;
kenken0721 2:c5996dd62e9c 88 bool ontheline = false;
kenken0721 2:c5996dd62e9c 89 int directmode = STRAIGHT;
kenken0721 3:aaa2fde4fafd 90 int state = START;
kenken0721 3:aaa2fde4fafd 91 int times = 0;
kenken0721 3:aaa2fde4fafd 92 bool throwing = false;
kenken0721 4:eabb7a738ff8 93 int receive = NOT_RECEIVE;
kenken0721 4:eabb7a738ff8 94 int redcount = 0;
kenken0721 4:eabb7a738ff8 95 int bluecount = 0;
kenken0721 4:eabb7a738ff8 96 int yellowcount = 0;
kenken0721 4:eabb7a738ff8 97 bool syokika_flag=false;
kenken0721 4:eabb7a738ff8 98 bool set_flag = false;
kenken0721 4:eabb7a738ff8 99 int encoder_count = 1000;
kenken0721 4:eabb7a738ff8 100
kenken0721 3:aaa2fde4fafd 101 int main() {
kenken0721 3:aaa2fde4fafd 102 timer.reset();
kenken0721 3:aaa2fde4fafd 103 timer.stop();
kenken0721 4:eabb7a738ff8 104 servo1.init(0);//アーム
kenken0721 4:eabb7a738ff8 105 servo2.init(1);//回転軸
kenken0721 4:eabb7a738ff8 106 dpin=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 //motor_relay = 0.0;
kenken0721 4:eabb7a738ff8 111 micro_switch.mode(PullUp);
kenken0721 4:eabb7a738ff8 112
kenken0721 4:eabb7a738ff8 113 motor_pwm=0;
kenken0721 4:eabb7a738ff8 114 motor_dir1=1; //dir1,dir2はこの組み合わせじゃないと死ぬ
kenken0721 4:eabb7a738ff8 115 motor_dir2=1;
kenken0721 4:eabb7a738ff8 116
kenken0721 1:f4dbd6b9cc27 117 S_pid.init();
kenken0721 3:aaa2fde4fafd 118 R_pid.init();
kenken0721 3:aaa2fde4fafd 119 L_pid.init();
kenken0721 1:f4dbd6b9cc27 120 pwm_pins[0].period(0.00005);
kenken0721 1:f4dbd6b9cc27 121 pwm_pins[1].period(0.00005);
kenken0721 1:f4dbd6b9cc27 122 pwm_pins[2].period(0.00005);
kenken0721 1:f4dbd6b9cc27 123 pwm_pins[3].period(0.00005);
kenken0721 4:eabb7a738ff8 124 motor_pwm.period(0.00005);
kenken0721 3:aaa2fde4fafd 125 pc.baud(115200);
kenken0721 2:c5996dd62e9c 126 kbtpc.baud(9600);
kenken0721 2:c5996dd62e9c 127 master.frequency(100000);
kenken0721 4:eabb7a738ff8 128 lamp = 1.0;
kenken0721 4:eabb7a738ff8 129 //-------初期化----------------------------------------
kenken0721 4:eabb7a738ff8 130 syokikanomai();
kenken0721 4:eabb7a738ff8 131 buzzer = 0;
kenken0721 4:eabb7a738ff8 132 //-----------メインループ----------------------------------------め
kenken0721 1:f4dbd6b9cc27 133 while (true) {
kenken0721 4:eabb7a738ff8 134 master.read(addr1,buff1,2);
kenken0721 4:eabb7a738ff8 135 linecheck(buff1,linedata_1);
kenken0721 4:eabb7a738ff8 136 master.read(addr2,buff2,2);
kenken0721 4:eabb7a738ff8 137 linecheck(buff2,linedata_2);
kenken0721 4:eabb7a738ff8 138 master.read(addr3,buff3,2);
kenken0721 4:eabb7a738ff8 139 linecheck(buff3,linedata_3);
kenken0721 2:c5996dd62e9c 140 x_dist = Enc_Carry_X.read_rotate();
kenken0721 2:c5996dd62e9c 141 y_dist = Enc_Carry_Y.read_rotate();
kenken0721 3:aaa2fde4fafd 142 kbtpc.putc(state);
kenken0721 1:f4dbd6b9cc27 143 if(kbtpc.readable()){
kenken0721 1:f4dbd6b9cc27 144 kbtread = kbtpc.getc();
kenken0721 1:f4dbd6b9cc27 145 }
kenken0721 1:f4dbd6b9cc27 146 if(kbtread == 255){
kenken0721 1:f4dbd6b9cc27 147 mode = true;
kenken0721 1:f4dbd6b9cc27 148 }else if(kbtread == 150){
kenken0721 1:f4dbd6b9cc27 149 mode = false;
kenken0721 1:f4dbd6b9cc27 150 }else if(kbtread == 100){
kenken0721 1:f4dbd6b9cc27 151 stopflag = true;
kenken0721 1:f4dbd6b9cc27 152 }else if(kbtread == 200){
kenken0721 1:f4dbd6b9cc27 153 stopflag = false;
kenken0721 2:c5996dd62e9c 154 }else if(kbtread == STRAIGHT){
kenken0721 2:c5996dd62e9c 155 directmode = STRAIGHT;
kenken0721 2:c5996dd62e9c 156 }else if(kbtread == RIGHT){
kenken0721 2:c5996dd62e9c 157 directmode = RIGHT;
kenken0721 2:c5996dd62e9c 158 }else if(kbtread == LEFT){
kenken0721 2:c5996dd62e9c 159 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 160 }else if(kbtread == MANUAL){
kenken0721 3:aaa2fde4fafd 161 state = MANUAL;
kenken0721 3:aaa2fde4fafd 162 }else if(kbtread == START){
kenken0721 3:aaa2fde4fafd 163 state = START;
kenken0721 4:eabb7a738ff8 164 }else if(kbtread == START_TEST){
kenken0721 4:eabb7a738ff8 165 state = START_TEST;
kenken0721 1:f4dbd6b9cc27 166 }
kenken0721 1:f4dbd6b9cc27 167 //--------------手動--------------------------------------
kenken0721 1:f4dbd6b9cc27 168 if(mode == true){
kenken0721 1:f4dbd6b9cc27 169 led2 = 1.0;
kenken0721 2:c5996dd62e9c 170 if(kbtread == STRAIGHT){
kenken0721 1:f4dbd6b9cc27 171 Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 172 }else if(kbtread == BACK){
kenken0721 1:f4dbd6b9cc27 173 Back(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 174 }else if(kbtread == RIGHT){
kenken0721 1:f4dbd6b9cc27 175 Right(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 176 }else if(kbtread == LEFT){
kenken0721 1:f4dbd6b9cc27 177 Left(power, power, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 178 }else{
kenken0721 1:f4dbd6b9cc27 179 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 180 }
kenken0721 1:f4dbd6b9cc27 181 //--------------自動---------------------------------
kenken0721 1:f4dbd6b9cc27 182 }else if(mode == false){
kenken0721 1:f4dbd6b9cc27 183 led2 = 0.0;
kenken0721 1:f4dbd6b9cc27 184 if(stopflag == true){
kenken0721 1:f4dbd6b9cc27 185 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 186 }else{
kenken0721 2:c5996dd62e9c 187 LineCheck(directmode);
kenken0721 3:aaa2fde4fafd 188 switch(state){
kenken0721 4:eabb7a738ff8 189 //------------------任意に進行方向を決めれる----------------------------------------
kenken0721 3:aaa2fde4fafd 190 case MANUAL:
kenken0721 3:aaa2fde4fafd 191 if(crosscount == 1){
kenken0721 3:aaa2fde4fafd 192 directmode = STOP;
kenken0721 3:aaa2fde4fafd 193 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 3:aaa2fde4fafd 194 crosscount = 0;
kenken0721 3:aaa2fde4fafd 195 }
kenken0721 3:aaa2fde4fafd 196 break;
kenken0721 4:eabb7a738ff8 197 //------------------テスト用--------------------------------------------------------
kenken0721 4:eabb7a738ff8 198 case START_TEST:
kenken0721 4:eabb7a738ff8 199 if(crosscount == 0){
kenken0721 4:eabb7a738ff8 200 directmode = RIGHT;
kenken0721 4:eabb7a738ff8 201 }else if(crosscount == 1){
kenken0721 4:eabb7a738ff8 202 directmode = STOP;
kenken0721 4:eabb7a738ff8 203 state = STAND_BY_TEST;
kenken0721 4:eabb7a738ff8 204 crosscount = 0;
kenken0721 4:eabb7a738ff8 205 prelinedata = 0;
kenken0721 4:eabb7a738ff8 206 }
kenken0721 4:eabb7a738ff8 207 break;
kenken0721 4:eabb7a738ff8 208 //------------------テスト用待機位置-----------------------------------------------
kenken0721 4:eabb7a738ff8 209 case STAND_BY_TEST:
kenken0721 4:eabb7a738ff8 210 directmode = STOP;
kenken0721 4:eabb7a738ff8 211 if(distance == 1.0){
kenken0721 4:eabb7a738ff8 212 led2 = 1.0;
kenken0721 4:eabb7a738ff8 213 if(redIn == 1.0){
kenken0721 4:eabb7a738ff8 214 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 215 redcount++;
kenken0721 4:eabb7a738ff8 216 }else if(blueIn == 1.0){
kenken0721 4:eabb7a738ff8 217 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 218 bluecount++;
kenken0721 4:eabb7a738ff8 219 }else{
kenken0721 4:eabb7a738ff8 220 buzzer = 0.0;
kenken0721 4:eabb7a738ff8 221 }
kenken0721 4:eabb7a738ff8 222 if(redcount >= 50){
kenken0721 4:eabb7a738ff8 223 servo_hand.move(hand_tsukami_degree);
kenken0721 4:eabb7a738ff8 224 //pre_touteki();
kenken0721 4:eabb7a738ff8 225 }else if(bluecount >= 50){
kenken0721 4:eabb7a738ff8 226 servo_hand.move(hand_tsukami_degree);
kenken0721 4:eabb7a738ff8 227 //pre_touteki();
kenken0721 4:eabb7a738ff8 228 }
kenken0721 4:eabb7a738ff8 229 }else if(distance == 0.0 && redcount >= 50){
kenken0721 4:eabb7a738ff8 230 servo2.move(koshi_nage_degree);
kenken0721 4:eabb7a738ff8 231 state = THROWING_TEST;
kenken0721 4:eabb7a738ff8 232 directmode = STOP;
kenken0721 4:eabb7a738ff8 233 led2 = 0.0;
kenken0721 4:eabb7a738ff8 234 redcount = 0;
kenken0721 4:eabb7a738ff8 235 crosscount = 0;
kenken0721 4:eabb7a738ff8 236 }else if(distance == 0.0 && bluecount >= 50){
kenken0721 4:eabb7a738ff8 237 servo2.move(koshi_nage_degree);
kenken0721 4:eabb7a738ff8 238 state = THROWING_TEST;
kenken0721 4:eabb7a738ff8 239 directmode = STOP;
kenken0721 4:eabb7a738ff8 240 bluecount = 0;
kenken0721 4:eabb7a738ff8 241 led2 = 0.0;
kenken0721 4:eabb7a738ff8 242 crosscount = 0;
kenken0721 4:eabb7a738ff8 243 }
kenken0721 4:eabb7a738ff8 244 break;
kenken0721 4:eabb7a738ff8 245 //------------------------------------------------------------------------------
kenken0721 4:eabb7a738ff8 246 case THROWING_TEST:
kenken0721 4:eabb7a738ff8 247 if(crosscount == 0){
kenken0721 4:eabb7a738ff8 248 directmode = LEFT;
kenken0721 4:eabb7a738ff8 249 }else if(crosscount == 1){
kenken0721 4:eabb7a738ff8 250 directmode = STOP;
kenken0721 4:eabb7a738ff8 251 touteki(0.8, 190, 0);
kenken0721 4:eabb7a738ff8 252 //syokikanomai();
kenken0721 4:eabb7a738ff8 253 buzzer = 0.0;
kenken0721 4:eabb7a738ff8 254 }
kenken0721 4:eabb7a738ff8 255 break;
kenken0721 4:eabb7a738ff8 256 //------------------初期位置から待機位置1------------------------------------------
kenken0721 3:aaa2fde4fafd 257 case START:
kenken0721 3:aaa2fde4fafd 258 if(crosscount == 0){
kenken0721 3:aaa2fde4fafd 259 directmode = STRAIGHT;
kenken0721 3:aaa2fde4fafd 260 }else if(crosscount == 1 && directmode == STRAIGHT){
kenken0721 3:aaa2fde4fafd 261 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 262 prelinedata = 0;
kenken0721 3:aaa2fde4fafd 263 }else if(crosscount == 1 && directmode == LEFT){
kenken0721 3:aaa2fde4fafd 264 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 265 }else if(crosscount == 2){
kenken0721 3:aaa2fde4fafd 266 directmode = STOP;
kenken0721 3:aaa2fde4fafd 267 state = STAND_BY_1;
kenken0721 3:aaa2fde4fafd 268 crosscount = 0;
kenken0721 3:aaa2fde4fafd 269 prelinedata = 0;
kenken0721 3:aaa2fde4fafd 270 }
kenken0721 3:aaa2fde4fafd 271 break;
kenken0721 4:eabb7a738ff8 272 //------------------待機位置1----------------------------------------------------
kenken0721 3:aaa2fde4fafd 273 case STAND_BY_1:
kenken0721 3:aaa2fde4fafd 274 crosscount = 0;
kenken0721 3:aaa2fde4fafd 275 directmode = STOP;
kenken0721 4:eabb7a738ff8 276 if(distance == 1.0){
kenken0721 4:eabb7a738ff8 277 if(redIn == 1.0){
kenken0721 4:eabb7a738ff8 278 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 279 redcount++;
kenken0721 4:eabb7a738ff8 280 }else if(blueIn == 1.0){
kenken0721 4:eabb7a738ff8 281 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 282 bluecount++;
kenken0721 4:eabb7a738ff8 283 }else{
kenken0721 4:eabb7a738ff8 284 buzzer = 0.0;
kenken0721 4:eabb7a738ff8 285 }
kenken0721 4:eabb7a738ff8 286 if(redcount >= 100 ){
kenken0721 4:eabb7a738ff8 287 servo_hand.move(hand_tsukami_degree);
kenken0721 4:eabb7a738ff8 288 servo1.move(hiji_tsukami_degree);
kenken0721 4:eabb7a738ff8 289 }else if(bluecount >= 100){
kenken0721 4:eabb7a738ff8 290 servo_hand.move(hand_tsukami_degree);
kenken0721 4:eabb7a738ff8 291 servo1.move(hiji_tsukami_degree);
kenken0721 4:eabb7a738ff8 292 }
kenken0721 4:eabb7a738ff8 293 }else if(distance == 0.0 && redcount >= 100){
kenken0721 4:eabb7a738ff8 294 servo2.move(koshi_nage_degree);
kenken0721 3:aaa2fde4fafd 295 state = RED_RECEIVE;
kenken0721 4:eabb7a738ff8 296 directmode = STOP;
kenken0721 4:eabb7a738ff8 297 redcount = 0;
kenken0721 4:eabb7a738ff8 298 crosscount = 0;
kenken0721 4:eabb7a738ff8 299 }else if(distance == 0.0 && bluecount >= 100){
kenken0721 4:eabb7a738ff8 300 state = BLUE_RECEIVE_1;
kenken0721 4:eabb7a738ff8 301 directmode = STOP;
kenken0721 4:eabb7a738ff8 302 bluecount = 0;
kenken0721 4:eabb7a738ff8 303 crosscount = 0;
kenken0721 4:eabb7a738ff8 304 }
kenken0721 3:aaa2fde4fafd 305 break;
kenken0721 4:eabb7a738ff8 306 //------------------待機位置1から投てき位置1----------------------------------------
kenken0721 3:aaa2fde4fafd 307 case RED_RECEIVE:
kenken0721 3:aaa2fde4fafd 308 if(crosscount == 0){
kenken0721 3:aaa2fde4fafd 309 directmode = LEFT;
kenken0721 4:eabb7a738ff8 310 }else if(crosscount == 1){
kenken0721 3:aaa2fde4fafd 311 directmode = STOP;
kenken0721 3:aaa2fde4fafd 312 prelinedata = 0;
kenken0721 4:eabb7a738ff8 313 touteki(0.8,190,0);
kenken0721 4:eabb7a738ff8 314 if(set_flag == false){
kenken0721 3:aaa2fde4fafd 315 directmode = RIGHT;
kenken0721 3:aaa2fde4fafd 316 }
kenken0721 3:aaa2fde4fafd 317 }else if(crosscount == 2){
kenken0721 3:aaa2fde4fafd 318 directmode = STOP;
kenken0721 4:eabb7a738ff8 319 syokikanomai2();
kenken0721 4:eabb7a738ff8 320 if(set_flag == true){
kenken0721 4:eabb7a738ff8 321 state = STAND_BY_1;
kenken0721 4:eabb7a738ff8 322 //throwing = false;
kenken0721 4:eabb7a738ff8 323 crosscount = 0;
kenken0721 4:eabb7a738ff8 324 }
kenken0721 3:aaa2fde4fafd 325 }
kenken0721 3:aaa2fde4fafd 326 break;
kenken0721 4:eabb7a738ff8 327 //------------------待機位置1から投てき位置2----------------------------------------
kenken0721 3:aaa2fde4fafd 328 case BLUE_RECEIVE_1:
kenken0721 3:aaa2fde4fafd 329 if(crosscount == 0){
kenken0721 3:aaa2fde4fafd 330 directmode = RIGHT;
kenken0721 3:aaa2fde4fafd 331 }else if(crosscount == 1){
kenken0721 3:aaa2fde4fafd 332 directmode = STRAIGHT;
kenken0721 3:aaa2fde4fafd 333 }else if(crosscount == 2){
kenken0721 3:aaa2fde4fafd 334 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 335 }else if(crosscount == 3){
kenken0721 3:aaa2fde4fafd 336 directmode = STOP;
kenken0721 3:aaa2fde4fafd 337 prelinedata = 0;
kenken0721 4:eabb7a738ff8 338 state = BLUE_RECEIVE_2;
kenken0721 3:aaa2fde4fafd 339 crosscount = 0;
kenken0721 3:aaa2fde4fafd 340 }
kenken0721 3:aaa2fde4fafd 341 break;
kenken0721 4:eabb7a738ff8 342 //------------------待機位置2----------------------------------------------------
kenken0721 3:aaa2fde4fafd 343 case STAND_BY_2:
kenken0721 3:aaa2fde4fafd 344 crosscount = 0;
kenken0721 4:eabb7a738ff8 345 directmode = STOP;
kenken0721 4:eabb7a738ff8 346 if(distance == 1.0){
kenken0721 4:eabb7a738ff8 347 if(blueIn == 1.0){
kenken0721 4:eabb7a738ff8 348 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 349 bluecount++;
kenken0721 4:eabb7a738ff8 350 }else if(micro_switch == 1.0){
kenken0721 4:eabb7a738ff8 351 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 352 yellowcount++;
kenken0721 4:eabb7a738ff8 353 }else{
kenken0721 4:eabb7a738ff8 354 buzzer = 0.0;
kenken0721 4:eabb7a738ff8 355 }
kenken0721 5:c5a2a5cf600d 356 if(bluecount >= 100){
kenken0721 5:c5a2a5cf600d 357 servo_hand.move(hand_tsukami_degree);
kenken0721 5:c5a2a5cf600d 358 servo1.move(hiji_tsukami_degree);
kenken0721 4:eabb7a738ff8 359 }else if(yellowcount >= 500){
kenken0721 5:c5a2a5cf600d 360 air = 0.0;
kenken0721 4:eabb7a738ff8 361 }
kenken0721 5:c5a2a5cf600d 362 }else if(distance == 0.0 && bluecount >= 100){
kenken0721 5:c5a2a5cf600d 363 servo2.move(koshi_nage_degree);
kenken0721 4:eabb7a738ff8 364 state = BLUE_RECEIVE_2;
kenken0721 4:eabb7a738ff8 365 directmode = STOP;
kenken0721 4:eabb7a738ff8 366 bluecount = 0;
kenken0721 4:eabb7a738ff8 367 crosscount = 0;
kenken0721 4:eabb7a738ff8 368 }
kenken0721 3:aaa2fde4fafd 369 break;
kenken0721 4:eabb7a738ff8 370 //------------------投てき位置2---------------------------------------------------
kenken0721 3:aaa2fde4fafd 371 case BLUE_RECEIVE_2:
kenken0721 3:aaa2fde4fafd 372 if(crosscount == 0){
kenken0721 3:aaa2fde4fafd 373 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 374 }else if(crosscount == 1){
kenken0721 5:c5a2a5cf600d 375 directmode = STOP;
kenken0721 5:c5a2a5cf600d 376 prelinedata = 0;
kenken0721 5:c5a2a5cf600d 377 touteki(1.0,190,1);
kenken0721 5:c5a2a5cf600d 378 if(set_flag == false){
kenken0721 5:c5a2a5cf600d 379 directmode = RIGHT;
kenken0721 3:aaa2fde4fafd 380 }
kenken0721 3:aaa2fde4fafd 381 }else if(crosscount == 2){
kenken0721 3:aaa2fde4fafd 382 directmode = STOP;
kenken0721 5:c5a2a5cf600d 383 syokikanomai2();
kenken0721 5:c5a2a5cf600d 384 if(set_flag == true){
kenken0721 5:c5a2a5cf600d 385 state = STAND_BY_2;
kenken0721 5:c5a2a5cf600d 386 crosscount = 0;
kenken0721 5:c5a2a5cf600d 387 }
kenken0721 5:c5a2a5cf600d 388 }
kenken0721 3:aaa2fde4fafd 389 break;
kenken0721 4:eabb7a738ff8 390 //------------------投てき位置3---------------------------------------------------
kenken0721 3:aaa2fde4fafd 391 case YELLOW_RECEIVE:
kenken0721 3:aaa2fde4fafd 392 if(crosscount != 4){
kenken0721 3:aaa2fde4fafd 393 directmode = LEFT;
kenken0721 3:aaa2fde4fafd 394 }else{
kenken0721 3:aaa2fde4fafd 395 directmode = STOP;
kenken0721 3:aaa2fde4fafd 396 }
kenken0721 3:aaa2fde4fafd 397 break;
kenken0721 3:aaa2fde4fafd 398 default:
kenken0721 3:aaa2fde4fafd 399 break;
kenken0721 3:aaa2fde4fafd 400 }
kenken0721 4:eabb7a738ff8 401 //--------------進行方向---------------------------------------------------------
kenken0721 3:aaa2fde4fafd 402 switch(directmode){
kenken0721 2:c5996dd62e9c 403 case STOP:
kenken0721 2:c5996dd62e9c 404 Stop(pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 405 break;
kenken0721 2:c5996dd62e9c 406 case STRAIGHT:
kenken0721 2:c5996dd62e9c 407 output = S_pid.compute((double)linedata_1[0]);
kenken0721 2:c5996dd62e9c 408 if(output >= 0){
kenken0721 2:c5996dd62e9c 409 rpower = power;
kenken0721 2:c5996dd62e9c 410 lpower = power + output;
kenken0721 2:c5996dd62e9c 411 Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 412 }else {
kenken0721 2:c5996dd62e9c 413 rpower = power + (-1 * output);
kenken0721 2:c5996dd62e9c 414 lpower = power;
kenken0721 2:c5996dd62e9c 415 Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 416 }
kenken0721 2:c5996dd62e9c 417 break;
kenken0721 2:c5996dd62e9c 418 case RIGHT:
kenken0721 2:c5996dd62e9c 419 output = R_pid.compute((double)linedata_2[0]);
kenken0721 1:f4dbd6b9cc27 420 if(output >= 0){
kenken0721 2:c5996dd62e9c 421 rpower = power;
kenken0721 2:c5996dd62e9c 422 lpower = power + output;
kenken0721 2:c5996dd62e9c 423 Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 1:f4dbd6b9cc27 424 }else {
kenken0721 2:c5996dd62e9c 425 rpower = power + (-1 * output);
kenken0721 2:c5996dd62e9c 426 lpower = power;
kenken0721 2:c5996dd62e9c 427 Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 428 }
kenken0721 2:c5996dd62e9c 429 break;
kenken0721 2:c5996dd62e9c 430 case LEFT:
kenken0721 2:c5996dd62e9c 431 output = L_pid.compute((double)linedata_3[0]);
kenken0721 2:c5996dd62e9c 432 if(output >= 0){
kenken0721 2:c5996dd62e9c 433 rpower = power;
kenken0721 2:c5996dd62e9c 434 lpower = power + output;
kenken0721 3:aaa2fde4fafd 435 Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 436 }else {
kenken0721 2:c5996dd62e9c 437 rpower = power + (-1 * output);
kenken0721 2:c5996dd62e9c 438 lpower = power;
kenken0721 2:c5996dd62e9c 439 Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2);
kenken0721 2:c5996dd62e9c 440 }
kenken0721 2:c5996dd62e9c 441 break;
kenken0721 2:c5996dd62e9c 442 default:
kenken0721 2:c5996dd62e9c 443 break;
kenken0721 1:f4dbd6b9cc27 444 }
kenken0721 1:f4dbd6b9cc27 445 }
kenken0721 1:f4dbd6b9cc27 446 }
kenken0721 3:aaa2fde4fafd 447 //pc.printf("kbtread = %d \r" ,kbtread);
kenken0721 2:c5996dd62e9c 448 //pc.printf("buff1[0] = %d \r" ,buff1[0]);
kenken0721 2:c5996dd62e9c 449 //pc.printf("buff2[0] = %d \r" ,buff2[0]);
kenken0721 2:c5996dd62e9c 450 //pc.printf("line1 = %d \r" ,linedata_1[0]);
kenken0721 3:aaa2fde4fafd 451 //pc.printf("line2 = %d \r" ,linedata_3[1]);
kenken0721 4:eabb7a738ff8 452 pc.printf("xdist = %f \r",x_dist);
kenken0721 3:aaa2fde4fafd 453 //pc.printf("tar_xdist = %f \r",tar_x_dist);
kenken0721 3:aaa2fde4fafd 454 //pc.printf("ontheline = %d \r",ontheline);
kenken0721 3:aaa2fde4fafd 455
kenken0721 4:eabb7a738ff8 456 pc.printf("ydist = %f \r",y_dist);
kenken0721 2:c5996dd62e9c 457 //pc.printf("tary = %f \r",tar_y_dist);
kenken0721 2:c5996dd62e9c 458 //pc.printf("stopflag = %d \r",stopflag);
kenken0721 2:c5996dd62e9c 459 //pc.printf("cross = %d \r",crosscount);
kenken0721 1:f4dbd6b9cc27 460 //pc.printf("buff[1] = %d" ,buff1[1]);
kenken0721 2:c5996dd62e9c 461 //pc.printf("line = %d" ,line);
kenken0721 2:c5996dd62e9c 462 //pc.printf("rpower = %lf" ,rpower);
kenken0721 3:aaa2fde4fafd 463 //pc.printf("lpower = %lf" ,lpower);
kenken0721 4:eabb7a738ff8 464 //pc.printf("dicmode = %d \r",directmode);
kenken0721 1:f4dbd6b9cc27 465 pc.printf("output = %lf \n" ,output);
kenken0721 1:f4dbd6b9cc27 466 }
kenken0721 1:f4dbd6b9cc27 467 }
kenken0721 1:f4dbd6b9cc27 468
kenken0721 4:eabb7a738ff8 469 //---------------初期化---------------------------
kenken0721 4:eabb7a738ff8 470
kenken0721 4:eabb7a738ff8 471
kenken0721 4:eabb7a738ff8 472 void syokikanomai(void){
kenken0721 4:eabb7a738ff8 473 //timer.start();
kenken0721 4:eabb7a738ff8 474 //int times = timer.read();
kenken0721 4:eabb7a738ff8 475 syokika_flag = false;
kenken0721 4:eabb7a738ff8 476 while (set_flag==false) {
kenken0721 4:eabb7a738ff8 477 encoder_count= Enc_Arm.read_rotate();
kenken0721 4:eabb7a738ff8 478 servo_hand.move(hand_uke_degree);
kenken0721 4:eabb7a738ff8 479 servo1.move(hiji_nage_degree);//退避
kenken0721 4:eabb7a738ff8 480 servo2.move(koshi_uke_degree);
kenken0721 4:eabb7a738ff8 481 //motor_relay = 0;
kenken0721 4:eabb7a738ff8 482 motor_dir1=0;
kenken0721 4:eabb7a738ff8 483 motor_dir2=1;
kenken0721 4:eabb7a738ff8 484 motor_pwm=0.15;
kenken0721 4:eabb7a738ff8 485
kenken0721 4:eabb7a738ff8 486 if(Enc_Arm.read_z() == 1) {
kenken0721 4:eabb7a738ff8 487 syokika_flag=true;
kenken0721 4:eabb7a738ff8 488 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 489 }
kenken0721 4:eabb7a738ff8 490
kenken0721 4:eabb7a738ff8 491 if (syokika_flag==true) {
kenken0721 4:eabb7a738ff8 492 if(encoder_count == 170) {
kenken0721 4:eabb7a738ff8 493 motor_pwm=0;
kenken0721 4:eabb7a738ff8 494 motor_dir1=1;
kenken0721 4:eabb7a738ff8 495 motor_dir2=1;
kenken0721 4:eabb7a738ff8 496 set_flag=true; //投てき後，falseにする.
kenken0721 4:eabb7a738ff8 497 }
kenken0721 4:eabb7a738ff8 498 }
kenken0721 4:eabb7a738ff8 499 }
kenken0721 4:eabb7a738ff8 500 //if(times >= 1.0){
kenken0721 4:eabb7a738ff8 501 //wait(0.5);
kenken0721 4:eabb7a738ff8 502 //servo_hand.move(hand_uke_degree);
kenken0721 4:eabb7a738ff8 503 servo1.move(hiji_uke_degree);
kenken0721 4:eabb7a738ff8 504 servo1.move(hiji_uke_degree);
kenken0721 4:eabb7a738ff8 505 //servo2.move(koshi_uke_degree);
kenken0721 4:eabb7a738ff8 506 //}
kenken0721 4:eabb7a738ff8 507 }
kenken0721 4:eabb7a738ff8 508
kenken0721 4:eabb7a738ff8 509 void syokikanomai2(void){
kenken0721 4:eabb7a738ff8 510 //timer.start();
kenken0721 4:eabb7a738ff8 511 //int times = timer.read();
kenken0721 4:eabb7a738ff8 512 //syokika_flag = false;
kenken0721 4:eabb7a738ff8 513 if(set_flag==false) {
kenken0721 4:eabb7a738ff8 514 encoder_count= Enc_Arm.read_rotate();
kenken0721 4:eabb7a738ff8 515 servo_hand.move(hand_uke_degree);
kenken0721 4:eabb7a738ff8 516 servo1.move(hiji_nage_degree);//退避
kenken0721 4:eabb7a738ff8 517 servo2.move(koshi_uke_degree);
kenken0721 4:eabb7a738ff8 518 //motor_relay = 0;
kenken0721 4:eabb7a738ff8 519 motor_dir1=0;
kenken0721 4:eabb7a738ff8 520 motor_dir2=1;
kenken0721 4:eabb7a738ff8 521 motor_pwm=0.15;
kenken0721 4:eabb7a738ff8 522
kenken0721 4:eabb7a738ff8 523 if(Enc_Arm.read_z() == 1) {
kenken0721 4:eabb7a738ff8 524 syokika_flag = true;
kenken0721 4:eabb7a738ff8 525 buzzer = 1.0;
kenken0721 4:eabb7a738ff8 526 }
kenken0721 4:eabb7a738ff8 527
kenken0721 4:eabb7a738ff8 528 if (syokika_flag==true) {
kenken0721 4:eabb7a738ff8 529 if(encoder_count == 170) {
kenken0721 4:eabb7a738ff8 530 motor_pwm=0;
kenken0721 4:eabb7a738ff8 531 motor_dir1=1;
kenken0721 4:eabb7a738ff8 532 motor_dir2=1;
kenken0721 4:eabb7a738ff8 533 servo1.move(hiji_uke_degree);
kenken0721 4:eabb7a738ff8 534 servo1.move(hiji_uke_degree);
kenken0721 5:c5a2a5cf600d 535 //syokika_flag = false;
kenken0721 4:eabb7a738ff8 536 set_flag = true; //投てき後，falseにする.
kenken0721 4:eabb7a738ff8 537 }
kenken0721 4:eabb7a738ff8 538 }
kenken0721 4:eabb7a738ff8 539 }
kenken0721 4:eabb7a738ff8 540 //if(times >= 1.0){
kenken0721 4:eabb7a738ff8 541 //wait(0.5);
kenken0721 4:eabb7a738ff8 542 //servo_hand.move(hand_uke_degree);
kenken0721 4:eabb7a738ff8 543 //servo2.move(koshi_uke_degree);
kenken0721 4:eabb7a738ff8 544 //}
kenken0721 4:eabb7a738ff8 545 }
kenken0721 4:eabb7a738ff8 546
kenken0721 4:eabb7a738ff8 547 void pre_touteki(void){
kenken0721 4:eabb7a738ff8 548 /*
kenken0721 4:eabb7a738ff8 549 while(Enc_Arm.read_rotate()<pos) {
kenken0721 4:eabb7a738ff8 550 motor_dir1 = 0;
kenken0721 4:eabb7a738ff8 551 motor_dir2 = 1;
kenken0721 4:eabb7a738ff8 552 motor_pwm = 0.15;
kenken0721 4:eabb7a738ff8 553 }
kenken0721 4:eabb7a738ff8 554 motor_dir1=1;
kenken0721 4:eabb7a738ff8 555 motor_dir2=1;
kenken0721 4:eabb7a738ff8 556 motor_pwm=0;
kenken0721 4:eabb7a738ff8 557 */
kenken0721 4:eabb7a738ff8 558 //wait(1.0);
kenken0721 4:eabb7a738ff8 559 servo_hand.move(hand_tsukami_degree);
kenken0721 4:eabb7a738ff8 560
kenken0721 4:eabb7a738ff8 561 //wait(1.0);
kenken0721 4:eabb7a738ff8 562 servo2.move(koshi_nage_degree);
kenken0721 4:eabb7a738ff8 563 //servo1.move(hiji_tsukami_degree);
kenken0721 4:eabb7a738ff8 564 }
kenken0721 4:eabb7a738ff8 565
kenken0721 4:eabb7a738ff8 566 void touteki(float armpower ,int pos, bool thro_mode){
kenken0721 4:eabb7a738ff8 567 if(set_flag == true){
kenken0721 4:eabb7a738ff8 568 timer.start();
kenken0721 4:eabb7a738ff8 569 }
kenken0721 4:eabb7a738ff8 570 int times = timer.read();
kenken0721 4:eabb7a738ff8 571 //wait(1.0);
kenken0721 4:eabb7a738ff8 572 if(times >= 1.0 && times < 2.0){
kenken0721 4:eabb7a738ff8 573 servo_hand.move(hand_uke_degree);
kenken0721 4:eabb7a738ff8 574 }
kenken0721 4:eabb7a738ff8 575 //wait(1.0);
kenken0721 4:eabb7a738ff8 576 //lamp = 0.0;
kenken0721 4:eabb7a738ff8 577
kenken0721 4:eabb7a738ff8 578 if(times >= 2.0 && times < 3.0){
kenken0721 4:eabb7a738ff8 579 servo1.move(hiji_nage_degree);
kenken0721 4:eabb7a738ff8 580 }
kenken0721 4:eabb7a738ff8 581
kenken0721 4:eabb7a738ff8 582 //wait(1.0);
kenken0721 4:eabb7a738ff8 583 if(times >= 3.0 && times < 5.0){
kenken0721 4:eabb7a738ff8 584 lamp = 0.0;
kenken0721 4:eabb7a738ff8 585 while(Enc_Arm.read_rotate() < pos) {
kenken0721 4:eabb7a738ff8 586 motor_dir1 = 0;
kenken0721 4:eabb7a738ff8 587 motor_dir2 = 1;
kenken0721 4:eabb7a738ff8 588 motor_pwm = 0.15;
kenken0721 4:eabb7a738ff8 589 }
kenken0721 4:eabb7a738ff8 590 }
kenken0721 4:eabb7a738ff8 591 motor_dir1=1;
kenken0721 4:eabb7a738ff8 592 motor_dir2=1;
kenken0721 4:eabb7a738ff8 593 motor_pwm=0;
kenken0721 4:eabb7a738ff8 594 //wait(2.0);
kenken0721 4:eabb7a738ff8 595 //----投てき-------------------------
kenken0721 5:c5a2a5cf600d 596 if(thro_mode == 0.0){
kenken0721 5:c5a2a5cf600d 597 if(times >= 5.0 && times < 6.0){
kenken0721 5:c5a2a5cf600d 598 motor_dir1 = 0;
kenken0721 5:c5a2a5cf600d 599 motor_dir2 = 1;
kenken0721 5:c5a2a5cf600d 600 motor_pwm = armpower;
kenken0721 5:c5a2a5cf600d 601 }
kenken0721 5:c5a2a5cf600d 602 }else if(thro_mode == 1.0){
kenken0721 5:c5a2a5cf600d 603 if(times >= 5.0 && times < 5.5){
kenken0721 5:c5a2a5cf600d 604 dpin = 1.0;
kenken0721 5:c5a2a5cf600d 605 }else{
kenken0721 5:c5a2a5cf600d 606 dpin = 0.0;
kenken0721 5:c5a2a5cf600d 607 }
kenken0721 4:eabb7a738ff8 608 }
kenken0721 4:eabb7a738ff8 609 //wait(1.0);
kenken0721 4:eabb7a738ff8 610 if(times >= 6.0 && times <7.0){
kenken0721 4:eabb7a738ff8 611 motor_pwm = 0.0;
kenken0721 4:eabb7a738ff8 612 motor_dir1 = 0;
kenken0721 4:eabb7a738ff8 613 motor_dir2 = 0;
kenken0721 4:eabb7a738ff8 614 buzzer = 0;
kenken0721 4:eabb7a738ff8 615 lamp = 1.0;
kenken0721 4:eabb7a738ff8 616 set_flag = false;
kenken0721 4:eabb7a738ff8 617 timer.reset();
kenken0721 4:eabb7a738ff8 618 timer.stop();
kenken0721 4:eabb7a738ff8 619 }
kenken0721 4:eabb7a738ff8 620 }
kenken0721 4:eabb7a738ff8 621
kenken0721 2:c5996dd62e9c 622 void LineCheck(int dmode){
kenken0721 2:c5996dd62e9c 623 if(dmode == STRAIGHT){
kenken0721 2:c5996dd62e9c 624 if(linedata_1[1] == 1){
kenken0721 2:c5996dd62e9c 625 ontheline = true;
kenken0721 2:c5996dd62e9c 626 tar_y_dist = y_dist;
kenken0721 2:c5996dd62e9c 627 }
kenken0721 2:c5996dd62e9c 628 if(ontheline == true){
kenken0721 3:aaa2fde4fafd 629 if((y_dist - tar_y_dist) >= Y_STOP_DIST){
kenken0721 2:c5996dd62e9c 630 ontheline = false;
kenken0721 2:c5996dd62e9c 631 crosscount++;
kenken0721 2:c5996dd62e9c 632 }
kenken0721 2:c5996dd62e9c 633 }
kenken0721 2:c5996dd62e9c 634 }else if(dmode == RIGHT){
kenken0721 3:aaa2fde4fafd 635 if(linedata_2[1] == 1){
kenken0721 3:aaa2fde4fafd 636 ontheline = true;
kenken0721 3:aaa2fde4fafd 637 tar_x_dist = x_dist;
kenken0721 3:aaa2fde4fafd 638 }
kenken0721 2:c5996dd62e9c 639 if(ontheline == true){
kenken0721 3:aaa2fde4fafd 640 if((x_dist - tar_x_dist) >= X_STOP_DIST){
kenken0721 2:c5996dd62e9c 641 ontheline = false;
kenken0721 2:c5996dd62e9c 642 crosscount++;
kenken0721 2:c5996dd62e9c 643 }
kenken0721 2:c5996dd62e9c 644 }
kenken0721 2:c5996dd62e9c 645 }else if(dmode == LEFT){
kenken0721 3:aaa2fde4fafd 646 if(linedata_3[1] == 1){
kenken0721 3:aaa2fde4fafd 647 ontheline = true;
kenken0721 3:aaa2fde4fafd 648 tar_x_dist = x_dist;
kenken0721 3:aaa2fde4fafd 649 }
kenken0721 2:c5996dd62e9c 650 if(ontheline == true){
kenken0721 3:aaa2fde4fafd 651 if((tar_x_dist - x_dist) >= X_STOP_DIST){
kenken0721 2:c5996dd62e9c 652 ontheline = false;
kenken0721 2:c5996dd62e9c 653 crosscount++;
kenken0721 2:c5996dd62e9c 654 }
kenken0721 2:c5996dd62e9c 655 }
kenken0721 2:c5996dd62e9c 656 }
kenken0721 2:c5996dd62e9c 657 }
kenken0721 0:f6828b914b1c 658
kenken0721 2:c5996dd62e9c 659 void linecheck(char *buff, int data[2]){
kenken0721 0:f6828b914b1c 660 if(buff[0] == 24){
kenken0721 2:c5996dd62e9c 661 data[0] = 0;
kenken0721 2:c5996dd62e9c 662 data[1] = 0;
kenken0721 0:f6828b914b1c 663 }else if(buff[0] == 56){
kenken0721 2:c5996dd62e9c 664 data[0] = 1;
kenken0721 2:c5996dd62e9c 665 data[1] = 0;
kenken0721 0:f6828b914b1c 666 }else if(buff[0] == 48){
kenken0721 2:c5996dd62e9c 667 data[0] = 2;
kenken0721 2:c5996dd62e9c 668 data[1] = 0;
kenken0721 0:f6828b914b1c 669 }else if(buff[0] == 112){
kenken0721 2:c5996dd62e9c 670 data[0] = 3;
kenken0721 2:c5996dd62e9c 671 data[1] = 0;
kenken0721 0:f6828b914b1c 672 }else if(buff[0] == 96){
kenken0721 2:c5996dd62e9c 673 data[0] = 4;
kenken0721 2:c5996dd62e9c 674 data[1] = 0;
kenken0721 0:f6828b914b1c 675 }else if(buff[0] == 224){
kenken0721 2:c5996dd62e9c 676 data[0] = 5;
kenken0721 2:c5996dd62e9c 677 data[1] = 0;
kenken0721 0:f6828b914b1c 678 }else if(buff[0] == 192){
kenken0721 2:c5996dd62e9c 679 data[0] = 6;
kenken0721 2:c5996dd62e9c 680 data[1] = 0;
kenken0721 0:f6828b914b1c 681 }else if(buff[0] == 128){
kenken0721 2:c5996dd62e9c 682 data[0] = 7;
kenken0721 2:c5996dd62e9c 683 data[1] = 0;
kenken0721 0:f6828b914b1c 684 }else if(buff[0] == 28){
kenken0721 2:c5996dd62e9c 685 data[0] = -1;
kenken0721 2:c5996dd62e9c 686 data[1] = 0;
kenken0721 0:f6828b914b1c 687 }else if(buff[0] == 12){
kenken0721 2:c5996dd62e9c 688 data[0] = -2;
kenken0721 2:c5996dd62e9c 689 data[1] = 0;
kenken0721 0:f6828b914b1c 690 }else if(buff[0] == 14){
kenken0721 2:c5996dd62e9c 691 data[0] = -3;
kenken0721 2:c5996dd62e9c 692 data[1] = 0;
kenken0721 0:f6828b914b1c 693 }else if(buff[0] == 6){
kenken0721 2:c5996dd62e9c 694 data[0] = -4;
kenken0721 2:c5996dd62e9c 695 data[1] = 0;
kenken0721 0:f6828b914b1c 696 }else if(buff[0] == 7){
kenken0721 2:c5996dd62e9c 697 data[0] = -5;
kenken0721 2:c5996dd62e9c 698 data[1] = 0;
kenken0721 0:f6828b914b1c 699 }else if(buff[0] == 3){
kenken0721 2:c5996dd62e9c 700 data[0] = -6;
kenken0721 2:c5996dd62e9c 701 data[1] = 0;
kenken0721 0:f6828b914b1c 702 }else if(buff[0] == 1){
kenken0721 2:c5996dd62e9c 703 data[0] = -7;
kenken0721 2:c5996dd62e9c 704 data[1] = 0;
kenken0721 2:c5996dd62e9c 705 }else if(buff[0] == 255){
kenken0721 2:c5996dd62e9c 706 data[0] = prelinedata;
kenken0721 2:c5996dd62e9c 707 data[1] = 1;
kenken0721 2:c5996dd62e9c 708 }else if(buff[0] == 0){
kenken0721 2:c5996dd62e9c 709 data[0] = prelinedata;
kenken0721 2:c5996dd62e9c 710 data[1] = 0;
kenken0721 0:f6828b914b1c 711 }else{
kenken0721 2:c5996dd62e9c 712 data[0] = prelinedata;
kenken0721 2:c5996dd62e9c 713 data[1] = 0;
kenken0721 0:f6828b914b1c 714 }
kenken0721 2:c5996dd62e9c 715 prelinedata = data[0];
kenken0721 3:aaa2fde4fafd 716 }