final cup

Dependencies:   HMC6352 PID mbed

Committer:
akudohune
Date:
Mon Apr 29 06:39:53 2013 +0000
Revision:
0:2eebd0301c66
final

Who changed what in which revision?

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