ryo seki / Mbed 2 deprecated ver2_0_6

hello

main.cpp@0:79ccc03117ea, 2013-05-04 (annotated)

Committer:
akudohune
Date:
Sat May 04 23:56:15 2013 +0000
Revision:
0:79ccc03117ea
cuppppp

Who changed what in which revision?

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