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@2:09fabba6c00d, 2013-04-05 (annotated)

Committer:
yusuke_robocup
Date:
Fri Apr 05 07:26:42 2013 +0000
Revision:
2:09fabba6c00d
Parent:
1:89408fff7cc9
Child:
3:b4fb2b5365a7
aa

Who changed what in which revision?

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