ryo seki / Mbed 2 deprecated ver2_0_5

final

main.cpp@0:fb6510639aa4, 2013-05-01 (annotated)

Committer:
akudohune
Date:
Wed May 01 08:47:59 2013 +0000
Revision:
0:fb6510639aa4
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akudohune 0:fb6510639aa4 1 #include <math.h>
akudohune 0:fb6510639aa4 2 #include <sstream>
akudohune 0:fb6510639aa4 3 #include "mbed.h"
akudohune 0:fb6510639aa4 4 #include "HMC6352.h"
akudohune 0:fb6510639aa4 5 #include "PID.h"
akudohune 0:fb6510639aa4 6 #include "main.h"
akudohune 0:fb6510639aa4 7
akudohune 0:fb6510639aa4 8
akudohune 0:fb6510639aa4 9
akudohune 0:fb6510639aa4 10 void PidUpdate()
akudohune 0:fb6510639aa4 11 {
akudohune 0:fb6510639aa4 12 static uint8_t Fflag = 0;
akudohune 0:fb6510639aa4 13
akudohune 0:fb6510639aa4 14 inputPID = (((int)(compass.sample() - ((standard + standTu) * 10.0) + 9000.0) % 3600) / 10.0);
akudohune 0:fb6510639aa4 15
akudohune 0:fb6510639aa4 16 //pc.printf("%f\n",timer1.read());
akudohune 0:fb6510639aa4 17 pid.setProcessValue(inputPID);
akudohune 0:fb6510639aa4 18 //timer1.reset();
akudohune 0:fb6510639aa4 19
akudohune 0:fb6510639aa4 20 compassPID = -(pid.compute());
akudohune 0:fb6510639aa4 21
akudohune 0:fb6510639aa4 22 if(!Fflag){
akudohune 0:fb6510639aa4 23 Fflag = 1;
akudohune 0:fb6510639aa4 24 compassPID = 0;
akudohune 0:fb6510639aa4 25 }
akudohune 0:fb6510639aa4 26 //pc.printf("%f\n",standard);
akudohune 0:fb6510639aa4 27 //pc.printf("%d\n",diff);
akudohune 0:fb6510639aa4 28 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
akudohune 0:fb6510639aa4 29 //pc.printf("%d\n",(int)compassPID);
akudohune 0:fb6510639aa4 30 //pc.printf("%d\t%d\n",Distance,direction);
akudohune 0:fb6510639aa4 31 //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
akudohune 0:fb6510639aa4 32 }
akudohune 0:fb6510639aa4 33
akudohune 0:fb6510639aa4 34 void move(int vxx, int vyy, int vss)
akudohune 0:fb6510639aa4 35 {
akudohune 0:fb6510639aa4 36 double motVal[MOT_NUM] = {0};
akudohune 0:fb6510639aa4 37
akudohune 0:fb6510639aa4 38 motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
akudohune 0:fb6510639aa4 39 motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2);
akudohune 0:fb6510639aa4 40 motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
akudohune 0:fb6510639aa4 41 motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4);
akudohune 0:fb6510639aa4 42
akudohune 0:fb6510639aa4 43 for(uint8_t i = 0 ; i < MOT_NUM ; i++){
akudohune 0:fb6510639aa4 44 if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
akudohune 0:fb6510639aa4 45 else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
akudohune 0:fb6510639aa4 46 speed[i] = (int)motVal[i];
akudohune 0:fb6510639aa4 47 }
akudohune 0:fb6510639aa4 48 }
akudohune 0:fb6510639aa4 49
akudohune 0:fb6510639aa4 50 /*********** Serial interrupt ***********/
akudohune 0:fb6510639aa4 51
akudohune 0:fb6510639aa4 52 void Tx_interrupt()
akudohune 0:fb6510639aa4 53 {
akudohune 0:fb6510639aa4 54 array(speed[0],speed[1],speed[2],speed[3]);
akudohune 0:fb6510639aa4 55 driver.printf("%s",StringFIN.c_str());
akudohune 0:fb6510639aa4 56 //pc.printf("%s",StringFIN.c_str());
akudohune 0:fb6510639aa4 57 }
akudohune 0:fb6510639aa4 58 /*
akudohune 0:fb6510639aa4 59 void Rx_interrupt()
akudohune 0:fb6510639aa4 60 {
akudohune 0:fb6510639aa4 61 if(driver.readable()){
akudohune 0:fb6510639aa4 62 //pc.printf("%d\n",driver.getc());
akudohune 0:fb6510639aa4 63 }
akudohune 0:fb6510639aa4 64 }*/
akudohune 0:fb6510639aa4 65
akudohune 0:fb6510639aa4 66
akudohune 0:fb6510639aa4 67 /*********** Serial interrupt **********/
akudohune 0:fb6510639aa4 68
akudohune 0:fb6510639aa4 69
akudohune 0:fb6510639aa4 70 void init()
akudohune 0:fb6510639aa4 71 {
akudohune 0:fb6510639aa4 72 int scanfSuccess;
akudohune 0:fb6510639aa4 73 int printfSuccess;
akudohune 0:fb6510639aa4 74 int closeSuccess;
akudohune 0:fb6510639aa4 75 int close2Success;
akudohune 0:fb6510639aa4 76 uint8_t MissFlag = 0;
akudohune 0:fb6510639aa4 77 FILE *fp;
akudohune 0:fb6510639aa4 78
akudohune 0:fb6510639aa4 79 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
akudohune 0:fb6510639aa4 80
akudohune 0:fb6510639aa4 81 StartButton.mode(PullUp);
akudohune 0:fb6510639aa4 82 CalibEnterButton.mode(PullUp);
akudohune 0:fb6510639aa4 83 CalibExitButton.mode(PullUp);
akudohune 0:fb6510639aa4 84 EEPROMButton.mode(PullUp);
akudohune 0:fb6510639aa4 85
akudohune 0:fb6510639aa4 86 driver.baud(BAUD_RATE);
akudohune 0:fb6510639aa4 87 device2.baud(BAUD_RATE2);
akudohune 0:fb6510639aa4 88 wait_ms(MOTDRIVER_WAIT);
akudohune 0:fb6510639aa4 89 driver.printf("1F0002F0003F0004F000\r\n");
akudohune 0:fb6510639aa4 90 device2.printf("START");
akudohune 0:fb6510639aa4 91
akudohune 0:fb6510639aa4 92 driver.attach(&Tx_interrupt, Serial::TxIrq);
akudohune 0:fb6510639aa4 93 //driver.attach(&Rx_interrupt, Serial::RxIrq);
akudohune 0:fb6510639aa4 94 device2.attach(&dev_rx,Serial::RxIrq);
akudohune 0:fb6510639aa4 95 device2.attach(&dev_tx,Serial::TxIrq);
akudohune 0:fb6510639aa4 96
akudohune 0:fb6510639aa4 97 pid.setInputLimits(MINIMUM,MAXIMUM);
akudohune 0:fb6510639aa4 98 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
akudohune 0:fb6510639aa4 99 pid.setBias(PID_BIAS);
akudohune 0:fb6510639aa4 100 pid.setMode(AUTO_MODE);
akudohune 0:fb6510639aa4 101 pid.setSetPoint(REFERENCE);
akudohune 0:fb6510639aa4 102
akudohune 0:fb6510639aa4 103 Survtimer.start();
akudohune 0:fb6510639aa4 104
akudohune 0:fb6510639aa4 105 mbedleds = 1;
akudohune 0:fb6510639aa4 106
akudohune 0:fb6510639aa4 107 while(StartButton){
akudohune 0:fb6510639aa4 108 MissFlag = 0;
akudohune 0:fb6510639aa4 109 if(!CalibEnterButton){
akudohune 0:fb6510639aa4 110 mbedleds = 2;
akudohune 0:fb6510639aa4 111 compass.setCalibrationMode(ENTER);
akudohune 0:fb6510639aa4 112 while(CalibExitButton);
akudohune 0:fb6510639aa4 113 compass.setCalibrationMode(EXIT);
akudohune 0:fb6510639aa4 114 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 115 mbedleds = 4;
akudohune 0:fb6510639aa4 116 }
akudohune 0:fb6510639aa4 117
akudohune 0:fb6510639aa4 118 if(!EEPROMButton){
akudohune 0:fb6510639aa4 119 Survtimer.reset();
akudohune 0:fb6510639aa4 120 fp = fopen("/local/out.txt", "r");
akudohune 0:fb6510639aa4 121 if(fp == NULL){
akudohune 0:fb6510639aa4 122 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 123 MissFlag = 1;
akudohune 0:fb6510639aa4 124 }else{
akudohune 0:fb6510639aa4 125 scanfSuccess = fscanf(fp, "%lf",&standard);
akudohune 0:fb6510639aa4 126 if(scanfSuccess == EOF){
akudohune 0:fb6510639aa4 127 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 128 MissFlag = 1;
akudohune 0:fb6510639aa4 129 }else{
akudohune 0:fb6510639aa4 130 closeSuccess = fclose(fp);
akudohune 0:fb6510639aa4 131 if(closeSuccess == EOF){
akudohune 0:fb6510639aa4 132 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 133 MissFlag = 1;
akudohune 0:fb6510639aa4 134 }else{
akudohune 0:fb6510639aa4 135 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 136 }
akudohune 0:fb6510639aa4 137 }
akudohune 0:fb6510639aa4 138 }
akudohune 0:fb6510639aa4 139 if((Survtimer.read() > (BUT_WAIT + 0.1)) || (MissFlag)){
akudohune 0:fb6510639aa4 140 for(uint8_t i = 0;i < 2;i++){
akudohune 0:fb6510639aa4 141 mbedleds = 15;
akudohune 0:fb6510639aa4 142 wait(0.1);
akudohune 0:fb6510639aa4 143 mbedleds = 0;
akudohune 0:fb6510639aa4 144 wait(0.1);
akudohune 0:fb6510639aa4 145 }
akudohune 0:fb6510639aa4 146 mbedleds = 15;
akudohune 0:fb6510639aa4 147 }else{
akudohune 0:fb6510639aa4 148 mbedleds = 8;
akudohune 0:fb6510639aa4 149 }
akudohune 0:fb6510639aa4 150 }
akudohune 0:fb6510639aa4 151
akudohune 0:fb6510639aa4 152 if(!CalibExitButton){
akudohune 0:fb6510639aa4 153 Survtimer.reset();
akudohune 0:fb6510639aa4 154
akudohune 0:fb6510639aa4 155 standard = compass.sample() / 10.0;
akudohune 0:fb6510639aa4 156
akudohune 0:fb6510639aa4 157 fp = fopen("/local/out.txt", "w");
akudohune 0:fb6510639aa4 158 if(fp == NULL){
akudohune 0:fb6510639aa4 159 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 160 MissFlag = 1;
akudohune 0:fb6510639aa4 161 }else{
akudohune 0:fb6510639aa4 162 printfSuccess = fprintf(fp, "%f",standard);
akudohune 0:fb6510639aa4 163 if(printfSuccess == EOF){
akudohune 0:fb6510639aa4 164 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 165 MissFlag = 1;
akudohune 0:fb6510639aa4 166 }else{
akudohune 0:fb6510639aa4 167 close2Success = fclose(fp);
akudohune 0:fb6510639aa4 168 if(close2Success == EOF){
akudohune 0:fb6510639aa4 169 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 170 MissFlag = 1;
akudohune 0:fb6510639aa4 171 }else{
akudohune 0:fb6510639aa4 172 wait(BUT_WAIT);
akudohune 0:fb6510639aa4 173 }
akudohune 0:fb6510639aa4 174 }
akudohune 0:fb6510639aa4 175 }
akudohune 0:fb6510639aa4 176 if((Survtimer.read() > (BUT_WAIT + 0.2)) || (MissFlag)){
akudohune 0:fb6510639aa4 177 for(uint8_t i = 0;i < 4;i++){
akudohune 0:fb6510639aa4 178 mbedleds = 15;
akudohune 0:fb6510639aa4 179 wait(0.1);
akudohune 0:fb6510639aa4 180 mbedleds = 0;
akudohune 0:fb6510639aa4 181 wait(0.1);
akudohune 0:fb6510639aa4 182 }
akudohune 0:fb6510639aa4 183 mbedleds = 15;
akudohune 0:fb6510639aa4 184 }else{
akudohune 0:fb6510639aa4 185 mbedleds = 10;
akudohune 0:fb6510639aa4 186 }
akudohune 0:fb6510639aa4 187 }
akudohune 0:fb6510639aa4 188 }
akudohune 0:fb6510639aa4 189
akudohune 0:fb6510639aa4 190 mbedleds = 0;
akudohune 0:fb6510639aa4 191
akudohune 0:fb6510639aa4 192 Survtimer.stop();
akudohune 0:fb6510639aa4 193
akudohune 0:fb6510639aa4 194 pidUpdata.attach(&PidUpdate, PID_CYCLE);
akudohune 0:fb6510639aa4 195
akudohune 0:fb6510639aa4 196 //timer1.start();
akudohune 0:fb6510639aa4 197 }
akudohune 0:fb6510639aa4 198
akudohune 0:fb6510639aa4 199 int main()
akudohune 0:fb6510639aa4 200 {
akudohune 0:fb6510639aa4 201 int vx=0,vy=0,vs=0;
akudohune 0:fb6510639aa4 202
akudohune 0:fb6510639aa4 203 init();
akudohune 0:fb6510639aa4 204
akudohune 0:fb6510639aa4 205 while(1){
akudohune 0:fb6510639aa4 206
akudohune 0:fb6510639aa4 207 hold_flag = 0;
akudohune 0:fb6510639aa4 208 vx = 0;
akudohune 0:fb6510639aa4 209 vy = 0;
akudohune 0:fb6510639aa4 210 vs = (int)compassPID;
akudohune 0:fb6510639aa4 211
akudohune 0:fb6510639aa4 212 //move(vx,vy,vs);
akudohune 0:fb6510639aa4 213
akudohune 0:fb6510639aa4 214 /******************************************************
akudohune 0:fb6510639aa4 215 *******************************************************
akudohune 0:fb6510639aa4 216 ********************Change state **********************
akudohune 0:fb6510639aa4 217 *******************************************************
akudohune 0:fb6510639aa4 218 ******************************************************/
akudohune 0:fb6510639aa4 219
akudohune 0:fb6510639aa4 220 if(state == HOLD){
akudohune 0:fb6510639aa4 221 if(FRONT_SONIC < 100)hold_flag = 1;
akudohune 0:fb6510639aa4 222
akudohune 0:fb6510639aa4 223 if(Distance > 140){ //審判のボール持ち上げ判定
akudohune 0:fb6510639aa4 224 state = HOME_WAIT;
akudohune 0:fb6510639aa4 225 }else if(!((direction == 0) || (direction == 15) || (direction == 1))){//ボールを見失った
akudohune 0:fb6510639aa4 226 state = DIFFENCE;
akudohune 0:fb6510639aa4 227 }else if(!(Distance <= 30)){//ボールを見失った
akudohune 0:fb6510639aa4 228 state = DIFFENCE;
akudohune 0:fb6510639aa4 229 }
akudohune 0:fb6510639aa4 230 }else{
akudohune 0:fb6510639aa4 231 standTu = 0;
akudohune 0:fb6510639aa4 232 if(state == DIFFENCE){
akudohune 0:fb6510639aa4 233 if((direction == 0) && (Distance <= 10) && (IR_found) && (!xbee)){
akudohune 0:fb6510639aa4 234 state = HOLD;
akudohune 0:fb6510639aa4 235 }else if((Distance < 180) && (IR_found) && (!xbee)){
akudohune 0:fb6510639aa4 236 state = DIFFENCE;
akudohune 0:fb6510639aa4 237 }else{
akudohune 0:fb6510639aa4 238 if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
akudohune 0:fb6510639aa4 239 if((IR_found) && (!xbee)){
akudohune 0:fb6510639aa4 240 state = DIFFENCE;
akudohune 0:fb6510639aa4 241 }
akudohune 0:fb6510639aa4 242 }else if((diff > 15) && (!xbee) && (IR_found)){
akudohune 0:fb6510639aa4 243 state = DIFFENCE;
akudohune 0:fb6510639aa4 244 }else{
akudohune 0:fb6510639aa4 245 state = HOME_WAIT;
akudohune 0:fb6510639aa4 246 }
akudohune 0:fb6510639aa4 247 }
akudohune 0:fb6510639aa4 248
akudohune 0:fb6510639aa4 249 }else{
akudohune 0:fb6510639aa4 250 if((direction == 0) && (Distance <= 10) && (IR_found) && (!xbee)){
akudohune 0:fb6510639aa4 251 state = HOLD;
akudohune 0:fb6510639aa4 252 }else if((Distance <= 140) && (IR_found) && (!xbee)){
akudohune 0:fb6510639aa4 253 state = DIFFENCE;
akudohune 0:fb6510639aa4 254 }else{
akudohune 0:fb6510639aa4 255 if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
akudohune 0:fb6510639aa4 256 if((IR_found) && (!xbee)){
akudohune 0:fb6510639aa4 257 state = DIFFENCE;
akudohune 0:fb6510639aa4 258 }
akudohune 0:fb6510639aa4 259 }else if((diff > 15) && (!xbee) && (IR_found)){
akudohune 0:fb6510639aa4 260 state = DIFFENCE;
akudohune 0:fb6510639aa4 261 }else{
akudohune 0:fb6510639aa4 262 state = HOME_WAIT;
akudohune 0:fb6510639aa4 263 }
akudohune 0:fb6510639aa4 264 }
akudohune 0:fb6510639aa4 265 }
akudohune 0:fb6510639aa4 266 }
akudohune 0:fb6510639aa4 267
akudohune 0:fb6510639aa4 268 /******************************************************
akudohune 0:fb6510639aa4 269 *******************************************************
akudohune 0:fb6510639aa4 270 ********************Change state **********************
akudohune 0:fb6510639aa4 271 *******************************************************
akudohune 0:fb6510639aa4 272 ******************************************************/
akudohune 0:fb6510639aa4 273
akudohune 0:fb6510639aa4 274 if(state == HOME_WAIT){
akudohune 0:fb6510639aa4 275 mbedleds = 1;
akudohune 0:fb6510639aa4 276 if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 850.0)){
akudohune 0:fb6510639aa4 277 if((LEFT_SONIC > 425.0) && (RIGHT_SONIC > 425.0)){
akudohune 0:fb6510639aa4 278 vx = 0;
akudohune 0:fb6510639aa4 279 }else if(RIGHT_SONIC < 425.0){
akudohune 0:fb6510639aa4 280 vx = (int)((RIGHT_SONIC - 450.0) * 0.02 - 10.0);
akudohune 0:fb6510639aa4 281 if(vx < -20)vx = -20;
akudohune 0:fb6510639aa4 282 }else if(LEFT_SONIC < 425.0){
akudohune 0:fb6510639aa4 283 vx = (int)((450.0 - LEFT_SONIC ) * 0.02 + 10.0);
akudohune 0:fb6510639aa4 284 if(vx > 20)vx = 20;
akudohune 0:fb6510639aa4 285 }
akudohune 0:fb6510639aa4 286
akudohune 0:fb6510639aa4 287 if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
akudohune 0:fb6510639aa4 288 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
akudohune 0:fb6510639aa4 289 vy = 0;
akudohune 0:fb6510639aa4 290 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 291 vy = 4;
akudohune 0:fb6510639aa4 292 }else{
akudohune 0:fb6510639aa4 293 vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 294 if(vy < -10)vy = -10;
akudohune 0:fb6510639aa4 295 }
akudohune 0:fb6510639aa4 296 }else{
akudohune 0:fb6510639aa4 297 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
akudohune 0:fb6510639aa4 298 vy = 0;
akudohune 0:fb6510639aa4 299 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 300 vy = 4;
akudohune 0:fb6510639aa4 301 }else{
akudohune 0:fb6510639aa4 302 vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 303 if(vy < -10)vy = -10;
akudohune 0:fb6510639aa4 304 }
akudohune 0:fb6510639aa4 305 }
akudohune 0:fb6510639aa4 306 }else if((RIGHT_SONIC + LEFT_SONIC) <= 850.0){
akudohune 0:fb6510639aa4 307 if(BACK_SONIC < 200.0){
akudohune 0:fb6510639aa4 308 if(RIGHT_SONIC > LEFT_SONIC){
akudohune 0:fb6510639aa4 309 vx = 10;
akudohune 0:fb6510639aa4 310 vy = 3;
akudohune 0:fb6510639aa4 311 }else{
akudohune 0:fb6510639aa4 312 vx = -10;
akudohune 0:fb6510639aa4 313 vy = 3;
akudohune 0:fb6510639aa4 314 }
akudohune 0:fb6510639aa4 315 }else{
akudohune 0:fb6510639aa4 316 vx = 0;
akudohune 0:fb6510639aa4 317 vy = -10;
akudohune 0:fb6510639aa4 318 }
akudohune 0:fb6510639aa4 319 }else{
akudohune 0:fb6510639aa4 320 if(BACK_SONIC > 500.0){
akudohune 0:fb6510639aa4 321 if(RIGHT_SONIC > LEFT_SONIC){
akudohune 0:fb6510639aa4 322 vx = 10;
akudohune 0:fb6510639aa4 323 vy = -5;
akudohune 0:fb6510639aa4 324 }else{
akudohune 0:fb6510639aa4 325 vx = -10;
akudohune 0:fb6510639aa4 326 vy = -5;
akudohune 0:fb6510639aa4 327 }
akudohune 0:fb6510639aa4 328 }
akudohune 0:fb6510639aa4 329 }
akudohune 0:fb6510639aa4 330 }else if(state == DIFFENCE){
akudohune 0:fb6510639aa4 331 mbedleds = 4;
akudohune 0:fb6510639aa4 332 if(direction == 6){
akudohune 0:fb6510639aa4 333 vx = 15;
akudohune 0:fb6510639aa4 334
akudohune 0:fb6510639aa4 335 if(LEFT_SONIC < 330.0){
akudohune 0:fb6510639aa4 336 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
akudohune 0:fb6510639aa4 337 vy = 0;
akudohune 0:fb6510639aa4 338 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 339 vy = 4;
akudohune 0:fb6510639aa4 340 }else{
akudohune 0:fb6510639aa4 341 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 342 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 343 }
akudohune 0:fb6510639aa4 344 }else if(RIGHT_SONIC < 330.0){
akudohune 0:fb6510639aa4 345 vx = 10;
akudohune 0:fb6510639aa4 346 vy = -5;
akudohune 0:fb6510639aa4 347 }else{
akudohune 0:fb6510639aa4 348 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
akudohune 0:fb6510639aa4 349 vy = 0;
akudohune 0:fb6510639aa4 350 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 351 vy = 4;
akudohune 0:fb6510639aa4 352 }else{
akudohune 0:fb6510639aa4 353 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 354 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 355 }
akudohune 0:fb6510639aa4 356 }
akudohune 0:fb6510639aa4 357
akudohune 0:fb6510639aa4 358 }else if(direction == 10){
akudohune 0:fb6510639aa4 359 vx = -15;
akudohune 0:fb6510639aa4 360
akudohune 0:fb6510639aa4 361 if(RIGHT_SONIC < 330.0){
akudohune 0:fb6510639aa4 362 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
akudohune 0:fb6510639aa4 363 vy = 0;
akudohune 0:fb6510639aa4 364 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 365 vy = 4;
akudohune 0:fb6510639aa4 366 }else{
akudohune 0:fb6510639aa4 367 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 368 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 369 }
akudohune 0:fb6510639aa4 370 }else if(LEFT_SONIC < 330.0){
akudohune 0:fb6510639aa4 371 vx = -10;
akudohune 0:fb6510639aa4 372 vy = -10;
akudohune 0:fb6510639aa4 373 }else{
akudohune 0:fb6510639aa4 374 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
akudohune 0:fb6510639aa4 375 vy = 0;
akudohune 0:fb6510639aa4 376 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 377 vy = 4;
akudohune 0:fb6510639aa4 378 }else{
akudohune 0:fb6510639aa4 379 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 380 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 381 }
akudohune 0:fb6510639aa4 382 }
akudohune 0:fb6510639aa4 383 }else if(direction == 4){
akudohune 0:fb6510639aa4 384
akudohune 0:fb6510639aa4 385 vx = 15;
akudohune 0:fb6510639aa4 386
akudohune 0:fb6510639aa4 387 if(LEFT_SONIC < 330.0){
akudohune 0:fb6510639aa4 388 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
akudohune 0:fb6510639aa4 389 vy = 0;
akudohune 0:fb6510639aa4 390 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 391 vy = 4;
akudohune 0:fb6510639aa4 392 }else{
akudohune 0:fb6510639aa4 393 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 394 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 395 }
akudohune 0:fb6510639aa4 396 }else if(RIGHT_SONIC < 330.0){
akudohune 0:fb6510639aa4 397 vx = 10;
akudohune 0:fb6510639aa4 398 vy = -10;
akudohune 0:fb6510639aa4 399 }else{
akudohune 0:fb6510639aa4 400 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
akudohune 0:fb6510639aa4 401 vy = 0;
akudohune 0:fb6510639aa4 402 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 403 vy = 4;
akudohune 0:fb6510639aa4 404 }else{
akudohune 0:fb6510639aa4 405 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 406 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 407 }
akudohune 0:fb6510639aa4 408 }
akudohune 0:fb6510639aa4 409
akudohune 0:fb6510639aa4 410
akudohune 0:fb6510639aa4 411 }else if(direction == 12){
akudohune 0:fb6510639aa4 412
akudohune 0:fb6510639aa4 413 vx = -15;
akudohune 0:fb6510639aa4 414
akudohune 0:fb6510639aa4 415 if(RIGHT_SONIC < 330.0){
akudohune 0:fb6510639aa4 416 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
akudohune 0:fb6510639aa4 417 vy = 0;
akudohune 0:fb6510639aa4 418 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 419 vy = 4;
akudohune 0:fb6510639aa4 420 }else{
akudohune 0:fb6510639aa4 421 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 422 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 423 }
akudohune 0:fb6510639aa4 424 }else if(LEFT_SONIC < 330.0){
akudohune 0:fb6510639aa4 425 vx = -10;
akudohune 0:fb6510639aa4 426 vy = -10;
akudohune 0:fb6510639aa4 427 }else{
akudohune 0:fb6510639aa4 428 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
akudohune 0:fb6510639aa4 429 vy = 0;
akudohune 0:fb6510639aa4 430 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 431 vy = 4;
akudohune 0:fb6510639aa4 432 }else{
akudohune 0:fb6510639aa4 433 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 434 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 435 }
akudohune 0:fb6510639aa4 436 }
akudohune 0:fb6510639aa4 437
akudohune 0:fb6510639aa4 438 }else if(direction == 8){
akudohune 0:fb6510639aa4 439
akudohune 0:fb6510639aa4 440 if(LEFT_SONIC > RIGHT_SONIC){
akudohune 0:fb6510639aa4 441 vx = -15;
akudohune 0:fb6510639aa4 442 }else{
akudohune 0:fb6510639aa4 443 vx = 15;
akudohune 0:fb6510639aa4 444 }
akudohune 0:fb6510639aa4 445 if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
akudohune 0:fb6510639aa4 446 if(BACK_SONIC < 45.0){
akudohune 0:fb6510639aa4 447 vy = -5;
akudohune 0:fb6510639aa4 448 }else{
akudohune 0:fb6510639aa4 449 vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 450 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 451 }
akudohune 0:fb6510639aa4 452 }else{
akudohune 0:fb6510639aa4 453 if(BACK_SONIC < 145.0){
akudohune 0:fb6510639aa4 454 vy = -5;
akudohune 0:fb6510639aa4 455 }else{
akudohune 0:fb6510639aa4 456 vy = (int)(0.015 * (130.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 457 if(vy < -15)vy = -15;
akudohune 0:fb6510639aa4 458 }
akudohune 0:fb6510639aa4 459 }
akudohune 0:fb6510639aa4 460
akudohune 0:fb6510639aa4 461 }else if(direction == 2){
akudohune 0:fb6510639aa4 462
akudohune 0:fb6510639aa4 463 if(BACK_SONIC < 150.0){
akudohune 0:fb6510639aa4 464 vx = 20;
akudohune 0:fb6510639aa4 465 vy = 0;
akudohune 0:fb6510639aa4 466 }else{
akudohune 0:fb6510639aa4 467 vx = 12;
akudohune 0:fb6510639aa4 468 vy = 0;
akudohune 0:fb6510639aa4 469 }
akudohune 0:fb6510639aa4 470 }else if(direction == 14){
akudohune 0:fb6510639aa4 471
akudohune 0:fb6510639aa4 472 if(BACK_SONIC < 150.0){
akudohune 0:fb6510639aa4 473 vx = -20;
akudohune 0:fb6510639aa4 474 vy = -4;
akudohune 0:fb6510639aa4 475 }else{
akudohune 0:fb6510639aa4 476 vx = -12;
akudohune 0:fb6510639aa4 477 vy = -2;
akudohune 0:fb6510639aa4 478 }
akudohune 0:fb6510639aa4 479
akudohune 0:fb6510639aa4 480 }else if(direction == 1){
akudohune 0:fb6510639aa4 481
akudohune 0:fb6510639aa4 482 if(BACK_SONIC < 150.0){
akudohune 0:fb6510639aa4 483 vx = 15;
akudohune 0:fb6510639aa4 484 vy = 0;
akudohune 0:fb6510639aa4 485 }else{
akudohune 0:fb6510639aa4 486 vx = 10;
akudohune 0:fb6510639aa4 487 vy = 0;
akudohune 0:fb6510639aa4 488 }
akudohune 0:fb6510639aa4 489
akudohune 0:fb6510639aa4 490 }else if(direction == 15){
akudohune 0:fb6510639aa4 491
akudohune 0:fb6510639aa4 492 if(BACK_SONIC < 150.0){
akudohune 0:fb6510639aa4 493 vx = -15;
akudohune 0:fb6510639aa4 494 vy = -3;
akudohune 0:fb6510639aa4 495 }else{
akudohune 0:fb6510639aa4 496 vx = -10;
akudohune 0:fb6510639aa4 497 vy = -1;
akudohune 0:fb6510639aa4 498 }
akudohune 0:fb6510639aa4 499 }else if(direction == 0){
akudohune 0:fb6510639aa4 500
akudohune 0:fb6510639aa4 501 vx = 0;
akudohune 0:fb6510639aa4 502 vy = 10;
akudohune 0:fb6510639aa4 503
akudohune 0:fb6510639aa4 504 }else{//error
akudohune 0:fb6510639aa4 505
akudohune 0:fb6510639aa4 506 vx = 0;
akudohune 0:fb6510639aa4 507 vy = 0;
akudohune 0:fb6510639aa4 508
akudohune 0:fb6510639aa4 509 }
akudohune 0:fb6510639aa4 510 }else if(state == WARNING){
akudohune 0:fb6510639aa4 511 mbedleds = 2;
akudohune 0:fb6510639aa4 512 if(direction == 0){
akudohune 0:fb6510639aa4 513
akudohune 0:fb6510639aa4 514 vx = 0;
akudohune 0:fb6510639aa4 515
akudohune 0:fb6510639aa4 516 }else if(direction == 1){
akudohune 0:fb6510639aa4 517
akudohune 0:fb6510639aa4 518 vx = 10;
akudohune 0:fb6510639aa4 519
akudohune 0:fb6510639aa4 520 }else if(direction == 2){
akudohune 0:fb6510639aa4 521
akudohune 0:fb6510639aa4 522 vx = 15;
akudohune 0:fb6510639aa4 523
akudohune 0:fb6510639aa4 524 }else if(direction == 14){
akudohune 0:fb6510639aa4 525
akudohune 0:fb6510639aa4 526 vx = -15;
akudohune 0:fb6510639aa4 527
akudohune 0:fb6510639aa4 528 }else if(direction == 15){
akudohune 0:fb6510639aa4 529
akudohune 0:fb6510639aa4 530 vx = -10;
akudohune 0:fb6510639aa4 531
akudohune 0:fb6510639aa4 532 }
akudohune 0:fb6510639aa4 533
akudohune 0:fb6510639aa4 534 if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
akudohune 0:fb6510639aa4 535 if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
akudohune 0:fb6510639aa4 536 vy = 0;
akudohune 0:fb6510639aa4 537 }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 538 vy = 4;
akudohune 0:fb6510639aa4 539 }else{
akudohune 0:fb6510639aa4 540 vy = (int)(0.01 * (30.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 541 if(vy < -10)vy = -10;
akudohune 0:fb6510639aa4 542 }
akudohune 0:fb6510639aa4 543 }else{
akudohune 0:fb6510639aa4 544 if((BACK_SONIC > 115.0) && (BACK_SONIC < 145.0)){
akudohune 0:fb6510639aa4 545 vy = 0;
akudohune 0:fb6510639aa4 546 }else if((BACK_SONIC <= 115.0) || (BACK_SONIC == PING_ERROR)){
akudohune 0:fb6510639aa4 547 vy = 4;
akudohune 0:fb6510639aa4 548 }else{
akudohune 0:fb6510639aa4 549 vy = (int)(0.01 * (130.0 - BACK_SONIC) - 4);
akudohune 0:fb6510639aa4 550 if(vy < -10)vy = -10;
akudohune 0:fb6510639aa4 551 }
akudohune 0:fb6510639aa4 552 }
akudohune 0:fb6510639aa4 553 }else if(state == HOLD){
akudohune 0:fb6510639aa4 554 mbedleds = 8;
akudohune 0:fb6510639aa4 555 vy = 15;
akudohune 0:fb6510639aa4 556 if(FRONT_SONIC < 100){
akudohune 0:fb6510639aa4 557 if(RIGHT_SONIC < LEFT_SONIC){
akudohune 0:fb6510639aa4 558 if(BACK_SONIC > 500){
akudohune 0:fb6510639aa4 559 vy = 0;
akudohune 0:fb6510639aa4 560 vs = 3;
akudohune 0:fb6510639aa4 561 }
akudohune 0:fb6510639aa4 562 }else{
akudohune 0:fb6510639aa4 563 if(BACK_SONIC > 500){
akudohune 0:fb6510639aa4 564 vy = 0;
akudohune 0:fb6510639aa4 565 vs = -3;
akudohune 0:fb6510639aa4 566 }
akudohune 0:fb6510639aa4 567 }
akudohune 0:fb6510639aa4 568 }else{
akudohune 0:fb6510639aa4 569 if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 800.0)){
akudohune 0:fb6510639aa4 570 standTu = (RIGHT_SONIC - LEFT_SONIC) / 30.0;
akudohune 0:fb6510639aa4 571 }
akudohune 0:fb6510639aa4 572 }
akudohune 0:fb6510639aa4 573 }
akudohune 0:fb6510639aa4 574 //move(vx,vy,vs);
akudohune 0:fb6510639aa4 575 }
akudohune 0:fb6510639aa4 576 }