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