yusuke takahashi / Mbed 2 deprecated ver3_1_0

driver

Dependencies:   HMC6352 PID mbed

Fork of ver3_1_0 by ryo seki

main.cpp@0:bde8ed56b164, 2013-06-19 (annotated)

Committer:
akudohune
Date:
Wed Jun 19 08:42:55 2013 +0000
Revision:
0:bde8ed56b164
Child:
1:3b61675573b1
final_master

Who changed what in which revision?

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