ryo seki / Mbed 2 deprecated ver2_0_1

a

Dependencies:   HMC6352 PID mbed

main.cpp@0:8215f0743d86, 2013-04-19 (annotated)

Committer:
akudohune
Date:
Fri Apr 19 09:13:32 2013 +0000
Revision:
0:8215f0743d86
to yusuke

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akudohune 0:8215f0743d86 1 #include <math.h>
akudohune 0:8215f0743d86 2 #include <sstream>
akudohune 0:8215f0743d86 3 #include "mbed.h"
akudohune 0:8215f0743d86 4 #include "HMC6352.h"
akudohune 0:8215f0743d86 5 #include "PID.h"
akudohune 0:8215f0743d86 6 #include "main.h"
akudohune 0:8215f0743d86 7
akudohune 0:8215f0743d86 8
akudohune 0:8215f0743d86 9
akudohune 0:8215f0743d86 10 void PidUpdata()
akudohune 0:8215f0743d86 11 {
akudohune 0:8215f0743d86 12 inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);
akudohune 0:8215f0743d86 13
akudohune 0:8215f0743d86 14 //pc.printf("%f\n",timer1.read());
akudohune 0:8215f0743d86 15 pid.setProcessValue(inputPID);
akudohune 0:8215f0743d86 16 //timer1.reset();
akudohune 0:8215f0743d86 17
akudohune 0:8215f0743d86 18 compassPID = -(pid.compute());
akudohune 0:8215f0743d86 19 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
akudohune 0:8215f0743d86 20 //pc.printf("%f\n",compassPID);
akudohune 0:8215f0743d86 21 }
akudohune 0:8215f0743d86 22
akudohune 0:8215f0743d86 23 void move(int vxx, int vyy, int vss)
akudohune 0:8215f0743d86 24 {
akudohune 0:8215f0743d86 25 double motVal[MOT_NUM] = {0};
akudohune 0:8215f0743d86 26
akudohune 0:8215f0743d86 27 motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
akudohune 0:8215f0743d86 28 motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2);
akudohune 0:8215f0743d86 29 motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
akudohune 0:8215f0743d86 30 motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4);
akudohune 0:8215f0743d86 31
akudohune 0:8215f0743d86 32 for(uint8_t i = 0 ; i < MOT_NUM ; i++){
akudohune 0:8215f0743d86 33 if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
akudohune 0:8215f0743d86 34 else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
akudohune 0:8215f0743d86 35 speed[i] = motVal[i];
akudohune 0:8215f0743d86 36 }
akudohune 0:8215f0743d86 37 //pc.printf("%s",StringFIN.c_str());
akudohune 0:8215f0743d86 38 }
akudohune 0:8215f0743d86 39
akudohune 0:8215f0743d86 40 /*********** Serial interrupt ***********/
akudohune 0:8215f0743d86 41
akudohune 0:8215f0743d86 42 void Tx_interrupt()
akudohune 0:8215f0743d86 43 {
akudohune 0:8215f0743d86 44 array(speed[0],speed[1],speed[2],speed[3]);
akudohune 0:8215f0743d86 45 driver.printf("%s",StringFIN.c_str());
akudohune 0:8215f0743d86 46 //pc.printf("%s",StringFIN.c_str());
akudohune 0:8215f0743d86 47 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
akudohune 0:8215f0743d86 48 }
akudohune 0:8215f0743d86 49 /*
akudohune 0:8215f0743d86 50 void Rx_interrupt()
akudohune 0:8215f0743d86 51 {
akudohune 0:8215f0743d86 52 if(driver.readable()){
akudohune 0:8215f0743d86 53 //pc.printf("%d\n",driver.getc());
akudohune 0:8215f0743d86 54 }
akudohune 0:8215f0743d86 55 }*/
akudohune 0:8215f0743d86 56
akudohune 0:8215f0743d86 57
akudohune 0:8215f0743d86 58 /*********** Serial interrupt end **********/
akudohune 0:8215f0743d86 59
akudohune 0:8215f0743d86 60
akudohune 0:8215f0743d86 61 void init()
akudohune 0:8215f0743d86 62 {
akudohune 0:8215f0743d86 63 uint8_t initFlag = 0;
akudohune 0:8215f0743d86 64 FILE *fp;
akudohune 0:8215f0743d86 65
akudohune 0:8215f0743d86 66 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
akudohune 0:8215f0743d86 67
akudohune 0:8215f0743d86 68 StartButton.mode(PullUp);
akudohune 0:8215f0743d86 69 CalibEnterButton.mode(PullUp);
akudohune 0:8215f0743d86 70 CalibExitButton.mode(PullUp);
akudohune 0:8215f0743d86 71 EEPROMButton.mode(PullUp);
akudohune 0:8215f0743d86 72
akudohune 0:8215f0743d86 73 driver.baud(BAUD_RATE);
akudohune 0:8215f0743d86 74 wait_ms(MOTDRIVER_WAIT);
akudohune 0:8215f0743d86 75 driver.printf("1F0002F0003F0004F000\r\n");
akudohune 0:8215f0743d86 76 device2.printf("START");
akudohune 0:8215f0743d86 77
akudohune 0:8215f0743d86 78 device2.attach(&dev_rx,Serial::RxIrq);
akudohune 0:8215f0743d86 79 device2.attach(&dev_tx,Serial::TxIrq);
akudohune 0:8215f0743d86 80
akudohune 0:8215f0743d86 81 led1 = ON;
akudohune 0:8215f0743d86 82
akudohune 0:8215f0743d86 83 while(StartButton){
akudohune 0:8215f0743d86 84 if(!CalibEnterButton){
akudohune 0:8215f0743d86 85 led1 = OFF;
akudohune 0:8215f0743d86 86 led2 = ON;
akudohune 0:8215f0743d86 87 compass.setCalibrationMode(ENTER);
akudohune 0:8215f0743d86 88 while(CalibExitButton);
akudohune 0:8215f0743d86 89 compass.setCalibrationMode(EXIT);
akudohune 0:8215f0743d86 90 led2 = OFF;
akudohune 0:8215f0743d86 91 led3 = ON;
akudohune 0:8215f0743d86 92 }
akudohune 0:8215f0743d86 93 if(!EEPROMButton){
akudohune 0:8215f0743d86 94 led3 = OFF;
akudohune 0:8215f0743d86 95 led1 = OFF;
akudohune 0:8215f0743d86 96 led4 = ON;
akudohune 0:8215f0743d86 97 initFlag = 1;
akudohune 0:8215f0743d86 98 fp = fopen("/local/out.txt", "r");
akudohune 0:8215f0743d86 99 fscanf(fp, "%lf",&standard);
akudohune 0:8215f0743d86 100 fclose(fp);
akudohune 0:8215f0743d86 101 }
akudohune 0:8215f0743d86 102 }
akudohune 0:8215f0743d86 103
akudohune 0:8215f0743d86 104 if(!initFlag){
akudohune 0:8215f0743d86 105 standard = compass.sample() / 10.0;
akudohune 0:8215f0743d86 106 fp = fopen("/local/out.txt", "w");
akudohune 0:8215f0743d86 107 fprintf(fp, "%f",standard);
akudohune 0:8215f0743d86 108 fclose(fp);
akudohune 0:8215f0743d86 109 }
akudohune 0:8215f0743d86 110
akudohune 0:8215f0743d86 111 led1 = OFF;
akudohune 0:8215f0743d86 112 led3 = OFF;
akudohune 0:8215f0743d86 113 led4 = OFF;
akudohune 0:8215f0743d86 114
akudohune 0:8215f0743d86 115 pid.setInputLimits(0.0, 360.0);
akudohune 0:8215f0743d86 116 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
akudohune 0:8215f0743d86 117 pid.setBias(0.0);
akudohune 0:8215f0743d86 118 pid.setMode(AUTO_MODE);
akudohune 0:8215f0743d86 119 pid.setSetPoint(180.0);
akudohune 0:8215f0743d86 120
akudohune 0:8215f0743d86 121 pidUpdata.attach(&PidUpdata, 0.06);
akudohune 0:8215f0743d86 122 wait_ms(500); //danger
akudohune 0:8215f0743d86 123 driver.attach(&Tx_interrupt, Serial::TxIrq);
akudohune 0:8215f0743d86 124 //driver.attach(&Rx_interrupt, Serial::RxIrq);
akudohune 0:8215f0743d86 125
akudohune 0:8215f0743d86 126 //timer1.start();
akudohune 0:8215f0743d86 127 }
akudohune 0:8215f0743d86 128
akudohune 0:8215f0743d86 129 int main()
akudohune 0:8215f0743d86 130 {
akudohune 0:8215f0743d86 131 int vx=0,vy=0,vs=0;
akudohune 0:8215f0743d86 132 uint8_t flag = 0;
akudohune 0:8215f0743d86 133 uint8_t comp_flag6 = 0;
akudohune 0:8215f0743d86 134 uint8_t comp_flag10 = 0;
akudohune 0:8215f0743d86 135
akudohune 0:8215f0743d86 136 init();
akudohune 0:8215f0743d86 137
akudohune 0:8215f0743d86 138 while(1){
akudohune 0:8215f0743d86 139 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
akudohune 0:8215f0743d86 140 //wait(0.1);
akudohune 0:8215f0743d86 141 //pc.printf("%d\t,%d\n",direction,Distance);
akudohune 0:8215f0743d86 142
akudohune 0:8215f0743d86 143 //pc.printf("%d\n",distance);
akudohune 0:8215f0743d86 144
akudohune 0:8215f0743d86 145 vx = 0;
akudohune 0:8215f0743d86 146 vy = 0;
akudohune 0:8215f0743d86 147 vs = compassPID;
akudohune 0:8215f0743d86 148
akudohune 0:8215f0743d86 149 /*************** Change state **************************/
akudohune 0:8215f0743d86 150
akudohune 0:8215f0743d86 151 if((Distance <= 30) && (IR_found) && (!flag)){
akudohune 0:8215f0743d86 152 state = DIFFENCE;
akudohune 0:8215f0743d86 153 }else{
akudohune 0:8215f0743d86 154 if((direction == 4) || (direction == 6)|| (direction == 8) || (direction == 10)|| (direction == 12)){
akudohune 0:8215f0743d86 155 state = DIFFENCE;
akudohune 0:8215f0743d86 156 }else if((Distance <= 120) && (IR_found) && (!flag)){
akudohune 0:8215f0743d86 157 state = WARNING;
akudohune 0:8215f0743d86 158 }else{
akudohune 0:8215f0743d86 159 state = HOME_WAIT;
akudohune 0:8215f0743d86 160 }
akudohune 0:8215f0743d86 161 }
akudohune 0:8215f0743d86 162
akudohune 0:8215f0743d86 163 if((IR_found) && (!flag)){
akudohune 0:8215f0743d86 164 state = WARNING;
akudohune 0:8215f0743d86 165 }else{
akudohune 0:8215f0743d86 166 state = HOME_WAIT;
akudohune 0:8215f0743d86 167 }
akudohune 0:8215f0743d86 168
akudohune 0:8215f0743d86 169 /*************** Change state **************************/
akudohune 0:8215f0743d86 170
akudohune 0:8215f0743d86 171
akudohune 0:8215f0743d86 172 if(state == HOME_WAIT){
akudohune 0:8215f0743d86 173 comp_flag6 = 0;
akudohune 0:8215f0743d86 174 comp_flag10 = 0;
akudohune 0:8215f0743d86 175 if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 600.0)){
akudohune 0:8215f0743d86 176 if((BACK_SONIC > 115.0) && (BACK_SONIC < 125.0)){
akudohune 0:8215f0743d86 177 vy = 0;
akudohune 0:8215f0743d86 178 flag = 0;
akudohune 0:8215f0743d86 179 }else if(BACK_SONIC <= 115.0){
akudohune 0:8215f0743d86 180 vy = 3;
akudohune 0:8215f0743d86 181 }else if(BACK_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 182 vy = 5;
akudohune 0:8215f0743d86 183 }else{
akudohune 0:8215f0743d86 184 vy = (int)(0.05 * (120.0 - BACK_SONIC) - 4);
akudohune 0:8215f0743d86 185 if(vy < -30)vy = -30;
akudohune 0:8215f0743d86 186 }
akudohune 0:8215f0743d86 187
akudohune 0:8215f0743d86 188 if((LEFT_SONIC > 450.0) && (RIGHT_SONIC > 450.0)){
akudohune 0:8215f0743d86 189 vx = 0;
akudohune 0:8215f0743d86 190 }else if(RIGHT_SONIC < 450.0){
akudohune 0:8215f0743d86 191 vx = (int)((RIGHT_SONIC - 450.0) / 15.0 - 10.0);
akudohune 0:8215f0743d86 192 if(vx < -30)vx = -30;
akudohune 0:8215f0743d86 193 }else if(LEFT_SONIC < 450.0){
akudohune 0:8215f0743d86 194 vx = (int)((450.0 - LEFT_SONIC ) / 15.0 + 10.0);
akudohune 0:8215f0743d86 195 if(vx > 30)vx = 30;
akudohune 0:8215f0743d86 196 }
akudohune 0:8215f0743d86 197 }else if((RIGHT_SONIC + LEFT_SONIC) <= 600.0){
akudohune 0:8215f0743d86 198 if(BACK_SONIC < 100.0){
akudohune 0:8215f0743d86 199 if(RIGHT_SONIC > LEFT_SONIC){
akudohune 0:8215f0743d86 200 vx = 5;
akudohune 0:8215f0743d86 201 vy = 10;
akudohune 0:8215f0743d86 202 }else{
akudohune 0:8215f0743d86 203 vx = -5;
akudohune 0:8215f0743d86 204 vy = 10;
akudohune 0:8215f0743d86 205 }
akudohune 0:8215f0743d86 206 }else if(BACK_SONIC < 200.0){
akudohune 0:8215f0743d86 207 if(RIGHT_SONIC > LEFT_SONIC){
akudohune 0:8215f0743d86 208 vx = 20;
akudohune 0:8215f0743d86 209 vy = 0;
akudohune 0:8215f0743d86 210 }else{
akudohune 0:8215f0743d86 211 vx = -20;
akudohune 0:8215f0743d86 212 vy = 0;
akudohune 0:8215f0743d86 213 }
akudohune 0:8215f0743d86 214 }
akudohune 0:8215f0743d86 215 }else{
akudohune 0:8215f0743d86 216 if(BACK_SONIC > 500.0){
akudohune 0:8215f0743d86 217 if(RIGHT_SONIC > LEFT_SONIC){
akudohune 0:8215f0743d86 218 vx = 25;
akudohune 0:8215f0743d86 219 vy = -10;
akudohune 0:8215f0743d86 220 }else{
akudohune 0:8215f0743d86 221 vx = -25;
akudohune 0:8215f0743d86 222 vy = -10;
akudohune 0:8215f0743d86 223 }
akudohune 0:8215f0743d86 224 }
akudohune 0:8215f0743d86 225 }
akudohune 0:8215f0743d86 226 }else if(state == DIFFENCE){
akudohune 0:8215f0743d86 227 if(direction == 6){
akudohune 0:8215f0743d86 228 comp_flag10 = 0;
akudohune 0:8215f0743d86 229 if(comp_flag6){
akudohune 0:8215f0743d86 230 if(BACK_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 231
akudohune 0:8215f0743d86 232 }else if(BACK_SONIC > 50.0){
akudohune 0:8215f0743d86 233 vy = -10;
akudohune 0:8215f0743d86 234 }else{
akudohune 0:8215f0743d86 235
akudohune 0:8215f0743d86 236 }
akudohune 0:8215f0743d86 237
akudohune 0:8215f0743d86 238 if(LEFT_SONIC < 500.0){
akudohune 0:8215f0743d86 239 vy = -20;
akudohune 0:8215f0743d86 240 }else if(LEFT_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 241 vx = 20;
akudohune 0:8215f0743d86 242 }else{
akudohune 0:8215f0743d86 243 vx = 20;
akudohune 0:8215f0743d86 244 vy = -20;
akudohune 0:8215f0743d86 245 }
akudohune 0:8215f0743d86 246 }else{
akudohune 0:8215f0743d86 247 if(BACK_SONIC < 110.0){
akudohune 0:8215f0743d86 248 vy = 5;
akudohune 0:8215f0743d86 249 }else if(BACK_SONIC > 500.0){
akudohune 0:8215f0743d86 250 vy = -20;
akudohune 0:8215f0743d86 251 }else if(BACK_SONIC > 300.0){
akudohune 0:8215f0743d86 252 vy = -10;
akudohune 0:8215f0743d86 253 }else if(BACK_SONIC > 140.0){
akudohune 0:8215f0743d86 254 vy = -5;
akudohune 0:8215f0743d86 255 }else{
akudohune 0:8215f0743d86 256 vy = 0;
akudohune 0:8215f0743d86 257 }
akudohune 0:8215f0743d86 258
akudohune 0:8215f0743d86 259 if(LEFT_SONIC < 700.0){
akudohune 0:8215f0743d86 260 vx = 20;
akudohune 0:8215f0743d86 261 }else{
akudohune 0:8215f0743d86 262 if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
akudohune 0:8215f0743d86 263 comp_flag6 = 1;
akudohune 0:8215f0743d86 264 }else{
akudohune 0:8215f0743d86 265 vx = 5;
akudohune 0:8215f0743d86 266 vy = -15;
akudohune 0:8215f0743d86 267 }
akudohune 0:8215f0743d86 268 }
akudohune 0:8215f0743d86 269 }
akudohune 0:8215f0743d86 270 }else if(direction == 10){
akudohune 0:8215f0743d86 271 comp_flag6 = 0;
akudohune 0:8215f0743d86 272 if(comp_flag10){
akudohune 0:8215f0743d86 273 if(BACK_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 274
akudohune 0:8215f0743d86 275 }else if(BACK_SONIC > 50.0){
akudohune 0:8215f0743d86 276 vy = -10;
akudohune 0:8215f0743d86 277 }else{
akudohune 0:8215f0743d86 278
akudohune 0:8215f0743d86 279 }
akudohune 0:8215f0743d86 280
akudohune 0:8215f0743d86 281 if(RIGHT_SONIC < 500.0){
akudohune 0:8215f0743d86 282 vy = -20;
akudohune 0:8215f0743d86 283 }else if(RIGHT_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 284 vx = -20;
akudohune 0:8215f0743d86 285 }else{
akudohune 0:8215f0743d86 286 vx = -20;
akudohune 0:8215f0743d86 287 vy = -20;
akudohune 0:8215f0743d86 288 }
akudohune 0:8215f0743d86 289 }else{
akudohune 0:8215f0743d86 290
akudohune 0:8215f0743d86 291 if(BACK_SONIC < 110.0){
akudohune 0:8215f0743d86 292 vy = 5;
akudohune 0:8215f0743d86 293 }else if(BACK_SONIC > 500.0){
akudohune 0:8215f0743d86 294 vy = -20;
akudohune 0:8215f0743d86 295 }else if(BACK_SONIC > 300.0){
akudohune 0:8215f0743d86 296 vy = -10;
akudohune 0:8215f0743d86 297 }else if(BACK_SONIC > 140.0){
akudohune 0:8215f0743d86 298 vy = -5;
akudohune 0:8215f0743d86 299 }else{
akudohune 0:8215f0743d86 300 vy = 0;
akudohune 0:8215f0743d86 301 }
akudohune 0:8215f0743d86 302
akudohune 0:8215f0743d86 303 if(RIGHT_SONIC < 700.0){
akudohune 0:8215f0743d86 304 vx = -20;
akudohune 0:8215f0743d86 305 }else{
akudohune 0:8215f0743d86 306 if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
akudohune 0:8215f0743d86 307 comp_flag10 = 1;
akudohune 0:8215f0743d86 308 }else{
akudohune 0:8215f0743d86 309 vx = -5;
akudohune 0:8215f0743d86 310 vy = -15;
akudohune 0:8215f0743d86 311 }
akudohune 0:8215f0743d86 312 }
akudohune 0:8215f0743d86 313 }
akudohune 0:8215f0743d86 314
akudohune 0:8215f0743d86 315 }else if(direction == 4){
akudohune 0:8215f0743d86 316 comp_flag10 = 0;
akudohune 0:8215f0743d86 317 if(comp_flag6){
akudohune 0:8215f0743d86 318 if(BACK_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 319
akudohune 0:8215f0743d86 320 }else if(BACK_SONIC > 50.0){
akudohune 0:8215f0743d86 321 vy = -10;
akudohune 0:8215f0743d86 322 }else{
akudohune 0:8215f0743d86 323
akudohune 0:8215f0743d86 324 }
akudohune 0:8215f0743d86 325
akudohune 0:8215f0743d86 326 if(LEFT_SONIC < 500.0){
akudohune 0:8215f0743d86 327 vy = -20;
akudohune 0:8215f0743d86 328 }else if(LEFT_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 329 vx = 20;
akudohune 0:8215f0743d86 330 }else{
akudohune 0:8215f0743d86 331 vx = 20;
akudohune 0:8215f0743d86 332 vy = -20;
akudohune 0:8215f0743d86 333 }
akudohune 0:8215f0743d86 334 }else{
akudohune 0:8215f0743d86 335 if(BACK_SONIC < 110.0){
akudohune 0:8215f0743d86 336 vy = 5;
akudohune 0:8215f0743d86 337 }else if(BACK_SONIC > 500.0){
akudohune 0:8215f0743d86 338 vy = -20;
akudohune 0:8215f0743d86 339 }else if(BACK_SONIC > 300.0){
akudohune 0:8215f0743d86 340 vy = -10;
akudohune 0:8215f0743d86 341 }else if(BACK_SONIC > 140.0){
akudohune 0:8215f0743d86 342 vy = -5;
akudohune 0:8215f0743d86 343 }else{
akudohune 0:8215f0743d86 344 vy = 0;
akudohune 0:8215f0743d86 345 }
akudohune 0:8215f0743d86 346
akudohune 0:8215f0743d86 347 if(LEFT_SONIC < 700.0){
akudohune 0:8215f0743d86 348 vx = 20;
akudohune 0:8215f0743d86 349 }else{
akudohune 0:8215f0743d86 350 if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
akudohune 0:8215f0743d86 351 comp_flag6 = 1;
akudohune 0:8215f0743d86 352 }else{
akudohune 0:8215f0743d86 353 vx = 5;
akudohune 0:8215f0743d86 354 vy = -15;
akudohune 0:8215f0743d86 355 }
akudohune 0:8215f0743d86 356 }
akudohune 0:8215f0743d86 357 }
akudohune 0:8215f0743d86 358 }else if(direction == 12){
akudohune 0:8215f0743d86 359 comp_flag6 = 0;
akudohune 0:8215f0743d86 360 if(comp_flag10){
akudohune 0:8215f0743d86 361 if(BACK_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 362
akudohune 0:8215f0743d86 363 }else if(BACK_SONIC > 50.0){
akudohune 0:8215f0743d86 364 vy = -10;
akudohune 0:8215f0743d86 365 }else{
akudohune 0:8215f0743d86 366
akudohune 0:8215f0743d86 367 }
akudohune 0:8215f0743d86 368
akudohune 0:8215f0743d86 369 if(RIGHT_SONIC < 500.0){
akudohune 0:8215f0743d86 370 vy = -20;
akudohune 0:8215f0743d86 371 }else if(RIGHT_SONIC == PING_ERROR){
akudohune 0:8215f0743d86 372 vx = -20;
akudohune 0:8215f0743d86 373 }else{
akudohune 0:8215f0743d86 374 vx = -20;
akudohune 0:8215f0743d86 375 vy = -20;
akudohune 0:8215f0743d86 376 }
akudohune 0:8215f0743d86 377 }else{
akudohune 0:8215f0743d86 378 if(BACK_SONIC < 110.0){
akudohune 0:8215f0743d86 379 vy = 5;
akudohune 0:8215f0743d86 380 }else if(BACK_SONIC > 500.0){
akudohune 0:8215f0743d86 381 vy = -20;
akudohune 0:8215f0743d86 382 }else if(BACK_SONIC > 300.0){
akudohune 0:8215f0743d86 383 vy = -10;
akudohune 0:8215f0743d86 384 }else if(BACK_SONIC > 140.0){
akudohune 0:8215f0743d86 385 vy = -5;
akudohune 0:8215f0743d86 386 }else{
akudohune 0:8215f0743d86 387 vy = 0;
akudohune 0:8215f0743d86 388 }
akudohune 0:8215f0743d86 389
akudohune 0:8215f0743d86 390 if(RIGHT_SONIC < 700.0){
akudohune 0:8215f0743d86 391 vx = -20;
akudohune 0:8215f0743d86 392 }else{
akudohune 0:8215f0743d86 393 if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
akudohune 0:8215f0743d86 394 comp_flag10 = 1;
akudohune 0:8215f0743d86 395 }else{
akudohune 0:8215f0743d86 396 vx = -5;
akudohune 0:8215f0743d86 397 vy = -15;
akudohune 0:8215f0743d86 398 }
akudohune 0:8215f0743d86 399 }
akudohune 0:8215f0743d86 400 }
akudohune 0:8215f0743d86 401 }else if(direction == 8){
akudohune 0:8215f0743d86 402 if(comp_flag6){
akudohune 0:8215f0743d86 403 vx = -15;
akudohune 0:8215f0743d86 404 vy = -20;
akudohune 0:8215f0743d86 405 }else if(comp_flag10){
akudohune 0:8215f0743d86 406 vx = 15;
akudohune 0:8215f0743d86 407 vy = -20;
akudohune 0:8215f0743d86 408 }else{
akudohune 0:8215f0743d86 409 if(LEFT_SONIC > RIGHT_SONIC){
akudohune 0:8215f0743d86 410 vx = -15;
akudohune 0:8215f0743d86 411 vy = -20;
akudohune 0:8215f0743d86 412 }else{
akudohune 0:8215f0743d86 413 vx = 15;
akudohune 0:8215f0743d86 414 vy = -20;
akudohune 0:8215f0743d86 415 }
akudohune 0:8215f0743d86 416 }
akudohune 0:8215f0743d86 417 }else if((direction == 2) || (direction == 1)){
akudohune 0:8215f0743d86 418 comp_flag6 = 0;
akudohune 0:8215f0743d86 419 comp_flag10 = 0;
akudohune 0:8215f0743d86 420 if((RIGHT_SONIC > 700.0) && (!(RIGHT_SONIC == PING_ERROR))){
akudohune 0:8215f0743d86 421 vx = 30;
akudohune 0:8215f0743d86 422 }else{
akudohune 0:8215f0743d86 423 vx = 20;
akudohune 0:8215f0743d86 424 }
akudohune 0:8215f0743d86 425 if(BACK_SONIC > 140.0){
akudohune 0:8215f0743d86 426 vy = -10;
akudohune 0:8215f0743d86 427 }else if(BACK_SONIC < 110.0){
akudohune 0:8215f0743d86 428 vy = 5;
akudohune 0:8215f0743d86 429 }
akudohune 0:8215f0743d86 430 }else if((direction == 14) || (direction == 15)){
akudohune 0:8215f0743d86 431 comp_flag6 = 0;
akudohune 0:8215f0743d86 432 comp_flag10 = 0;
akudohune 0:8215f0743d86 433 if((LEFT_SONIC > 700.0) && (!(LEFT_SONIC == PING_ERROR))){
akudohune 0:8215f0743d86 434 vx = -30;
akudohune 0:8215f0743d86 435 }else{
akudohune 0:8215f0743d86 436 vx = -20;
akudohune 0:8215f0743d86 437 }
akudohune 0:8215f0743d86 438 if(BACK_SONIC > 140.0){
akudohune 0:8215f0743d86 439 vy = -10;
akudohune 0:8215f0743d86 440 }else if(BACK_SONIC < 110.0){
akudohune 0:8215f0743d86 441 vy = 5;
akudohune 0:8215f0743d86 442 }
akudohune 0:8215f0743d86 443 }else if(direction == 0){
akudohune 0:8215f0743d86 444 comp_flag6 = 0;
akudohune 0:8215f0743d86 445 comp_flag10 = 0;
akudohune 0:8215f0743d86 446 vx = 0;
akudohune 0:8215f0743d86 447 vy = 20;
akudohune 0:8215f0743d86 448 }else{
akudohune 0:8215f0743d86 449 comp_flag6 = 0;
akudohune 0:8215f0743d86 450 comp_flag10 = 0;
akudohune 0:8215f0743d86 451 vx = 0;
akudohune 0:8215f0743d86 452 vy = 0;
akudohune 0:8215f0743d86 453 }
akudohune 0:8215f0743d86 454 }else if(state == WARNING){
akudohune 0:8215f0743d86 455 comp_flag6 = 0;
akudohune 0:8215f0743d86 456 comp_flag10 = 0;
akudohune 0:8215f0743d86 457 if(direction == 0){
akudohune 0:8215f0743d86 458 vx = 0;
akudohune 0:8215f0743d86 459 }else if(direction == 1){
akudohune 0:8215f0743d86 460 vx = 15;
akudohune 0:8215f0743d86 461 }else if(direction == 2){
akudohune 0:8215f0743d86 462 vx = 20;
akudohune 0:8215f0743d86 463 }else if(direction == 14){
akudohune 0:8215f0743d86 464 vx = -20;
akudohune 0:8215f0743d86 465 }else if(direction == 15){
akudohune 0:8215f0743d86 466 vx = -15;
akudohune 0:8215f0743d86 467 }
akudohune 0:8215f0743d86 468
akudohune 0:8215f0743d86 469 if((LEFT_SONIC > 320) && (RIGHT_SONIC > 320)){
akudohune 0:8215f0743d86 470 if(BACK_SONIC > 150.0){
akudohune 0:8215f0743d86 471 vy = -5;
akudohune 0:8215f0743d86 472 }else if(BACK_SONIC < 110.0){
akudohune 0:8215f0743d86 473 vy = 5;
akudohune 0:8215f0743d86 474 }else{
akudohune 0:8215f0743d86 475 vy = 0;
akudohune 0:8215f0743d86 476 }
akudohune 0:8215f0743d86 477 }else{
akudohune 0:8215f0743d86 478 if(BACK_SONIC > 110.0){
akudohune 0:8215f0743d86 479 vy = -5;
akudohune 0:8215f0743d86 480 }else if(BACK_SONIC < 70.0){
akudohune 0:8215f0743d86 481 vy = 5;
akudohune 0:8215f0743d86 482 }else{
akudohune 0:8215f0743d86 483 vy = 0;
akudohune 0:8215f0743d86 484 }
akudohune 0:8215f0743d86 485 }
akudohune 0:8215f0743d86 486 }
akudohune 0:8215f0743d86 487 move(vx,vy,vs);
akudohune 0:8215f0743d86 488
akudohune 0:8215f0743d86 489 }
akudohune 0:8215f0743d86 490 }