aa

Dependencies:   HMC6352 PID mbed

Committer:
akudohune
Date:
Mon Jun 17 00:12:40 2013 +0000
Revision:
0:e9b97faa3e37
zaaa

Who changed what in which revision?

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