yusuke takahashi / Mbed 2 deprecated ver3_1_0

driver

Dependencies:   HMC6352 PID mbed

Fork of ver3_1_0 by ryo seki

main.cpp@1:3b61675573b1, 2014-01-24 (annotated)

Committer:
yusuke_robocup
Date:
Fri Jan 24 06:27:03 2014 +0000
Revision:
1:3b61675573b1
Parent:
0:bde8ed56b164 
driver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akudohune 0:bde8ed56b164 1 #include <math.h>
akudohune 0:bde8ed56b164 2 #include <sstream>
akudohune 0:bde8ed56b164 3 #include "mbed.h"
akudohune 0:bde8ed56b164 4 #include "HMC6352.h"
akudohune 0:bde8ed56b164 5 #include "PID.h"
akudohune 0:bde8ed56b164 6 #include "main.h"
akudohune 0:bde8ed56b164 7
akudohune 0:bde8ed56b164 8
yusuke_robocup 1:3b61675573b1 9 void whiteliner()
yusuke_robocup 1:3b61675573b1 10 {
yusuke_robocup 1:3b61675573b1 11 static int tmp[5] = {0};
yusuke_robocup 1:3b61675573b1 12 static int sum = 0;
yusuke_robocup 1:3b61675573b1 13
yusuke_robocup 1:3b61675573b1 14 sum -= tmp[4];
yusuke_robocup 1:3b61675573b1 15 sum += whiteout_flag;
yusuke_robocup 1:3b61675573b1 16 tmp[4] = tmp[3];
yusuke_robocup 1:3b61675573b1 17 tmp[3] = tmp[2];
yusuke_robocup 1:3b61675573b1 18 tmp[2] = tmp[1];
yusuke_robocup 1:3b61675573b1 19 tmp[1] = tmp[0];
yusuke_robocup 1:3b61675573b1 20 tmp[0] = whiteout_flag;
yusuke_robocup 1:3b61675573b1 21
yusuke_robocup 1:3b61675573b1 22 //return sum/5;
yusuke_robocup 1:3b61675573b1 23 }
yusuke_robocup 1:3b61675573b1 24
yusuke_robocup 1:3b61675573b1 25 int hansya_x(int x)
yusuke_robocup 1:3b61675573b1 26 {
yusuke_robocup 1:3b61675573b1 27 static int tmp[5] = {0};
yusuke_robocup 1:3b61675573b1 28 static int sum = 0;
yusuke_robocup 1:3b61675573b1 29
yusuke_robocup 1:3b61675573b1 30 sum -= tmp[4];
yusuke_robocup 1:3b61675573b1 31 sum += x;
yusuke_robocup 1:3b61675573b1 32 tmp[4] = tmp[3];
yusuke_robocup 1:3b61675573b1 33 tmp[3] = tmp[2];
yusuke_robocup 1:3b61675573b1 34 tmp[2] = tmp[1];
yusuke_robocup 1:3b61675573b1 35 tmp[1] = tmp[0];
yusuke_robocup 1:3b61675573b1 36 tmp[0] = x;
yusuke_robocup 1:3b61675573b1 37
yusuke_robocup 1:3b61675573b1 38 return sum/5;
yusuke_robocup 1:3b61675573b1 39 }
yusuke_robocup 1:3b61675573b1 40
yusuke_robocup 1:3b61675573b1 41
yusuke_robocup 1:3b61675573b1 42 int hansya_y(int y)
yusuke_robocup 1:3b61675573b1 43 {
yusuke_robocup 1:3b61675573b1 44 static int tmp[5] = {0};
yusuke_robocup 1:3b61675573b1 45 static int sum = 0;
yusuke_robocup 1:3b61675573b1 46
yusuke_robocup 1:3b61675573b1 47 sum -= tmp[4];
yusuke_robocup 1:3b61675573b1 48 sum += y;
yusuke_robocup 1:3b61675573b1 49 tmp[4] = tmp[3];
yusuke_robocup 1:3b61675573b1 50 tmp[3] = tmp[2];
yusuke_robocup 1:3b61675573b1 51 tmp[2] = tmp[1];
yusuke_robocup 1:3b61675573b1 52 tmp[1] = tmp[0];
yusuke_robocup 1:3b61675573b1 53 tmp[0] = y;
yusuke_robocup 1:3b61675573b1 54
yusuke_robocup 1:3b61675573b1 55 return sum/5;
yusuke_robocup 1:3b61675573b1 56 }
yusuke_robocup 1:3b61675573b1 57
akudohune 0:bde8ed56b164 58
akudohune 0:bde8ed56b164 59 void PidUpdate()
akudohune 0:bde8ed56b164 60 {
akudohune 0:bde8ed56b164 61 static uint8_t Fflag = 0;
akudohune 0:bde8ed56b164 62
akudohune 0:bde8ed56b164 63 pid.setSetPoint(((int)((REFERENCE + standTu) + 360) % 360) / 1.0);
yusuke_robocup 1:3b61675573b1 64 inputPID = (((int)(compass.sample() - ((standard) * 10.0) + 5400.0) % 3600) / 10.0);
akudohune 0:bde8ed56b164 65
akudohune 0:bde8ed56b164 66 //pc.printf("%f\n",timer1.read());
akudohune 0:bde8ed56b164 67 pid.setProcessValue(inputPID);
akudohune 0:bde8ed56b164 68 //timer1.reset();
akudohune 0:bde8ed56b164 69
akudohune 0:bde8ed56b164 70 compassPID = -(pid.compute());
akudohune 0:bde8ed56b164 71
akudohune 0:bde8ed56b164 72 if(!Fflag){
akudohune 0:bde8ed56b164 73 Fflag = 1;
akudohune 0:bde8ed56b164 74 compassPID = 0;
akudohune 0:bde8ed56b164 75 }
akudohune 0:bde8ed56b164 76 //pc.printf("%d\t%d\t%d\n",speed[0],speed[1],speed[2]);
akudohune 0:bde8ed56b164 77 //pc.printf("standard = \t\t%f\n",standard);
akudohune 0:bde8ed56b164 78 //pc.printf("%d\n",diff);
akudohune 0:bde8ed56b164 79 //pc.printf("compass.sample = \t%f\n",compass.sample() / 1.0);
akudohune 0:bde8ed56b164 80 //pc.printf("compassPID = \t\t%d\n",(int)compassPID);
akudohune 0:bde8ed56b164 81 //pc.printf("inputPID = \t\t%f\n\n",inputPID);
akudohune 0:bde8ed56b164 82
akudohune 0:bde8ed56b164 83 //pc.printf("%d\t%d\n",Distance,direction);
akudohune 0:bde8ed56b164 84 //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
akudohune 0:bde8ed56b164 85 }
akudohune 0:bde8ed56b164 86
akudohune 0:bde8ed56b164 87 void IrDistanceUpdate()
akudohune 0:bde8ed56b164 88 {
akudohune 0:bde8ed56b164 89 for(uint8_t i = 0;i < 6;i++)
akudohune 0:bde8ed56b164 90 {
akudohune 0:bde8ed56b164 91 AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
akudohune 0:bde8ed56b164 92 irDistance[i] = adcIn.read_u16() >> 4;
akudohune 0:bde8ed56b164 93 //pc.printf("%d\n",irDistance[0]);
akudohune 0:bde8ed56b164 94 }
akudohune 0:bde8ed56b164 95 }
akudohune 0:bde8ed56b164 96
yusuke_robocup 1:3b61675573b1 97 /*
akudohune 0:bde8ed56b164 98 void move(int vxx, int vyy, int vss)
akudohune 0:bde8ed56b164 99 {
akudohune 0:bde8ed56b164 100 double motVal[MOT_NUM] = {0};
akudohune 0:bde8ed56b164 101
akudohune 0:bde8ed56b164 102 motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
akudohune 0:bde8ed56b164 103 motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2);
akudohune 0:bde8ed56b164 104 motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
akudohune 0:bde8ed56b164 105 motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4);
akudohune 0:bde8ed56b164 106
akudohune 0:bde8ed56b164 107 for(uint8_t i = 0 ; i < MOT_NUM ; i++){
akudohune 0:bde8ed56b164 108 if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
akudohune 0:bde8ed56b164 109 else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
akudohune 0:bde8ed56b164 110 speed[i] = (int)motVal[i];
akudohune 0:bde8ed56b164 111 }
yusuke_robocup 1:3b61675573b1 112 }*/
yusuke_robocup 1:3b61675573b1 113
yusuke_robocup 1:3b61675573b1 114 void move(int vxx, int vyy, int vss)
yusuke_robocup 1:3b61675573b1 115 {
yusuke_robocup 1:3b61675573b1 116 double motVal[MOT_NUM] = {0};
yusuke_robocup 1:3b61675573b1 117
yusuke_robocup 1:3b61675573b1 118 int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360;
yusuke_robocup 1:3b61675573b1 119
yusuke_robocup 1:3b61675573b1 120 double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1;
yusuke_robocup 1:3b61675573b1 121
yusuke_robocup 1:3b61675573b1 122 if(angle == 180){
yusuke_robocup 1:3b61675573b1 123 hosei1 = 1.3;
yusuke_robocup 1:3b61675573b1 124 }
yusuke_robocup 1:3b61675573b1 125
yusuke_robocup 1:3b61675573b1 126 motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1;
yusuke_robocup 1:3b61675573b1 127 motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2)*hosei2;
yusuke_robocup 1:3b61675573b1 128 motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3;
yusuke_robocup 1:3b61675573b1 129 motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4)*hosei4;
yusuke_robocup 1:3b61675573b1 130
yusuke_robocup 1:3b61675573b1 131 for(uint8_t i = 0 ; i < MOT_NUM ; i++){
yusuke_robocup 1:3b61675573b1 132 if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
yusuke_robocup 1:3b61675573b1 133 else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
yusuke_robocup 1:3b61675573b1 134 speed[i] = motVal[i];
yusuke_robocup 1:3b61675573b1 135 }
yusuke_robocup 1:3b61675573b1 136
yusuke_robocup 1:3b61675573b1 137 //pc.printf("speed1 = %d\n",speed[0]);
yusuke_robocup 1:3b61675573b1 138 //pc.printf("speed2 = %d\n",speed[1]);
yusuke_robocup 1:3b61675573b1 139 //pc.printf("speed3 = %d\n",speed[2]);
yusuke_robocup 1:3b61675573b1 140 //pc.printf("speed4 = %d\n\n",speed[3]);
yusuke_robocup 1:3b61675573b1 141
yusuke_robocup 1:3b61675573b1 142 ////pc.printf("%s",StringFIN.c_str());
akudohune 0:bde8ed56b164 143 }
akudohune 0:bde8ed56b164 144
akudohune 0:bde8ed56b164 145 int vector(double Amp,double degree,int xy)
akudohune 0:bde8ed56b164 146 {
akudohune 0:bde8ed56b164 147 double result;
akudohune 0:bde8ed56b164 148
akudohune 0:bde8ed56b164 149 if(xy == X){
akudohune 0:bde8ed56b164 150 result = Amp * cos(degree * PI / 180.0);
akudohune 0:bde8ed56b164 151 }else if(xy == Y){
akudohune 0:bde8ed56b164 152 result = Amp * sin(degree * PI / 180.0) * (2.0 / sqrt(3.0));
akudohune 0:bde8ed56b164 153 }
akudohune 0:bde8ed56b164 154
akudohune 0:bde8ed56b164 155 return (int)result;
akudohune 0:bde8ed56b164 156 }
akudohune 0:bde8ed56b164 157
akudohune 0:bde8ed56b164 158 /*********** Serial interrupt ***********/
akudohune 0:bde8ed56b164 159
akudohune 0:bde8ed56b164 160 void Tx_interrupt()
akudohune 0:bde8ed56b164 161 {
akudohune 0:bde8ed56b164 162 array(speed[0],speed[1],speed[2],speed[3]);
akudohune 0:bde8ed56b164 163 driver.printf("%s",StringFIN.c_str());
akudohune 0:bde8ed56b164 164 //pc.printf("%s",StringFIN.c_str());
akudohune 0:bde8ed56b164 165 }
akudohune 0:bde8ed56b164 166 /*
akudohune 0:bde8ed56b164 167 void Rx_interrupt()
akudohune 0:bde8ed56b164 168 {
akudohune 0:bde8ed56b164 169 if(driver.readable()){
akudohune 0:bde8ed56b164 170 //pc.printf("%d\n",driver.getc());
akudohune 0:bde8ed56b164 171 }
akudohune 0:bde8ed56b164 172 }*/
akudohune 0:bde8ed56b164 173
akudohune 0:bde8ed56b164 174
akudohune 0:bde8ed56b164 175 /*********** Serial interrupt **********/
akudohune 0:bde8ed56b164 176
akudohune 0:bde8ed56b164 177
akudohune 0:bde8ed56b164 178 void init()
akudohune 0:bde8ed56b164 179 {
akudohune 0:bde8ed56b164 180 int scanfSuccess;
akudohune 0:bde8ed56b164 181 int printfSuccess;
akudohune 0:bde8ed56b164 182 int closeSuccess;
akudohune 0:bde8ed56b164 183 int close2Success;
akudohune 0:bde8ed56b164 184 uint8_t MissFlag = 0;
akudohune 0:bde8ed56b164 185 FILE *fp;
akudohune 0:bde8ed56b164 186
akudohune 0:bde8ed56b164 187 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
akudohune 0:bde8ed56b164 188
akudohune 0:bde8ed56b164 189 StartButton.mode(PullUp);
akudohune 0:bde8ed56b164 190 CalibEnterButton.mode(PullUp);
akudohune 0:bde8ed56b164 191 CalibExitButton.mode(PullUp);
akudohune 0:bde8ed56b164 192 EEPROMButton.mode(PullUp);
akudohune 0:bde8ed56b164 193
akudohune 0:bde8ed56b164 194 driver.baud(BAUD_RATE);
akudohune 0:bde8ed56b164 195 device2.baud(BAUD_RATE2);
akudohune 0:bde8ed56b164 196 wait_ms(MOTDRIVER_WAIT);
akudohune 0:bde8ed56b164 197 driver.printf("1F0002F0003F0004F000\r\n");
akudohune 0:bde8ed56b164 198 device2.printf("START");
akudohune 0:bde8ed56b164 199
akudohune 0:bde8ed56b164 200 driver.attach(&Tx_interrupt, Serial::TxIrq);
akudohune 0:bde8ed56b164 201 //driver.attach(&Rx_interrupt, Serial::RxIrq);
akudohune 0:bde8ed56b164 202 device2.attach(&dev_rx,Serial::RxIrq);
akudohune 0:bde8ed56b164 203 device2.attach(&dev_tx,Serial::TxIrq);
yusuke_robocup 1:3b61675573b1 204 underIR.attach(&whiteliner, 0.05);
akudohune 0:bde8ed56b164 205
akudohune 0:bde8ed56b164 206 pid.setInputLimits(MINIMUM,MAXIMUM);
akudohune 0:bde8ed56b164 207 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
akudohune 0:bde8ed56b164 208 pid.setBias(PID_BIAS);
akudohune 0:bde8ed56b164 209 pid.setMode(AUTO_MODE);
akudohune 0:bde8ed56b164 210 pid.setSetPoint(REFERENCE);
akudohune 0:bde8ed56b164 211
akudohune 0:bde8ed56b164 212 irDistanceUpdata.attach(&IrDistanceUpdate, 0.1);
akudohune 0:bde8ed56b164 213
akudohune 0:bde8ed56b164 214 Survtimer.start();
akudohune 0:bde8ed56b164 215
akudohune 0:bde8ed56b164 216 mbedleds = 1;
akudohune 0:bde8ed56b164 217
akudohune 0:bde8ed56b164 218 while(StartButton){
akudohune 0:bde8ed56b164 219 MissFlag = 0;
akudohune 0:bde8ed56b164 220 if(!CalibEnterButton){
akudohune 0:bde8ed56b164 221 mbedleds = 2;
akudohune 0:bde8ed56b164 222 compass.setCalibrationMode(ENTER);
akudohune 0:bde8ed56b164 223 while(CalibExitButton);
akudohune 0:bde8ed56b164 224 compass.setCalibrationMode(EXIT);
akudohune 0:bde8ed56b164 225 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 226 mbedleds = 4;
akudohune 0:bde8ed56b164 227 }
akudohune 0:bde8ed56b164 228
akudohune 0:bde8ed56b164 229 if(!EEPROMButton){
akudohune 0:bde8ed56b164 230 Survtimer.reset();
akudohune 0:bde8ed56b164 231 fp = fopen("/local/out.txt", "r");
akudohune 0:bde8ed56b164 232 if(fp == NULL){
akudohune 0:bde8ed56b164 233 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 234 MissFlag = 1;
akudohune 0:bde8ed56b164 235 }else{
akudohune 0:bde8ed56b164 236 scanfSuccess = fscanf(fp, "%lf",&standard);
akudohune 0:bde8ed56b164 237 if(scanfSuccess == EOF){
akudohune 0:bde8ed56b164 238 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 239 MissFlag = 1;
akudohune 0:bde8ed56b164 240 }else{
akudohune 0:bde8ed56b164 241 closeSuccess = fclose(fp);
akudohune 0:bde8ed56b164 242 if(closeSuccess == EOF){
akudohune 0:bde8ed56b164 243 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 244 MissFlag = 1;
akudohune 0:bde8ed56b164 245 }else{
akudohune 0:bde8ed56b164 246 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 247 }
akudohune 0:bde8ed56b164 248 }
akudohune 0:bde8ed56b164 249 }
akudohune 0:bde8ed56b164 250 if((Survtimer.read() > (BUT_WAIT + 0.1)) || (MissFlag)){
akudohune 0:bde8ed56b164 251 for(uint8_t i = 0;i < 2;i++){
akudohune 0:bde8ed56b164 252 mbedleds = 15;
akudohune 0:bde8ed56b164 253 wait(0.1);
akudohune 0:bde8ed56b164 254 mbedleds = 0;
akudohune 0:bde8ed56b164 255 wait(0.1);
akudohune 0:bde8ed56b164 256 }
akudohune 0:bde8ed56b164 257 mbedleds = 15;
akudohune 0:bde8ed56b164 258 }else{
akudohune 0:bde8ed56b164 259 mbedleds = 8;
akudohune 0:bde8ed56b164 260 }
akudohune 0:bde8ed56b164 261 }
akudohune 0:bde8ed56b164 262
akudohune 0:bde8ed56b164 263 if(!CalibExitButton){
akudohune 0:bde8ed56b164 264 Survtimer.reset();
akudohune 0:bde8ed56b164 265
akudohune 0:bde8ed56b164 266 standard = compass.sample() / 10.0;
akudohune 0:bde8ed56b164 267
akudohune 0:bde8ed56b164 268 fp = fopen("/local/out.txt", "w");
akudohune 0:bde8ed56b164 269 if(fp == NULL){
akudohune 0:bde8ed56b164 270 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 271 MissFlag = 1;
akudohune 0:bde8ed56b164 272 }else{
akudohune 0:bde8ed56b164 273 printfSuccess = fprintf(fp, "%f",standard);
akudohune 0:bde8ed56b164 274 if(printfSuccess == EOF){
akudohune 0:bde8ed56b164 275 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 276 MissFlag = 1;
akudohune 0:bde8ed56b164 277 }else{
akudohune 0:bde8ed56b164 278 close2Success = fclose(fp);
akudohune 0:bde8ed56b164 279 if(close2Success == EOF){
akudohune 0:bde8ed56b164 280 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 281 MissFlag = 1;
akudohune 0:bde8ed56b164 282 }else{
akudohune 0:bde8ed56b164 283 wait(BUT_WAIT);
akudohune 0:bde8ed56b164 284 }
akudohune 0:bde8ed56b164 285 }
akudohune 0:bde8ed56b164 286 }
akudohune 0:bde8ed56b164 287 if((Survtimer.read() > (BUT_WAIT + 0.2)) || (MissFlag)){
akudohune 0:bde8ed56b164 288 for(uint8_t i = 0;i < 4;i++){
akudohune 0:bde8ed56b164 289 mbedleds = 15;
akudohune 0:bde8ed56b164 290 wait(0.1);
akudohune 0:bde8ed56b164 291 mbedleds = 0;
akudohune 0:bde8ed56b164 292 wait(0.1);
akudohune 0:bde8ed56b164 293 }
akudohune 0:bde8ed56b164 294 mbedleds = 15;
akudohune 0:bde8ed56b164 295 }else{
akudohune 0:bde8ed56b164 296 mbedleds = 10;
akudohune 0:bde8ed56b164 297 }
akudohune 0:bde8ed56b164 298 }
akudohune 0:bde8ed56b164 299 }
akudohune 0:bde8ed56b164 300
akudohune 0:bde8ed56b164 301 mbedleds = 0;
akudohune 0:bde8ed56b164 302
akudohune 0:bde8ed56b164 303 Survtimer.stop();
akudohune 0:bde8ed56b164 304
akudohune 0:bde8ed56b164 305 for(uint8_t i = 0;i < 6;i++)
akudohune 0:bde8ed56b164 306 {
akudohune 0:bde8ed56b164 307 stand[i] = irDistance[i];
akudohune 0:bde8ed56b164 308 }
akudohune 0:bde8ed56b164 309 irDistanceUpdata.detach();
akudohune 0:bde8ed56b164 310
akudohune 0:bde8ed56b164 311 pidUpdata.attach(&PidUpdate, PID_CYCLE);
akudohune 0:bde8ed56b164 312 //irDistanceUpdata.attach(&IrDistanceUpdate, 0.1);
akudohune 0:bde8ed56b164 313 //timer1.start();
akudohune 0:bde8ed56b164 314 }
akudohune 0:bde8ed56b164 315
yusuke_robocup 1:3b61675573b1 316
akudohune 0:bde8ed56b164 317 int main()
akudohune 0:bde8ed56b164 318 {
akudohune 0:bde8ed56b164 319 int vx=0,vy=0,vs=0;
yusuke_robocup 1:3b61675573b1 320 int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
yusuke_robocup 1:3b61675573b1 321 int state_off = NONE;
yusuke_robocup 1:3b61675573b1 322 int direction_av = 0;
yusuke_robocup 1:3b61675573b1 323 int direction_past = 0;
yusuke_robocup 1:3b61675573b1 324 int past_state_off = 0;
yusuke_robocup 1:3b61675573b1 325 int accelera_Distance = 0;
akudohune 0:bde8ed56b164 326 uint8_t whiteFlag = 0;
yusuke_robocup 1:3b61675573b1 327 int save_vx = 0,save_vy = 0;
yusuke_robocup 1:3b61675573b1 328
yusuke_robocup 1:3b61675573b1 329 int movein = 0;
akudohune 0:bde8ed56b164 330
akudohune 0:bde8ed56b164 331 init();
akudohune 0:bde8ed56b164 332
yusuke_robocup 1:3b61675573b1 333 while(1) {
yusuke_robocup 1:3b61675573b1 334
yusuke_robocup 1:3b61675573b1 335 vx=0;
yusuke_robocup 1:3b61675573b1 336 vy=0;
akudohune 0:bde8ed56b164 337
yusuke_robocup 1:3b61675573b1 338 //tuning = 0;
yusuke_robocup 1:3b61675573b1 339
yusuke_robocup 1:3b61675573b1 340 x_dista = 0;
yusuke_robocup 1:3b61675573b1 341 y_dista = 0;
yusuke_robocup 1:3b61675573b1 342 x_turn = 0;
yusuke_robocup 1:3b61675573b1 343 y_turn = 0;
yusuke_robocup 1:3b61675573b1 344 //turn_flag = 0;
akudohune 0:bde8ed56b164 345
yusuke_robocup 1:3b61675573b1 346 vs = compassPID;
yusuke_robocup 1:3b61675573b1 347
yusuke_robocup 1:3b61675573b1 348 //direction_av = moving_ave_5point(direction);
yusuke_robocup 1:3b61675573b1 349
yusuke_robocup 1:3b61675573b1 350
yusuke_robocup 1:3b61675573b1 351
yusuke_robocup 1:3b61675573b1 352 for(uint8_t i = 0;i < 6;i++)
akudohune 0:bde8ed56b164 353 {
akudohune 0:bde8ed56b164 354 AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
akudohune 0:bde8ed56b164 355 irDistance[i] = ((adcIn.read_u16() >> 4) - stand[i]);
yusuke_robocup 1:3b61675573b1 356 if(irDistance[i] >= 30)
akudohune 0:bde8ed56b164 357 {
akudohune 0:bde8ed56b164 358 whiteFlag = 1;
yusuke_robocup 1:3b61675573b1 359 movein = 1;
yusuke_robocup 1:3b61675573b1 360 whiteout = 10;
yusuke_robocup 1:3b61675573b1 361 }
yusuke_robocup 1:3b61675573b1 362
yusuke_robocup 1:3b61675573b1 363 //pc.printf("%d\n",irDistance[0]);
yusuke_robocup 1:3b61675573b1 364 }
yusuke_robocup 1:3b61675573b1 365
yusuke_robocup 1:3b61675573b1 366 if(lineStateX == XNORMAL){
yusuke_robocup 1:3b61675573b1 367 if((LEFT_SONIC < 350) && (whiteFlag)){
yusuke_robocup 1:3b61675573b1 368 lineStateX = LEFT_OUT;
akudohune 0:bde8ed56b164 369 }
yusuke_robocup 1:3b61675573b1 370 if((LEFT_SONIC < 350) && (RIGHT_SONIC < 100) && (whiteFlag)){
yusuke_robocup 1:3b61675573b1 371 lineStateX = LEFT_OUT;
yusuke_robocup 1:3b61675573b1 372 }
yusuke_robocup 1:3b61675573b1 373
yusuke_robocup 1:3b61675573b1 374 if((RIGHT_SONIC < 350) && (whiteFlag)){
yusuke_robocup 1:3b61675573b1 375 lineStateX = RIGHT_OUT;
yusuke_robocup 1:3b61675573b1 376 }
yusuke_robocup 1:3b61675573b1 377 if((RIGHT_SONIC < 350) && (LEFT_SONIC < 100) && (whiteFlag)){
yusuke_robocup 1:3b61675573b1 378 lineStateX = RIGHT_OUT;
yusuke_robocup 1:3b61675573b1 379 }
yusuke_robocup 1:3b61675573b1 380
yusuke_robocup 1:3b61675573b1 381 }else if(lineStateX == LEFT_OUT){
yusuke_robocup 1:3b61675573b1 382 /*
yusuke_robocup 1:3b61675573b1 383 if((LEFT_SONIC > 450) && (!whiteFlag)){
yusuke_robocup 1:3b61675573b1 384 lineStateX = XNORMAL;
yusuke_robocup 1:3b61675573b1 385 whiteFlag = 0;
yusuke_robocup 1:3b61675573b1 386 }
yusuke_robocup 1:3b61675573b1 387 */
yusuke_robocup 1:3b61675573b1 388 if((LEFT_SONIC > 450)){
yusuke_robocup 1:3b61675573b1 389 lineStateX = XNORMAL;
yusuke_robocup 1:3b61675573b1 390 whiteFlag = 0;
yusuke_robocup 1:3b61675573b1 391 }
yusuke_robocup 1:3b61675573b1 392 }else if(lineStateX == RIGHT_OUT){
yusuke_robocup 1:3b61675573b1 393 /*
yusuke_robocup 1:3b61675573b1 394 if((RIGHT_SONIC > 450) && (!whiteFlag)){
yusuke_robocup 1:3b61675573b1 395 lineStateX = XNORMAL;
yusuke_robocup 1:3b61675573b1 396 whiteFlag = 0;
yusuke_robocup 1:3b61675573b1 397 }
yusuke_robocup 1:3b61675573b1 398 */
yusuke_robocup 1:3b61675573b1 399 if((RIGHT_SONIC > 450)){
yusuke_robocup 1:3b61675573b1 400 lineStateX = XNORMAL;
yusuke_robocup 1:3b61675573b1 401 whiteFlag = 0;
yusuke_robocup 1:3b61675573b1 402 }
akudohune 0:bde8ed56b164 403 }
akudohune 0:bde8ed56b164 404
yusuke_robocup 1:3b61675573b1 405
yusuke_robocup 1:3b61675573b1 406 if(lineStateY == YNORMAL){
yusuke_robocup 1:3b61675573b1 407 if((FRONT_SONIC < 400) && (whiteFlag)){
yusuke_robocup 1:3b61675573b1 408 lineStateY = FRONT_OUT;
yusuke_robocup 1:3b61675573b1 409 }
yusuke_robocup 1:3b61675573b1 410 if((FRONT_SONIC < 400)&& (BACK_SONIC < 100) && (whiteFlag)){
yusuke_robocup 1:3b61675573b1 411 lineStateY = FRONT_OUT;
yusuke_robocup 1:3b61675573b1 412 }
yusuke_robocup 1:3b61675573b1 413 if((BACK_SONIC < 400) && (whiteFlag)){
yusuke_robocup 1:3b61675573b1 414 lineStateY = BACK_OUT;
yusuke_robocup 1:3b61675573b1 415 }
yusuke_robocup 1:3b61675573b1 416 if((BACK_SONIC < 400) && (FRONT_SONIC < 100) && (whiteFlag)){
yusuke_robocup 1:3b61675573b1 417 lineStateY = BACK_OUT;
akudohune 0:bde8ed56b164 418 }
yusuke_robocup 1:3b61675573b1 419 }else if(lineStateY == FRONT_OUT){
yusuke_robocup 1:3b61675573b1 420 /*
yusuke_robocup 1:3b61675573b1 421 if((FRONT_SONIC > 500) && (!whiteFlag)){
yusuke_robocup 1:3b61675573b1 422 lineStateY = YNORMAL;
yusuke_robocup 1:3b61675573b1 423 whiteFlag = 0;
akudohune 0:bde8ed56b164 424 }
yusuke_robocup 1:3b61675573b1 425 */
yusuke_robocup 1:3b61675573b1 426 if((FRONT_SONIC > 500)){
yusuke_robocup 1:3b61675573b1 427 lineStateY = YNORMAL;
yusuke_robocup 1:3b61675573b1 428 whiteFlag = 0;
akudohune 0:bde8ed56b164 429 }
yusuke_robocup 1:3b61675573b1 430 }else if(lineStateY == BACK_OUT){
yusuke_robocup 1:3b61675573b1 431 /*
yusuke_robocup 1:3b61675573b1 432 if((BACK_SONIC > 500) && (!whiteFlag)){
yusuke_robocup 1:3b61675573b1 433 lineStateY = YNORMAL;
yusuke_robocup 1:3b61675573b1 434 whiteFlag = 0;
akudohune 0:bde8ed56b164 435 }
yusuke_robocup 1:3b61675573b1 436 */
yusuke_robocup 1:3b61675573b1 437 if((BACK_SONIC > 500)){
yusuke_robocup 1:3b61675573b1 438 lineStateY = YNORMAL;
yusuke_robocup 1:3b61675573b1 439 whiteFlag = 0;
akudohune 0:bde8ed56b164 440 }
akudohune 0:bde8ed56b164 441 }
akudohune 0:bde8ed56b164 442
akudohune 0:bde8ed56b164 443
yusuke_robocup 1:3b61675573b1 444 if((state_off == ATTACK)&&(Distance == 10)){
yusuke_robocup 1:3b61675573b1 445 state = 1;
akudohune 0:bde8ed56b164 446 }else{
yusuke_robocup 1:3b61675573b1 447 state = 0;
yusuke_robocup 1:3b61675573b1 448 }
yusuke_robocup 1:3b61675573b1 449
yusuke_robocup 1:3b61675573b1 450 if(((direction == 0)||(direction == 1)||(direction == 15))){
yusuke_robocup 1:3b61675573b1 451 state_off = ATTACK;
yusuke_robocup 1:3b61675573b1 452 }
yusuke_robocup 1:3b61675573b1 453 if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 90)){
yusuke_robocup 1:3b61675573b1 454 if((past_state_off != SNAKE)){
yusuke_robocup 1:3b61675573b1 455 accelera_Distance = Distance;
akudohune 0:bde8ed56b164 456 }
yusuke_robocup 1:3b61675573b1 457 state_off = SNAKE;
akudohune 0:bde8ed56b164 458 }
yusuke_robocup 1:3b61675573b1 459 if(Distance >= 120){
yusuke_robocup 1:3b61675573b1 460 state_off = SEARCH;
yusuke_robocup 1:3b61675573b1 461 }
yusuke_robocup 1:3b61675573b1 462
yusuke_robocup 1:3b61675573b1 463 past_state_off = state_off;
akudohune 0:bde8ed56b164 464
yusuke_robocup 1:3b61675573b1 465 if(IR_found){
yusuke_robocup 1:3b61675573b1 466 if(state_off == SNAKE){
yusuke_robocup 1:3b61675573b1 467 if((Distance == 30)||(Distance == 90)){
yusuke_robocup 1:3b61675573b1 468 x_dista = 12*ball_sankaku[direction][0];
yusuke_robocup 1:3b61675573b1 469 y_dista = 12*ball_sankaku[direction][1];
yusuke_robocup 1:3b61675573b1 470
yusuke_robocup 1:3b61675573b1 471 x_turn = 18*(turn_sankaku[direction][0]);
yusuke_robocup 1:3b61675573b1 472 y_turn = 18*(turn_sankaku[direction][1]);
yusuke_robocup 1:3b61675573b1 473
yusuke_robocup 1:3b61675573b1 474 if((direction == 2)||(direction == 14)){
yusuke_robocup 1:3b61675573b1 475 //x_turn *= 0.7;
yusuke_robocup 1:3b61675573b1 476 y_turn *= 0.7;
akudohune 0:bde8ed56b164 477 }
yusuke_robocup 1:3b61675573b1 478
yusuke_robocup 1:3b61675573b1 479 if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
yusuke_robocup 1:3b61675573b1 480 x_turn = 7*(turn_sankaku[direction][0]);
yusuke_robocup 1:3b61675573b1 481 y_turn = 7*(turn_sankaku[direction][1]);
yusuke_robocup 1:3b61675573b1 482
yusuke_robocup 1:3b61675573b1 483 x_dista = 15*ball_sankaku[direction][0];
yusuke_robocup 1:3b61675573b1 484 y_dista = 10*ball_sankaku[direction][1];
yusuke_robocup 1:3b61675573b1 485
yusuke_robocup 1:3b61675573b1 486 }
yusuke_robocup 1:3b61675573b1 487
yusuke_robocup 1:3b61675573b1 488 if(direction == 1){
yusuke_robocup 1:3b61675573b1 489 vx = 15;
akudohune 0:bde8ed56b164 490 vy = 0;
akudohune 0:bde8ed56b164 491 }
yusuke_robocup 1:3b61675573b1 492
yusuke_robocup 1:3b61675573b1 493 if(direction == 15){
akudohune 0:bde8ed56b164 494 vx = -15;
yusuke_robocup 1:3b61675573b1 495 vy = 0;
akudohune 0:bde8ed56b164 496 }
yusuke_robocup 1:3b61675573b1 497
yusuke_robocup 1:3b61675573b1 498 if(direction == 2){
yusuke_robocup 1:3b61675573b1 499 vx = 20;
akudohune 0:bde8ed56b164 500 vy = -10;
yusuke_robocup 1:3b61675573b1 501 }
yusuke_robocup 1:3b61675573b1 502
yusuke_robocup 1:3b61675573b1 503 if(direction == 14){
yusuke_robocup 1:3b61675573b1 504 vx = -20;
akudohune 0:bde8ed56b164 505 vy = -10;
akudohune 0:bde8ed56b164 506 }
yusuke_robocup 1:3b61675573b1 507
akudohune 0:bde8ed56b164 508 }
yusuke_robocup 1:3b61675573b1 509
yusuke_robocup 1:3b61675573b1 510 if(Distance == 10){
yusuke_robocup 1:3b61675573b1 511 x_dista = 8*(-ball_sankaku[direction][0]);
yusuke_robocup 1:3b61675573b1 512 y_dista = 8*(-ball_sankaku[direction][1]);
yusuke_robocup 1:3b61675573b1 513
yusuke_robocup 1:3b61675573b1 514 x_turn = 22*(turn_sankaku[direction][0]);
yusuke_robocup 1:3b61675573b1 515 y_turn = 22*(turn_sankaku[direction][1]);
yusuke_robocup 1:3b61675573b1 516
yusuke_robocup 1:3b61675573b1 517
yusuke_robocup 1:3b61675573b1 518 if((direction == 2)||(direction == 14)){
yusuke_robocup 1:3b61675573b1 519 y_turn *= 0.7;
akudohune 0:bde8ed56b164 520 }
yusuke_robocup 1:3b61675573b1 521
yusuke_robocup 1:3b61675573b1 522 if(direction == 2){
yusuke_robocup 1:3b61675573b1 523 vx = 20;
yusuke_robocup 1:3b61675573b1 524 vy = -15;
akudohune 0:bde8ed56b164 525 }
yusuke_robocup 1:3b61675573b1 526 if(direction == 14){
yusuke_robocup 1:3b61675573b1 527 vx = -20;
yusuke_robocup 1:3b61675573b1 528 vy = -15;
yusuke_robocup 1:3b61675573b1 529 }
akudohune 0:bde8ed56b164 530 }
yusuke_robocup 1:3b61675573b1 531
yusuke_robocup 1:3b61675573b1 532 if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
yusuke_robocup 1:3b61675573b1 533 x_dista = 0;
yusuke_robocup 1:3b61675573b1 534 y_dista = 0;
akudohune 0:bde8ed56b164 535 }
akudohune 0:bde8ed56b164 536
yusuke_robocup 1:3b61675573b1 537 if(direction == 2){
akudohune 0:bde8ed56b164 538 vx = 20;
yusuke_robocup 1:3b61675573b1 539 vy = -15;
akudohune 0:bde8ed56b164 540 }
yusuke_robocup 1:3b61675573b1 541 if(direction == 14){
yusuke_robocup 1:3b61675573b1 542 vx = -20;
yusuke_robocup 1:3b61675573b1 543 vy = -15;
yusuke_robocup 1:3b61675573b1 544 }
yusuke_robocup 1:3b61675573b1 545
yusuke_robocup 1:3b61675573b1 546 vx = x_turn + x_dista;
yusuke_robocup 1:3b61675573b1 547 vy = y_turn + y_dista;
yusuke_robocup 1:3b61675573b1 548
yusuke_robocup 1:3b61675573b1 549 /*
yusuke_robocup 1:3b61675573b1 550 if((accelera_Distance == 90)){
yusuke_robocup 1:3b61675573b1 551 if(Distance == 10){
yusuke_robocup 1:3b61675573b1 552 vx *= 0.3;
yusuke_robocup 1:3b61675573b1 553 vy *= 0.3;
yusuke_robocup 1:3b61675573b1 554 }
yusuke_robocup 1:3b61675573b1 555
yusuke_robocup 1:3b61675573b1 556 if(Distance == 30){
yusuke_robocup 1:3b61675573b1 557 vx *= 0.4;
yusuke_robocup 1:3b61675573b1 558 vy *= 0.4;
yusuke_robocup 1:3b61675573b1 559 }
yusuke_robocup 1:3b61675573b1 560 }*/
yusuke_robocup 1:3b61675573b1 561 /*
yusuke_robocup 1:3b61675573b1 562 if((accelera_Distance == 10)){
yusuke_robocup 1:3b61675573b1 563 if((direction == 4)||(direction == 12)){
yusuke_robocup 1:3b61675573b1 564 vx = 0;
yusuke_robocup 1:3b61675573b1 565 vy = -10;
yusuke_robocup 1:3b61675573b1 566 }
yusuke_robocup 1:3b61675573b1 567 }*/
yusuke_robocup 1:3b61675573b1 568
yusuke_robocup 1:3b61675573b1 569 }else if(state_off == ATTACK){
yusuke_robocup 1:3b61675573b1 570 if(direction == 0){
yusuke_robocup 1:3b61675573b1 571 vx = 10*ball_sankaku[direction][0];
yusuke_robocup 1:3b61675573b1 572 vy =20*ball_sankaku[direction][1];
yusuke_robocup 1:3b61675573b1 573 /*
yusuke_robocup 1:3b61675573b1 574 if(ultrasonicVal[1] < 380){
yusuke_robocup 1:3b61675573b1 575 vy = 10;
yusuke_robocup 1:3b61675573b1 576 vx = -20;
yusuke_robocup 1:3b61675573b1 577 }
yusuke_robocup 1:3b61675573b1 578
yusuke_robocup 1:3b61675573b1 579 if(ultrasonicVal[3] < 380){
yusuke_robocup 1:3b61675573b1 580 vy = 10;
yusuke_robocup 1:3b61675573b1 581 vx = 20;
yusuke_robocup 1:3b61675573b1 582 } */
yusuke_robocup 1:3b61675573b1 583 }
yusuke_robocup 1:3b61675573b1 584 if(direction == 1){
yusuke_robocup 1:3b61675573b1 585 vx = 15*1.3;
yusuke_robocup 1:3b61675573b1 586 vy = 20*1.3;
yusuke_robocup 1:3b61675573b1 587 }
yusuke_robocup 1:3b61675573b1 588 if(direction == 15){
yusuke_robocup 1:3b61675573b1 589 vx = -15*1.3;
yusuke_robocup 1:3b61675573b1 590 vy = 20*1.3;
yusuke_robocup 1:3b61675573b1 591 }
yusuke_robocup 1:3b61675573b1 592 if(direction == 2){
yusuke_robocup 1:3b61675573b1 593 vx = 25;
yusuke_robocup 1:3b61675573b1 594 vy = 0;
yusuke_robocup 1:3b61675573b1 595 }
yusuke_robocup 1:3b61675573b1 596 if(direction == 14){
yusuke_robocup 1:3b61675573b1 597 vx = -25;
yusuke_robocup 1:3b61675573b1 598 vy = 0;
akudohune 0:bde8ed56b164 599 }
akudohune 0:bde8ed56b164 600
yusuke_robocup 1:3b61675573b1 601 if(Distance > 30){
yusuke_robocup 1:3b61675573b1 602 if(direction == 2){
yusuke_robocup 1:3b61675573b1 603 vx = 20;
yusuke_robocup 1:3b61675573b1 604 vy = 10;
yusuke_robocup 1:3b61675573b1 605 }
yusuke_robocup 1:3b61675573b1 606 if(direction == 14){
yusuke_robocup 1:3b61675573b1 607 vx = -20;
yusuke_robocup 1:3b61675573b1 608 vy = 10;
yusuke_robocup 1:3b61675573b1 609 }
yusuke_robocup 1:3b61675573b1 610 }
akudohune 0:bde8ed56b164 611
yusuke_robocup 1:3b61675573b1 612 }else if(state_off == SEARCH){
yusuke_robocup 1:3b61675573b1 613 vx = 24*ball_sankaku[direction][0];
yusuke_robocup 1:3b61675573b1 614 vy = 24*ball_sankaku[direction][1];
akudohune 0:bde8ed56b164 615
yusuke_robocup 1:3b61675573b1 616 if(direction == 2){
yusuke_robocup 1:3b61675573b1 617 vx = 20;
yusuke_robocup 1:3b61675573b1 618 }
yusuke_robocup 1:3b61675573b1 619 if(direction == 14){
yusuke_robocup 1:3b61675573b1 620 vx = -20;
yusuke_robocup 1:3b61675573b1 621 }
yusuke_robocup 1:3b61675573b1 622 if(direction == 0){
yusuke_robocup 1:3b61675573b1 623 vx = 20*ball_sankaku[direction][0];
yusuke_robocup 1:3b61675573b1 624 vy = 15*ball_sankaku[direction][1];
yusuke_robocup 1:3b61675573b1 625 }
yusuke_robocup 1:3b61675573b1 626 if(direction == 1){
yusuke_robocup 1:3b61675573b1 627 vx = 20*ball_sankaku[direction][0];
yusuke_robocup 1:3b61675573b1 628 vy = 10*ball_sankaku[direction][1];
yusuke_robocup 1:3b61675573b1 629 }
yusuke_robocup 1:3b61675573b1 630 if(direction == 15){
yusuke_robocup 1:3b61675573b1 631 vx = 20*ball_sankaku[direction][0];
yusuke_robocup 1:3b61675573b1 632 vy = 10*ball_sankaku[direction][1];
yusuke_robocup 1:3b61675573b1 633 }
yusuke_robocup 1:3b61675573b1 634 if(direction == 4){
yusuke_robocup 1:3b61675573b1 635 vx = 0;
yusuke_robocup 1:3b61675573b1 636 vy = -15;
yusuke_robocup 1:3b61675573b1 637 }
yusuke_robocup 1:3b61675573b1 638 if(direction == 12){
yusuke_robocup 1:3b61675573b1 639 vx = 0;
yusuke_robocup 1:3b61675573b1 640 vy = -15;
yusuke_robocup 1:3b61675573b1 641 }
yusuke_robocup 1:3b61675573b1 642 }
akudohune 0:bde8ed56b164 643
yusuke_robocup 1:3b61675573b1 644 if(direction == 2){
akudohune 0:bde8ed56b164 645 vx = 20;
yusuke_robocup 1:3b61675573b1 646 vy = 0;
yusuke_robocup 1:3b61675573b1 647
yusuke_robocup 1:3b61675573b1 648 }
yusuke_robocup 1:3b61675573b1 649 if(direction == 14){
akudohune 0:bde8ed56b164 650 vx = -20;
akudohune 0:bde8ed56b164 651 vy = 0;
yusuke_robocup 1:3b61675573b1 652 }
yusuke_robocup 1:3b61675573b1 653
yusuke_robocup 1:3b61675573b1 654 }else{
yusuke_robocup 1:3b61675573b1 655 vx = 0;
yusuke_robocup 1:3b61675573b1 656 vy = 0;
akudohune 0:bde8ed56b164 657 }
akudohune 0:bde8ed56b164 658
yusuke_robocup 1:3b61675573b1 659 vx *= 1.3* 0.8;
yusuke_robocup 1:3b61675573b1 660 vy *= 0.7 * 0.8;
yusuke_robocup 1:3b61675573b1 661
yusuke_robocup 1:3b61675573b1 662
yusuke_robocup 1:3b61675573b1 663
yusuke_robocup 1:3b61675573b1 664 if(lineStateX == LEFT_OUT){
yusuke_robocup 1:3b61675573b1 665 vx += 20;
yusuke_robocup 1:3b61675573b1 666 }else if(lineStateX == RIGHT_OUT){
yusuke_robocup 1:3b61675573b1 667 vx -= 20;
akudohune 0:bde8ed56b164 668 }
akudohune 0:bde8ed56b164 669
yusuke_robocup 1:3b61675573b1 670 if(lineStateY == FRONT_OUT){
yusuke_robocup 1:3b61675573b1 671 vy -= 15;
yusuke_robocup 1:3b61675573b1 672 }else if(lineStateY == BACK_OUT){
yusuke_robocup 1:3b61675573b1 673 vy += 15;
yusuke_robocup 1:3b61675573b1 674 }
yusuke_robocup 1:3b61675573b1 675
yusuke_robocup 1:3b61675573b1 676 //vx *= 0.8;
yusuke_robocup 1:3b61675573b1 677 //vy *= 0.8;
yusuke_robocup 1:3b61675573b1 678
yusuke_robocup 1:3b61675573b1 679 direction_past = direction;
yusuke_robocup 1:3b61675573b1 680
akudohune 0:bde8ed56b164 681
akudohune 0:bde8ed56b164 682 move(vx,vy,vs);
akudohune 0:bde8ed56b164 683 }
yusuke_robocup 1:3b61675573b1 684 }