yusuke takahashi / Mbed 2 deprecated ver1_2_2_1

sasasa

Dependencies:   HMC6352 PID eeprom mbed

Fork of ver1_2_2_1 by ryo seki

main.cpp@3:b4fb2b5365a7, 2013-04-18 (annotated)

Committer:
yusuke_robocup
Date:
Thu Apr 18 08:42:17 2013 +0000
Revision:
3:b4fb2b5365a7
Parent:
2:09fabba6c00d 
new new new

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusuke_robocup 3:b4fb2b5365a7 1 #include <mbed.h>
akudohune 0:74bf4953c0d1 2 #include <math.h>
akudohune 0:74bf4953c0d1 3 #include <sstream>
akudohune 0:74bf4953c0d1 4 #include "mbed.h"
akudohune 0:74bf4953c0d1 5 #include "HMC6352.h"
akudohune 0:74bf4953c0d1 6 #include "PID.h"
akudohune 0:74bf4953c0d1 7 #include "main.h"
yusuke_robocup 2:09fabba6c00d 8 #include "eeprom.h"
akudohune 0:74bf4953c0d1 9
akudohune 0:74bf4953c0d1 10
yusuke_robocup 2:09fabba6c00d 11 BusOut mbedleds(LED4,LED3,LED2,LED1);
akudohune 1:89408fff7cc9 12
akudohune 1:89408fff7cc9 13 void PidUpdata()
yusuke_robocup 2:09fabba6c00d 14 {
yusuke_robocup 2:09fabba6c00d 15 if(turn_flag){
yusuke_robocup 2:09fabba6c00d 16 now_compass = (((int)(compass.sample() - (past_compass) + 5400.0) % 3600) / 10.0);
yusuke_robocup 2:09fabba6c00d 17 }else{
yusuke_robocup 2:09fabba6c00d 18 inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);
akudohune 1:89408fff7cc9 19
akudohune 1:89408fff7cc9 20 //pc.printf("%f\n",timer1.read());
yusuke_robocup 2:09fabba6c00d 21 pid.setProcessValue(inputPID);
akudohune 1:89408fff7cc9 22 //timer1.reset();
akudohune 1:89408fff7cc9 23
yusuke_robocup 2:09fabba6c00d 24 compassPID = -(pid.compute());
yusuke_robocup 2:09fabba6c00d 25 }
akudohune 1:89408fff7cc9 26 //pc.printf("%f\n",compassPID);
akudohune 1:89408fff7cc9 27
akudohune 1:89408fff7cc9 28 }
akudohune 1:89408fff7cc9 29
akudohune 1:89408fff7cc9 30 void move(int vxx, int vyy, int vss)
akudohune 1:89408fff7cc9 31 {
akudohune 1:89408fff7cc9 32 double motVal[MOT_NUM] = {0};
akudohune 1:89408fff7cc9 33
yusuke_robocup 3:b4fb2b5365a7 34 int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360;
yusuke_robocup 3:b4fb2b5365a7 35
yusuke_robocup 3:b4fb2b5365a7 36 double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1;
yusuke_robocup 3:b4fb2b5365a7 37
yusuke_robocup 3:b4fb2b5365a7 38 if((angle > 30)&&(angle < 80)){
yusuke_robocup 3:b4fb2b5365a7 39 hosei2 = 0.7;
yusuke_robocup 3:b4fb2b5365a7 40 hosei4 = 0.7;
yusuke_robocup 3:b4fb2b5365a7 41 }
yusuke_robocup 3:b4fb2b5365a7 42
yusuke_robocup 3:b4fb2b5365a7 43 motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1;
yusuke_robocup 3:b4fb2b5365a7 44 motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2)*hosei2;
yusuke_robocup 3:b4fb2b5365a7 45 motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3;
yusuke_robocup 3:b4fb2b5365a7 46 motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4)*hosei4;
akudohune 1:89408fff7cc9 47
akudohune 1:89408fff7cc9 48 for(uint8_t i = 0 ; i < MOT_NUM ; i++){
akudohune 1:89408fff7cc9 49 if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
akudohune 1:89408fff7cc9 50 else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
akudohune 1:89408fff7cc9 51 speed[i] = motVal[i];
akudohune 1:89408fff7cc9 52 }
akudohune 1:89408fff7cc9 53 /*
akudohune 1:89408fff7cc9 54 pc.printf("speed1 = %d\n",speed[0]);
akudohune 1:89408fff7cc9 55 pc.printf("speed2 = %d\n",speed[1]);
akudohune 1:89408fff7cc9 56 pc.printf("speed3 = %d\n",speed[2]);
akudohune 1:89408fff7cc9 57 pc.printf("speed4 = %d\n\n",speed[3]);
akudohune 1:89408fff7cc9 58 */
akudohune 1:89408fff7cc9 59 ////pc.printf("%s",StringFIN.c_str());
akudohune 1:89408fff7cc9 60 }
akudohune 1:89408fff7cc9 61
akudohune 0:74bf4953c0d1 62 /*********** Serial interrupt ***********/
akudohune 0:74bf4953c0d1 63
akudohune 0:74bf4953c0d1 64 void Tx_interrupt()
akudohune 0:74bf4953c0d1 65 {
akudohune 0:74bf4953c0d1 66 array(speed[0],speed[1],speed[2],speed[3]);
akudohune 0:74bf4953c0d1 67 driver.printf("%s",StringFIN.c_str());
akudohune 0:74bf4953c0d1 68 //pc.printf("%s",StringFIN.c_str());
akudohune 0:74bf4953c0d1 69 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
akudohune 0:74bf4953c0d1 70 }
akudohune 0:74bf4953c0d1 71 /*
akudohune 0:74bf4953c0d1 72 void Rx_interrupt()
akudohune 0:74bf4953c0d1 73 {
akudohune 0:74bf4953c0d1 74 if(driver.readable()){
akudohune 0:74bf4953c0d1 75 //pc.printf("%d\n",driver.getc());
akudohune 0:74bf4953c0d1 76 }
akudohune 0:74bf4953c0d1 77 }*/
akudohune 0:74bf4953c0d1 78
akudohune 0:74bf4953c0d1 79
akudohune 0:74bf4953c0d1 80 /*********** Serial interrupt end **********/
akudohune 0:74bf4953c0d1 81
akudohune 0:74bf4953c0d1 82
akudohune 0:74bf4953c0d1 83 void init()
akudohune 0:74bf4953c0d1 84 {
yusuke_robocup 2:09fabba6c00d 85 uint8_t initFlag = 0;
yusuke_robocup 2:09fabba6c00d 86 char *hozon;
akudohune 0:74bf4953c0d1 87
akudohune 0:74bf4953c0d1 88 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
akudohune 0:74bf4953c0d1 89 StartButton.mode(PullUp);
akudohune 0:74bf4953c0d1 90 CalibEnterButton.mode(PullUp);
akudohune 0:74bf4953c0d1 91 CalibExitButton.mode(PullUp);
yusuke_robocup 2:09fabba6c00d 92 EEPROMButton.mode(PullUp);
akudohune 0:74bf4953c0d1 93 driver.baud(BAUD_RATE);
akudohune 0:74bf4953c0d1 94 wait_ms(MOTDRIVER_WAIT);
akudohune 0:74bf4953c0d1 95 driver.printf("1F0002F0003F0004F000\r\n");
akudohune 0:74bf4953c0d1 96
akudohune 0:74bf4953c0d1 97 led1 = ON;
akudohune 0:74bf4953c0d1 98
akudohune 0:74bf4953c0d1 99 while(StartButton){
akudohune 0:74bf4953c0d1 100 if(!CalibEnterButton){
akudohune 0:74bf4953c0d1 101 led1 = OFF;
akudohune 0:74bf4953c0d1 102 led2 = ON;
akudohune 0:74bf4953c0d1 103 compass.setCalibrationMode(ENTER);
akudohune 0:74bf4953c0d1 104 while(CalibExitButton);
akudohune 0:74bf4953c0d1 105 compass.setCalibrationMode(EXIT);
akudohune 0:74bf4953c0d1 106 led2 = OFF;
akudohune 0:74bf4953c0d1 107 led3 = ON;
akudohune 0:74bf4953c0d1 108 }
yusuke_robocup 2:09fabba6c00d 109 if(!EEPROMButton){
yusuke_robocup 2:09fabba6c00d 110 initFlag = 1;
yusuke_robocup 2:09fabba6c00d 111 read_eeprom(hozon,(char *)&standard,sizeof(hozon));
yusuke_robocup 2:09fabba6c00d 112 }
akudohune 0:74bf4953c0d1 113 }
yusuke_robocup 2:09fabba6c00d 114 if(!initFlag){
yusuke_robocup 2:09fabba6c00d 115 standard = compass.sample() / 10.0;
yusuke_robocup 2:09fabba6c00d 116 write_eeprom((char *)&standard,hozon,sizeof((char *)&standard));
yusuke_robocup 2:09fabba6c00d 117 }
akudohune 0:74bf4953c0d1 118 led1 = OFF;
akudohune 0:74bf4953c0d1 119 led3 = OFF;
akudohune 0:74bf4953c0d1 120
akudohune 0:74bf4953c0d1 121 pid.setInputLimits(0.0, 360.0);
akudohune 0:74bf4953c0d1 122 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
akudohune 0:74bf4953c0d1 123 pid.setBias(0.0);
akudohune 0:74bf4953c0d1 124 pid.setMode(AUTO_MODE);
akudohune 0:74bf4953c0d1 125 pid.setSetPoint(180.0);
akudohune 0:74bf4953c0d1 126
akudohune 0:74bf4953c0d1 127 pidUpdata.attach(&PidUpdata, 0.06);
yusuke_robocup 2:09fabba6c00d 128 wait(0.3);
yusuke_robocup 2:09fabba6c00d 129 IR.attach(&IR_Position,0.06);
akudohune 0:74bf4953c0d1 130 ultrasonic.attach(&Ultrasonic, 0.05);
akudohune 0:74bf4953c0d1 131 driver.attach(&Tx_interrupt, Serial::TxIrq);
akudohune 0:74bf4953c0d1 132 //driver.attach(&Rx_interrupt, Serial::RxIrq);
akudohune 0:74bf4953c0d1 133
akudohune 0:74bf4953c0d1 134 timer1.start();
akudohune 0:74bf4953c0d1 135 timer2.start();
akudohune 0:74bf4953c0d1 136 }
akudohune 0:74bf4953c0d1 137
yusuke_robocup 2:09fabba6c00d 138 uint16_t moving_ave_5point(uint16_t data)
yusuke_robocup 2:09fabba6c00d 139 {
yusuke_robocup 2:09fabba6c00d 140 static uint16_t tmp[5] = {0};
yusuke_robocup 2:09fabba6c00d 141 static uint32_t sum = 0;
yusuke_robocup 2:09fabba6c00d 142 uint8_t i;
yusuke_robocup 2:09fabba6c00d 143 uint8_t count;
yusuke_robocup 2:09fabba6c00d 144
yusuke_robocup 2:09fabba6c00d 145 sum -= tmp[4];
yusuke_robocup 2:09fabba6c00d 146 sum += data;
yusuke_robocup 2:09fabba6c00d 147 tmp[4] = tmp[3];
yusuke_robocup 2:09fabba6c00d 148 tmp[3] = tmp[2];
yusuke_robocup 2:09fabba6c00d 149 tmp[2] = tmp[1];
yusuke_robocup 2:09fabba6c00d 150 tmp[1] = tmp[0];
yusuke_robocup 2:09fabba6c00d 151 tmp[0] = data;
yusuke_robocup 2:09fabba6c00d 152
yusuke_robocup 2:09fabba6c00d 153 return sum/5;
yusuke_robocup 2:09fabba6c00d 154 }
yusuke_robocup 2:09fabba6c00d 155
yusuke_robocup 2:09fabba6c00d 156
akudohune 0:74bf4953c0d1 157 int main()
akudohune 0:74bf4953c0d1 158 {
akudohune 0:74bf4953c0d1 159 int vx=0,vy=0,vs=0;
yusuke_robocup 2:09fabba6c00d 160 int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
yusuke_robocup 2:09fabba6c00d 161 int state = NONE;
yusuke_robocup 2:09fabba6c00d 162 int direction_av = 0;
yusuke_robocup 2:09fabba6c00d 163 int direction_past = 0;
akudohune 0:74bf4953c0d1 164
akudohune 0:74bf4953c0d1 165 init();
akudohune 0:74bf4953c0d1 166
akudohune 0:74bf4953c0d1 167 while(1) {
yusuke_robocup 2:09fabba6c00d 168 x_dista = 0;
yusuke_robocup 2:09fabba6c00d 169 y_dista = 0;
yusuke_robocup 2:09fabba6c00d 170 x_turn = 0;
yusuke_robocup 2:09fabba6c00d 171 y_turn = 0;
yusuke_robocup 2:09fabba6c00d 172 turn_flag = 0;
yusuke_robocup 2:09fabba6c00d 173
akudohune 0:74bf4953c0d1 174 vs = compassPID;
yusuke_robocup 2:09fabba6c00d 175 //vs = 0;
yusuke_robocup 2:09fabba6c00d 176 //past_compass = compass.sample() / 1.0;
yusuke_robocup 2:09fabba6c00d 177 //float now_compass = 180.0;
yusuke_robocup 2:09fabba6c00d 178 /*
yusuke_robocup 2:09fabba6c00d 179 while(1){
yusuke_robocup 3:b4fb2b5365a7 180 vx = 10;
yusuke_robocup 2:09fabba6c00d 181 vy = 10;
yusuke_robocup 2:09fabba6c00d 182 vs = compassPID;
yusuke_robocup 2:09fabba6c00d 183
yusuke_robocup 2:09fabba6c00d 184 move(vx,vy,vs);
yusuke_robocup 2:09fabba6c00d 185 }*/
akudohune 1:89408fff7cc9 186
yusuke_robocup 2:09fabba6c00d 187 direction_av = moving_ave_5point(direction);
yusuke_robocup 2:09fabba6c00d 188
yusuke_robocup 2:09fabba6c00d 189 if(direction_av == 0){
yusuke_robocup 2:09fabba6c00d 190 state = ATTACK;
yusuke_robocup 2:09fabba6c00d 191 }
yusuke_robocup 2:09fabba6c00d 192 if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 30)){
yusuke_robocup 2:09fabba6c00d 193 state = SNAKE;
yusuke_robocup 2:09fabba6c00d 194 }
yusuke_robocup 3:b4fb2b5365a7 195 if(Distance >= 90){
yusuke_robocup 2:09fabba6c00d 196 state = SEARCH;
yusuke_robocup 2:09fabba6c00d 197 }
yusuke_robocup 2:09fabba6c00d 198
akudohune 0:74bf4953c0d1 199 if(IR_found){
yusuke_robocup 2:09fabba6c00d 200 if(state == SNAKE){
yusuke_robocup 2:09fabba6c00d 201 if(Distance == 30){
yusuke_robocup 2:09fabba6c00d 202 x_dista = 20*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 203 y_dista = 20*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 204
yusuke_robocup 2:09fabba6c00d 205 x_turn = 10*(turn_sankaku[direction][0]);
yusuke_robocup 2:09fabba6c00d 206 y_turn = 10*(turn_sankaku[direction][1]);
yusuke_robocup 2:09fabba6c00d 207
yusuke_robocup 2:09fabba6c00d 208 if((direction == 2)||(direction == 14)){
yusuke_robocup 2:09fabba6c00d 209 x_turn *= 0.7;
yusuke_robocup 2:09fabba6c00d 210 y_turn *= 0.7;
yusuke_robocup 2:09fabba6c00d 211 }
yusuke_robocup 2:09fabba6c00d 212 }
yusuke_robocup 2:09fabba6c00d 213
yusuke_robocup 2:09fabba6c00d 214 if(Distance == 10){
yusuke_robocup 2:09fabba6c00d 215 x_dista = 8*(-ball_sankaku[direction][0]);
yusuke_robocup 2:09fabba6c00d 216 y_dista = 8*(-ball_sankaku[direction][1]);
yusuke_robocup 2:09fabba6c00d 217
yusuke_robocup 2:09fabba6c00d 218 x_turn = 22*(turn_sankaku[direction][0]);
yusuke_robocup 2:09fabba6c00d 219 y_turn = 22*(turn_sankaku[direction][1]);
yusuke_robocup 2:09fabba6c00d 220
yusuke_robocup 2:09fabba6c00d 221
yusuke_robocup 2:09fabba6c00d 222 if(direction == 2){
yusuke_robocup 2:09fabba6c00d 223 vs = -3;
yusuke_robocup 2:09fabba6c00d 224 }
yusuke_robocup 2:09fabba6c00d 225 if(direction == 14){
yusuke_robocup 2:09fabba6c00d 226 vs = 3;
yusuke_robocup 2:09fabba6c00d 227 }
yusuke_robocup 2:09fabba6c00d 228
yusuke_robocup 2:09fabba6c00d 229 if((direction == 2)||(direction == 14)){
yusuke_robocup 2:09fabba6c00d 230 x_turn *= 0.7;
yusuke_robocup 2:09fabba6c00d 231 y_turn *= 0.7;
yusuke_robocup 2:09fabba6c00d 232 }
yusuke_robocup 2:09fabba6c00d 233 }
yusuke_robocup 2:09fabba6c00d 234
yusuke_robocup 2:09fabba6c00d 235 if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
yusuke_robocup 2:09fabba6c00d 236 x_dista = 0;
yusuke_robocup 2:09fabba6c00d 237 y_dista = 0;
yusuke_robocup 2:09fabba6c00d 238 }
yusuke_robocup 2:09fabba6c00d 239
yusuke_robocup 2:09fabba6c00d 240 vx = x_turn + x_dista;
yusuke_robocup 2:09fabba6c00d 241 vy = y_turn + y_dista;
yusuke_robocup 2:09fabba6c00d 242
yusuke_robocup 2:09fabba6c00d 243 }else if(state == ATTACK){
yusuke_robocup 2:09fabba6c00d 244 if(direction == 0){
yusuke_robocup 2:09fabba6c00d 245 vx = 10*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 246 vy = 15*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 247 }
yusuke_robocup 2:09fabba6c00d 248 if(direction == 1){
yusuke_robocup 3:b4fb2b5365a7 249 vx = 20*ball_sankaku[direction][0];
yusuke_robocup 3:b4fb2b5365a7 250 vy = 12*ball_sankaku[direction][1];
yusuke_robocup 3:b4fb2b5365a7 251 vs = -2;
yusuke_robocup 2:09fabba6c00d 252 }
yusuke_robocup 2:09fabba6c00d 253 if(direction == 15){
yusuke_robocup 3:b4fb2b5365a7 254 vx = 20*ball_sankaku[direction][0];
yusuke_robocup 3:b4fb2b5365a7 255 vy = 12*ball_sankaku[direction][1];
yusuke_robocup 3:b4fb2b5365a7 256 vs = 2;
yusuke_robocup 2:09fabba6c00d 257 }
yusuke_robocup 2:09fabba6c00d 258 if(direction == 2){
yusuke_robocup 2:09fabba6c00d 259 vx = 10*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 260 vy = 10*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 261 }
yusuke_robocup 2:09fabba6c00d 262 if(direction == 14){
yusuke_robocup 2:09fabba6c00d 263 vx = 10*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 264 vy = 10*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 265 }
yusuke_robocup 2:09fabba6c00d 266
yusuke_robocup 2:09fabba6c00d 267 }else if(state == SEARCH){
yusuke_robocup 3:b4fb2b5365a7 268 vx = 15*ball_sankaku[direction][0];
yusuke_robocup 3:b4fb2b5365a7 269 vy = 15*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 270
yusuke_robocup 2:09fabba6c00d 271 if(direction == 2){
yusuke_robocup 2:09fabba6c00d 272 vx = 25*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 273 vy = 10*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 274 vs = -2;
yusuke_robocup 2:09fabba6c00d 275 }
yusuke_robocup 2:09fabba6c00d 276 if(direction == 14){
yusuke_robocup 2:09fabba6c00d 277 vx = -25*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 278 vy = 10*ball_sankaku[direction][1];
yusuke_robocup 3:b4fb2b5365a7 279 //vs = 2;
yusuke_robocup 2:09fabba6c00d 280 }
yusuke_robocup 2:09fabba6c00d 281 if(direction == 0){
yusuke_robocup 2:09fabba6c00d 282 vx = 10*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 283 vy = 15*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 284 }
yusuke_robocup 2:09fabba6c00d 285 if(direction == 1){
yusuke_robocup 2:09fabba6c00d 286 vx = 30*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 287 vy = 8*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 288 }
yusuke_robocup 2:09fabba6c00d 289 if(direction == 15){
yusuke_robocup 2:09fabba6c00d 290 vx = 30*ball_sankaku[direction][0];
yusuke_robocup 2:09fabba6c00d 291 vy = 8*ball_sankaku[direction][1];
yusuke_robocup 2:09fabba6c00d 292 }
yusuke_robocup 2:09fabba6c00d 293 }
akudohune 0:74bf4953c0d1 294 }else{
akudohune 0:74bf4953c0d1 295 vx = 0;
akudohune 0:74bf4953c0d1 296 vy = 0;
akudohune 0:74bf4953c0d1 297 }
akudohune 1:89408fff7cc9 298
akudohune 0:74bf4953c0d1 299 /*
yusuke_robocup 2:09fabba6c00d 300 if((ultrasonicVal[0]<50)||(ultrasonicVal[1]<50)||(ultrasonicVal[3]<50)){
yusuke_robocup 2:09fabba6c00d 301 vx = (int)((30-FULL)*ball_sankaku[direction][0]);
yusuke_robocup 2:09fabba6c00d 302 vy = (int)((30-FULL)*ball_sankaku[direction][1]);
yusuke_robocup 2:09fabba6c00d 303 }
yusuke_robocup 2:09fabba6c00d 304
yusuke_robocup 2:09fabba6c00d 305
yusuke_robocup 2:09fabba6c00d 306
yusuke_robocup 2:09fabba6c00d 307 if(Distance == 10){
yusuke_robocup 2:09fabba6c00d 308 mbedleds = 0xF;
yusuke_robocup 2:09fabba6c00d 309 }else{
yusuke_robocup 2:09fabba6c00d 310 mbedleds = 0x0;
yusuke_robocup 2:09fabba6c00d 311 }
yusuke_robocup 2:09fabba6c00d 312
yusuke_robocup 2:09fabba6c00d 313 if((ultrasonicVal[0]<100)&&(ultrasonicVal[1]<100)&&(ultrasonicVal[2]<100)&&((direction == 0)||(direction == 1)||(direction == 15))){
yusuke_robocup 2:09fabba6c00d 314 vx = 0;
yusuke_robocup 2:09fabba6c00d 315 vy = 0;
yusuke_robocup 2:09fabba6c00d 316 vs = 0;
yusuke_robocup 2:09fabba6c00d 317 past_compass = compass.sample() / 1.0;
yusuke_robocup 2:09fabba6c00d 318 float now_compass = 180.0;
yusuke_robocup 2:09fabba6c00d 319
yusuke_robocup 2:09fabba6c00d 320 if(inputPID > 180){
yusuke_robocup 2:09fabba6c00d 321 turn_flag = 1;
yusuke_robocup 2:09fabba6c00d 322 while((now_compass > 180.0 - (SHINPUKU / 2.0))&&((direction == 0)||(direction == 1)||(direction == 15))){
yusuke_robocup 2:09fabba6c00d 323 vs = 10;
yusuke_robocup 2:09fabba6c00d 324 move(vx,vy,vs);
akudohune 0:74bf4953c0d1 325 }
yusuke_robocup 2:09fabba6c00d 326 turn_flag = 0;
akudohune 0:74bf4953c0d1 327 }
akudohune 0:74bf4953c0d1 328
yusuke_robocup 2:09fabba6c00d 329 if(inputPID < 180){
yusuke_robocup 2:09fabba6c00d 330 turn_flag = 1;
yusuke_robocup 2:09fabba6c00d 331 while((now_compass < 180.0 + (SHINPUKU / 2.0))&&((direction == 0)||(direction == 1)||(direction == 15))){
yusuke_robocup 2:09fabba6c00d 332 vs = -10;
yusuke_robocup 2:09fabba6c00d 333 move(vx,vy,vs);
akudohune 1:89408fff7cc9 334 }
yusuke_robocup 2:09fabba6c00d 335 turn_flag = 0;
yusuke_robocup 2:09fabba6c00d 336 }
akudohune 1:89408fff7cc9 337 }*/
akudohune 1:89408fff7cc9 338
yusuke_robocup 2:09fabba6c00d 339 if(state == SNAKE){
yusuke_robocup 2:09fabba6c00d 340 mbedleds = 0xF;
yusuke_robocup 2:09fabba6c00d 341 }else{
yusuke_robocup 2:09fabba6c00d 342 mbedleds = 0x0;
yusuke_robocup 2:09fabba6c00d 343 }
yusuke_robocup 2:09fabba6c00d 344
yusuke_robocup 2:09fabba6c00d 345
yusuke_robocup 2:09fabba6c00d 346 vx *= 1;
yusuke_robocup 2:09fabba6c00d 347 vy *= 1;
yusuke_robocup 2:09fabba6c00d 348
yusuke_robocup 2:09fabba6c00d 349 direction_past = direction;
yusuke_robocup 2:09fabba6c00d 350
akudohune 0:74bf4953c0d1 351 move(vx,vy,vs);
akudohune 0:74bf4953c0d1 352 }
akudohune 0:74bf4953c0d1 353 }