driver
Dependencies: HMC6352 PID mbed
Fork of ver3_1_0 by
main.cpp@1:3b61675573b1, 2014-01-24 (annotated)
- Committer:
- yusuke_robocup
- Date:
- Fri Jan 24 06:27:03 2014 +0000
- Revision:
- 1:3b61675573b1
- Parent:
- 0:bde8ed56b164
driver
Who changed what in which revision?
User | Revision | Line number | New 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 | |
yusuke_robocup | 1:3b61675573b1 | 9 | void whiteliner() |
yusuke_robocup | 1:3b61675573b1 | 10 | { |
yusuke_robocup | 1:3b61675573b1 | 11 | static int tmp[5] = {0}; |
yusuke_robocup | 1:3b61675573b1 | 12 | static int sum = 0; |
yusuke_robocup | 1:3b61675573b1 | 13 | |
yusuke_robocup | 1:3b61675573b1 | 14 | sum -= tmp[4]; |
yusuke_robocup | 1:3b61675573b1 | 15 | sum += whiteout_flag; |
yusuke_robocup | 1:3b61675573b1 | 16 | tmp[4] = tmp[3]; |
yusuke_robocup | 1:3b61675573b1 | 17 | tmp[3] = tmp[2]; |
yusuke_robocup | 1:3b61675573b1 | 18 | tmp[2] = tmp[1]; |
yusuke_robocup | 1:3b61675573b1 | 19 | tmp[1] = tmp[0]; |
yusuke_robocup | 1:3b61675573b1 | 20 | tmp[0] = whiteout_flag; |
yusuke_robocup | 1:3b61675573b1 | 21 | |
yusuke_robocup | 1:3b61675573b1 | 22 | //return sum/5; |
yusuke_robocup | 1:3b61675573b1 | 23 | } |
yusuke_robocup | 1:3b61675573b1 | 24 | |
yusuke_robocup | 1:3b61675573b1 | 25 | int hansya_x(int x) |
yusuke_robocup | 1:3b61675573b1 | 26 | { |
yusuke_robocup | 1:3b61675573b1 | 27 | static int tmp[5] = {0}; |
yusuke_robocup | 1:3b61675573b1 | 28 | static int sum = 0; |
yusuke_robocup | 1:3b61675573b1 | 29 | |
yusuke_robocup | 1:3b61675573b1 | 30 | sum -= tmp[4]; |
yusuke_robocup | 1:3b61675573b1 | 31 | sum += x; |
yusuke_robocup | 1:3b61675573b1 | 32 | tmp[4] = tmp[3]; |
yusuke_robocup | 1:3b61675573b1 | 33 | tmp[3] = tmp[2]; |
yusuke_robocup | 1:3b61675573b1 | 34 | tmp[2] = tmp[1]; |
yusuke_robocup | 1:3b61675573b1 | 35 | tmp[1] = tmp[0]; |
yusuke_robocup | 1:3b61675573b1 | 36 | tmp[0] = x; |
yusuke_robocup | 1:3b61675573b1 | 37 | |
yusuke_robocup | 1:3b61675573b1 | 38 | return sum/5; |
yusuke_robocup | 1:3b61675573b1 | 39 | } |
yusuke_robocup | 1:3b61675573b1 | 40 | |
yusuke_robocup | 1:3b61675573b1 | 41 | |
yusuke_robocup | 1:3b61675573b1 | 42 | int hansya_y(int y) |
yusuke_robocup | 1:3b61675573b1 | 43 | { |
yusuke_robocup | 1:3b61675573b1 | 44 | static int tmp[5] = {0}; |
yusuke_robocup | 1:3b61675573b1 | 45 | static int sum = 0; |
yusuke_robocup | 1:3b61675573b1 | 46 | |
yusuke_robocup | 1:3b61675573b1 | 47 | sum -= tmp[4]; |
yusuke_robocup | 1:3b61675573b1 | 48 | sum += y; |
yusuke_robocup | 1:3b61675573b1 | 49 | tmp[4] = tmp[3]; |
yusuke_robocup | 1:3b61675573b1 | 50 | tmp[3] = tmp[2]; |
yusuke_robocup | 1:3b61675573b1 | 51 | tmp[2] = tmp[1]; |
yusuke_robocup | 1:3b61675573b1 | 52 | tmp[1] = tmp[0]; |
yusuke_robocup | 1:3b61675573b1 | 53 | tmp[0] = y; |
yusuke_robocup | 1:3b61675573b1 | 54 | |
yusuke_robocup | 1:3b61675573b1 | 55 | return sum/5; |
yusuke_robocup | 1:3b61675573b1 | 56 | } |
yusuke_robocup | 1:3b61675573b1 | 57 | |
akudohune | 0:bde8ed56b164 | 58 | |
akudohune | 0:bde8ed56b164 | 59 | void PidUpdate() |
akudohune | 0:bde8ed56b164 | 60 | { |
akudohune | 0:bde8ed56b164 | 61 | static uint8_t Fflag = 0; |
akudohune | 0:bde8ed56b164 | 62 | |
akudohune | 0:bde8ed56b164 | 63 | pid.setSetPoint(((int)((REFERENCE + standTu) + 360) % 360) / 1.0); |
yusuke_robocup | 1:3b61675573b1 | 64 | inputPID = (((int)(compass.sample() - ((standard) * 10.0) + 5400.0) % 3600) / 10.0); |
akudohune | 0:bde8ed56b164 | 65 | |
akudohune | 0:bde8ed56b164 | 66 | //pc.printf("%f\n",timer1.read()); |
akudohune | 0:bde8ed56b164 | 67 | pid.setProcessValue(inputPID); |
akudohune | 0:bde8ed56b164 | 68 | //timer1.reset(); |
akudohune | 0:bde8ed56b164 | 69 | |
akudohune | 0:bde8ed56b164 | 70 | compassPID = -(pid.compute()); |
akudohune | 0:bde8ed56b164 | 71 | |
akudohune | 0:bde8ed56b164 | 72 | if(!Fflag){ |
akudohune | 0:bde8ed56b164 | 73 | Fflag = 1; |
akudohune | 0:bde8ed56b164 | 74 | compassPID = 0; |
akudohune | 0:bde8ed56b164 | 75 | } |
akudohune | 0:bde8ed56b164 | 76 | //pc.printf("%d\t%d\t%d\n",speed[0],speed[1],speed[2]); |
akudohune | 0:bde8ed56b164 | 77 | //pc.printf("standard = \t\t%f\n",standard); |
akudohune | 0:bde8ed56b164 | 78 | //pc.printf("%d\n",diff); |
akudohune | 0:bde8ed56b164 | 79 | //pc.printf("compass.sample = \t%f\n",compass.sample() / 1.0); |
akudohune | 0:bde8ed56b164 | 80 | //pc.printf("compassPID = \t\t%d\n",(int)compassPID); |
akudohune | 0:bde8ed56b164 | 81 | //pc.printf("inputPID = \t\t%f\n\n",inputPID); |
akudohune | 0:bde8ed56b164 | 82 | |
akudohune | 0:bde8ed56b164 | 83 | //pc.printf("%d\t%d\n",Distance,direction); |
akudohune | 0:bde8ed56b164 | 84 | //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]); |
akudohune | 0:bde8ed56b164 | 85 | } |
akudohune | 0:bde8ed56b164 | 86 | |
akudohune | 0:bde8ed56b164 | 87 | void IrDistanceUpdate() |
akudohune | 0:bde8ed56b164 | 88 | { |
akudohune | 0:bde8ed56b164 | 89 | for(uint8_t i = 0;i < 6;i++) |
akudohune | 0:bde8ed56b164 | 90 | { |
akudohune | 0:bde8ed56b164 | 91 | AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */ |
akudohune | 0:bde8ed56b164 | 92 | irDistance[i] = adcIn.read_u16() >> 4; |
akudohune | 0:bde8ed56b164 | 93 | //pc.printf("%d\n",irDistance[0]); |
akudohune | 0:bde8ed56b164 | 94 | } |
akudohune | 0:bde8ed56b164 | 95 | } |
akudohune | 0:bde8ed56b164 | 96 | |
yusuke_robocup | 1:3b61675573b1 | 97 | /* |
akudohune | 0:bde8ed56b164 | 98 | void move(int vxx, int vyy, int vss) |
akudohune | 0:bde8ed56b164 | 99 | { |
akudohune | 0:bde8ed56b164 | 100 | double motVal[MOT_NUM] = {0}; |
akudohune | 0:bde8ed56b164 | 101 | |
akudohune | 0:bde8ed56b164 | 102 | motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1); |
akudohune | 0:bde8ed56b164 | 103 | motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2); |
akudohune | 0:bde8ed56b164 | 104 | motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3); |
akudohune | 0:bde8ed56b164 | 105 | motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4); |
akudohune | 0:bde8ed56b164 | 106 | |
akudohune | 0:bde8ed56b164 | 107 | for(uint8_t i = 0 ; i < MOT_NUM ; i++){ |
akudohune | 0:bde8ed56b164 | 108 | if(motVal[i] > MAX_POW)motVal[i] = MAX_POW; |
akudohune | 0:bde8ed56b164 | 109 | else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW; |
akudohune | 0:bde8ed56b164 | 110 | speed[i] = (int)motVal[i]; |
akudohune | 0:bde8ed56b164 | 111 | } |
yusuke_robocup | 1:3b61675573b1 | 112 | }*/ |
yusuke_robocup | 1:3b61675573b1 | 113 | |
yusuke_robocup | 1:3b61675573b1 | 114 | void move(int vxx, int vyy, int vss) |
yusuke_robocup | 1:3b61675573b1 | 115 | { |
yusuke_robocup | 1:3b61675573b1 | 116 | double motVal[MOT_NUM] = {0}; |
yusuke_robocup | 1:3b61675573b1 | 117 | |
yusuke_robocup | 1:3b61675573b1 | 118 | int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360; |
yusuke_robocup | 1:3b61675573b1 | 119 | |
yusuke_robocup | 1:3b61675573b1 | 120 | double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1; |
yusuke_robocup | 1:3b61675573b1 | 121 | |
yusuke_robocup | 1:3b61675573b1 | 122 | if(angle == 180){ |
yusuke_robocup | 1:3b61675573b1 | 123 | hosei1 = 1.3; |
yusuke_robocup | 1:3b61675573b1 | 124 | } |
yusuke_robocup | 1:3b61675573b1 | 125 | |
yusuke_robocup | 1:3b61675573b1 | 126 | motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1; |
yusuke_robocup | 1:3b61675573b1 | 127 | motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2)*hosei2; |
yusuke_robocup | 1:3b61675573b1 | 128 | motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3; |
yusuke_robocup | 1:3b61675573b1 | 129 | motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4)*hosei4; |
yusuke_robocup | 1:3b61675573b1 | 130 | |
yusuke_robocup | 1:3b61675573b1 | 131 | for(uint8_t i = 0 ; i < MOT_NUM ; i++){ |
yusuke_robocup | 1:3b61675573b1 | 132 | if(motVal[i] > MAX_POW)motVal[i] = MAX_POW; |
yusuke_robocup | 1:3b61675573b1 | 133 | else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW; |
yusuke_robocup | 1:3b61675573b1 | 134 | speed[i] = motVal[i]; |
yusuke_robocup | 1:3b61675573b1 | 135 | } |
yusuke_robocup | 1:3b61675573b1 | 136 | |
yusuke_robocup | 1:3b61675573b1 | 137 | //pc.printf("speed1 = %d\n",speed[0]); |
yusuke_robocup | 1:3b61675573b1 | 138 | //pc.printf("speed2 = %d\n",speed[1]); |
yusuke_robocup | 1:3b61675573b1 | 139 | //pc.printf("speed3 = %d\n",speed[2]); |
yusuke_robocup | 1:3b61675573b1 | 140 | //pc.printf("speed4 = %d\n\n",speed[3]); |
yusuke_robocup | 1:3b61675573b1 | 141 | |
yusuke_robocup | 1:3b61675573b1 | 142 | ////pc.printf("%s",StringFIN.c_str()); |
akudohune | 0:bde8ed56b164 | 143 | } |
akudohune | 0:bde8ed56b164 | 144 | |
akudohune | 0:bde8ed56b164 | 145 | int vector(double Amp,double degree,int xy) |
akudohune | 0:bde8ed56b164 | 146 | { |
akudohune | 0:bde8ed56b164 | 147 | double result; |
akudohune | 0:bde8ed56b164 | 148 | |
akudohune | 0:bde8ed56b164 | 149 | if(xy == X){ |
akudohune | 0:bde8ed56b164 | 150 | result = Amp * cos(degree * PI / 180.0); |
akudohune | 0:bde8ed56b164 | 151 | }else if(xy == Y){ |
akudohune | 0:bde8ed56b164 | 152 | result = Amp * sin(degree * PI / 180.0) * (2.0 / sqrt(3.0)); |
akudohune | 0:bde8ed56b164 | 153 | } |
akudohune | 0:bde8ed56b164 | 154 | |
akudohune | 0:bde8ed56b164 | 155 | return (int)result; |
akudohune | 0:bde8ed56b164 | 156 | } |
akudohune | 0:bde8ed56b164 | 157 | |
akudohune | 0:bde8ed56b164 | 158 | /*********** Serial interrupt ***********/ |
akudohune | 0:bde8ed56b164 | 159 | |
akudohune | 0:bde8ed56b164 | 160 | void Tx_interrupt() |
akudohune | 0:bde8ed56b164 | 161 | { |
akudohune | 0:bde8ed56b164 | 162 | array(speed[0],speed[1],speed[2],speed[3]); |
akudohune | 0:bde8ed56b164 | 163 | driver.printf("%s",StringFIN.c_str()); |
akudohune | 0:bde8ed56b164 | 164 | //pc.printf("%s",StringFIN.c_str()); |
akudohune | 0:bde8ed56b164 | 165 | } |
akudohune | 0:bde8ed56b164 | 166 | /* |
akudohune | 0:bde8ed56b164 | 167 | void Rx_interrupt() |
akudohune | 0:bde8ed56b164 | 168 | { |
akudohune | 0:bde8ed56b164 | 169 | if(driver.readable()){ |
akudohune | 0:bde8ed56b164 | 170 | //pc.printf("%d\n",driver.getc()); |
akudohune | 0:bde8ed56b164 | 171 | } |
akudohune | 0:bde8ed56b164 | 172 | }*/ |
akudohune | 0:bde8ed56b164 | 173 | |
akudohune | 0:bde8ed56b164 | 174 | |
akudohune | 0:bde8ed56b164 | 175 | /*********** Serial interrupt **********/ |
akudohune | 0:bde8ed56b164 | 176 | |
akudohune | 0:bde8ed56b164 | 177 | |
akudohune | 0:bde8ed56b164 | 178 | void init() |
akudohune | 0:bde8ed56b164 | 179 | { |
akudohune | 0:bde8ed56b164 | 180 | int scanfSuccess; |
akudohune | 0:bde8ed56b164 | 181 | int printfSuccess; |
akudohune | 0:bde8ed56b164 | 182 | int closeSuccess; |
akudohune | 0:bde8ed56b164 | 183 | int close2Success; |
akudohune | 0:bde8ed56b164 | 184 | uint8_t MissFlag = 0; |
akudohune | 0:bde8ed56b164 | 185 | FILE *fp; |
akudohune | 0:bde8ed56b164 | 186 | |
akudohune | 0:bde8ed56b164 | 187 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
akudohune | 0:bde8ed56b164 | 188 | |
akudohune | 0:bde8ed56b164 | 189 | StartButton.mode(PullUp); |
akudohune | 0:bde8ed56b164 | 190 | CalibEnterButton.mode(PullUp); |
akudohune | 0:bde8ed56b164 | 191 | CalibExitButton.mode(PullUp); |
akudohune | 0:bde8ed56b164 | 192 | EEPROMButton.mode(PullUp); |
akudohune | 0:bde8ed56b164 | 193 | |
akudohune | 0:bde8ed56b164 | 194 | driver.baud(BAUD_RATE); |
akudohune | 0:bde8ed56b164 | 195 | device2.baud(BAUD_RATE2); |
akudohune | 0:bde8ed56b164 | 196 | wait_ms(MOTDRIVER_WAIT); |
akudohune | 0:bde8ed56b164 | 197 | driver.printf("1F0002F0003F0004F000\r\n"); |
akudohune | 0:bde8ed56b164 | 198 | device2.printf("START"); |
akudohune | 0:bde8ed56b164 | 199 | |
akudohune | 0:bde8ed56b164 | 200 | driver.attach(&Tx_interrupt, Serial::TxIrq); |
akudohune | 0:bde8ed56b164 | 201 | //driver.attach(&Rx_interrupt, Serial::RxIrq); |
akudohune | 0:bde8ed56b164 | 202 | device2.attach(&dev_rx,Serial::RxIrq); |
akudohune | 0:bde8ed56b164 | 203 | device2.attach(&dev_tx,Serial::TxIrq); |
yusuke_robocup | 1:3b61675573b1 | 204 | underIR.attach(&whiteliner, 0.05); |
akudohune | 0:bde8ed56b164 | 205 | |
akudohune | 0:bde8ed56b164 | 206 | pid.setInputLimits(MINIMUM,MAXIMUM); |
akudohune | 0:bde8ed56b164 | 207 | pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); |
akudohune | 0:bde8ed56b164 | 208 | pid.setBias(PID_BIAS); |
akudohune | 0:bde8ed56b164 | 209 | pid.setMode(AUTO_MODE); |
akudohune | 0:bde8ed56b164 | 210 | pid.setSetPoint(REFERENCE); |
akudohune | 0:bde8ed56b164 | 211 | |
akudohune | 0:bde8ed56b164 | 212 | irDistanceUpdata.attach(&IrDistanceUpdate, 0.1); |
akudohune | 0:bde8ed56b164 | 213 | |
akudohune | 0:bde8ed56b164 | 214 | Survtimer.start(); |
akudohune | 0:bde8ed56b164 | 215 | |
akudohune | 0:bde8ed56b164 | 216 | mbedleds = 1; |
akudohune | 0:bde8ed56b164 | 217 | |
akudohune | 0:bde8ed56b164 | 218 | while(StartButton){ |
akudohune | 0:bde8ed56b164 | 219 | MissFlag = 0; |
akudohune | 0:bde8ed56b164 | 220 | if(!CalibEnterButton){ |
akudohune | 0:bde8ed56b164 | 221 | mbedleds = 2; |
akudohune | 0:bde8ed56b164 | 222 | compass.setCalibrationMode(ENTER); |
akudohune | 0:bde8ed56b164 | 223 | while(CalibExitButton); |
akudohune | 0:bde8ed56b164 | 224 | compass.setCalibrationMode(EXIT); |
akudohune | 0:bde8ed56b164 | 225 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 226 | mbedleds = 4; |
akudohune | 0:bde8ed56b164 | 227 | } |
akudohune | 0:bde8ed56b164 | 228 | |
akudohune | 0:bde8ed56b164 | 229 | if(!EEPROMButton){ |
akudohune | 0:bde8ed56b164 | 230 | Survtimer.reset(); |
akudohune | 0:bde8ed56b164 | 231 | fp = fopen("/local/out.txt", "r"); |
akudohune | 0:bde8ed56b164 | 232 | if(fp == NULL){ |
akudohune | 0:bde8ed56b164 | 233 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 234 | MissFlag = 1; |
akudohune | 0:bde8ed56b164 | 235 | }else{ |
akudohune | 0:bde8ed56b164 | 236 | scanfSuccess = fscanf(fp, "%lf",&standard); |
akudohune | 0:bde8ed56b164 | 237 | if(scanfSuccess == EOF){ |
akudohune | 0:bde8ed56b164 | 238 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 239 | MissFlag = 1; |
akudohune | 0:bde8ed56b164 | 240 | }else{ |
akudohune | 0:bde8ed56b164 | 241 | closeSuccess = fclose(fp); |
akudohune | 0:bde8ed56b164 | 242 | if(closeSuccess == EOF){ |
akudohune | 0:bde8ed56b164 | 243 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 244 | MissFlag = 1; |
akudohune | 0:bde8ed56b164 | 245 | }else{ |
akudohune | 0:bde8ed56b164 | 246 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 247 | } |
akudohune | 0:bde8ed56b164 | 248 | } |
akudohune | 0:bde8ed56b164 | 249 | } |
akudohune | 0:bde8ed56b164 | 250 | if((Survtimer.read() > (BUT_WAIT + 0.1)) || (MissFlag)){ |
akudohune | 0:bde8ed56b164 | 251 | for(uint8_t i = 0;i < 2;i++){ |
akudohune | 0:bde8ed56b164 | 252 | mbedleds = 15; |
akudohune | 0:bde8ed56b164 | 253 | wait(0.1); |
akudohune | 0:bde8ed56b164 | 254 | mbedleds = 0; |
akudohune | 0:bde8ed56b164 | 255 | wait(0.1); |
akudohune | 0:bde8ed56b164 | 256 | } |
akudohune | 0:bde8ed56b164 | 257 | mbedleds = 15; |
akudohune | 0:bde8ed56b164 | 258 | }else{ |
akudohune | 0:bde8ed56b164 | 259 | mbedleds = 8; |
akudohune | 0:bde8ed56b164 | 260 | } |
akudohune | 0:bde8ed56b164 | 261 | } |
akudohune | 0:bde8ed56b164 | 262 | |
akudohune | 0:bde8ed56b164 | 263 | if(!CalibExitButton){ |
akudohune | 0:bde8ed56b164 | 264 | Survtimer.reset(); |
akudohune | 0:bde8ed56b164 | 265 | |
akudohune | 0:bde8ed56b164 | 266 | standard = compass.sample() / 10.0; |
akudohune | 0:bde8ed56b164 | 267 | |
akudohune | 0:bde8ed56b164 | 268 | fp = fopen("/local/out.txt", "w"); |
akudohune | 0:bde8ed56b164 | 269 | if(fp == NULL){ |
akudohune | 0:bde8ed56b164 | 270 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 271 | MissFlag = 1; |
akudohune | 0:bde8ed56b164 | 272 | }else{ |
akudohune | 0:bde8ed56b164 | 273 | printfSuccess = fprintf(fp, "%f",standard); |
akudohune | 0:bde8ed56b164 | 274 | if(printfSuccess == EOF){ |
akudohune | 0:bde8ed56b164 | 275 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 276 | MissFlag = 1; |
akudohune | 0:bde8ed56b164 | 277 | }else{ |
akudohune | 0:bde8ed56b164 | 278 | close2Success = fclose(fp); |
akudohune | 0:bde8ed56b164 | 279 | if(close2Success == EOF){ |
akudohune | 0:bde8ed56b164 | 280 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 281 | MissFlag = 1; |
akudohune | 0:bde8ed56b164 | 282 | }else{ |
akudohune | 0:bde8ed56b164 | 283 | wait(BUT_WAIT); |
akudohune | 0:bde8ed56b164 | 284 | } |
akudohune | 0:bde8ed56b164 | 285 | } |
akudohune | 0:bde8ed56b164 | 286 | } |
akudohune | 0:bde8ed56b164 | 287 | if((Survtimer.read() > (BUT_WAIT + 0.2)) || (MissFlag)){ |
akudohune | 0:bde8ed56b164 | 288 | for(uint8_t i = 0;i < 4;i++){ |
akudohune | 0:bde8ed56b164 | 289 | mbedleds = 15; |
akudohune | 0:bde8ed56b164 | 290 | wait(0.1); |
akudohune | 0:bde8ed56b164 | 291 | mbedleds = 0; |
akudohune | 0:bde8ed56b164 | 292 | wait(0.1); |
akudohune | 0:bde8ed56b164 | 293 | } |
akudohune | 0:bde8ed56b164 | 294 | mbedleds = 15; |
akudohune | 0:bde8ed56b164 | 295 | }else{ |
akudohune | 0:bde8ed56b164 | 296 | mbedleds = 10; |
akudohune | 0:bde8ed56b164 | 297 | } |
akudohune | 0:bde8ed56b164 | 298 | } |
akudohune | 0:bde8ed56b164 | 299 | } |
akudohune | 0:bde8ed56b164 | 300 | |
akudohune | 0:bde8ed56b164 | 301 | mbedleds = 0; |
akudohune | 0:bde8ed56b164 | 302 | |
akudohune | 0:bde8ed56b164 | 303 | Survtimer.stop(); |
akudohune | 0:bde8ed56b164 | 304 | |
akudohune | 0:bde8ed56b164 | 305 | for(uint8_t i = 0;i < 6;i++) |
akudohune | 0:bde8ed56b164 | 306 | { |
akudohune | 0:bde8ed56b164 | 307 | stand[i] = irDistance[i]; |
akudohune | 0:bde8ed56b164 | 308 | } |
akudohune | 0:bde8ed56b164 | 309 | irDistanceUpdata.detach(); |
akudohune | 0:bde8ed56b164 | 310 | |
akudohune | 0:bde8ed56b164 | 311 | pidUpdata.attach(&PidUpdate, PID_CYCLE); |
akudohune | 0:bde8ed56b164 | 312 | //irDistanceUpdata.attach(&IrDistanceUpdate, 0.1); |
akudohune | 0:bde8ed56b164 | 313 | //timer1.start(); |
akudohune | 0:bde8ed56b164 | 314 | } |
akudohune | 0:bde8ed56b164 | 315 | |
yusuke_robocup | 1:3b61675573b1 | 316 | |
akudohune | 0:bde8ed56b164 | 317 | int main() |
akudohune | 0:bde8ed56b164 | 318 | { |
akudohune | 0:bde8ed56b164 | 319 | int vx=0,vy=0,vs=0; |
yusuke_robocup | 1:3b61675573b1 | 320 | int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0; |
yusuke_robocup | 1:3b61675573b1 | 321 | int state_off = NONE; |
yusuke_robocup | 1:3b61675573b1 | 322 | int direction_av = 0; |
yusuke_robocup | 1:3b61675573b1 | 323 | int direction_past = 0; |
yusuke_robocup | 1:3b61675573b1 | 324 | int past_state_off = 0; |
yusuke_robocup | 1:3b61675573b1 | 325 | int accelera_Distance = 0; |
akudohune | 0:bde8ed56b164 | 326 | uint8_t whiteFlag = 0; |
yusuke_robocup | 1:3b61675573b1 | 327 | int save_vx = 0,save_vy = 0; |
yusuke_robocup | 1:3b61675573b1 | 328 | |
yusuke_robocup | 1:3b61675573b1 | 329 | int movein = 0; |
akudohune | 0:bde8ed56b164 | 330 | |
akudohune | 0:bde8ed56b164 | 331 | init(); |
akudohune | 0:bde8ed56b164 | 332 | |
yusuke_robocup | 1:3b61675573b1 | 333 | while(1) { |
yusuke_robocup | 1:3b61675573b1 | 334 | |
yusuke_robocup | 1:3b61675573b1 | 335 | vx=0; |
yusuke_robocup | 1:3b61675573b1 | 336 | vy=0; |
akudohune | 0:bde8ed56b164 | 337 | |
yusuke_robocup | 1:3b61675573b1 | 338 | //tuning = 0; |
yusuke_robocup | 1:3b61675573b1 | 339 | |
yusuke_robocup | 1:3b61675573b1 | 340 | x_dista = 0; |
yusuke_robocup | 1:3b61675573b1 | 341 | y_dista = 0; |
yusuke_robocup | 1:3b61675573b1 | 342 | x_turn = 0; |
yusuke_robocup | 1:3b61675573b1 | 343 | y_turn = 0; |
yusuke_robocup | 1:3b61675573b1 | 344 | //turn_flag = 0; |
akudohune | 0:bde8ed56b164 | 345 | |
yusuke_robocup | 1:3b61675573b1 | 346 | vs = compassPID; |
yusuke_robocup | 1:3b61675573b1 | 347 | |
yusuke_robocup | 1:3b61675573b1 | 348 | //direction_av = moving_ave_5point(direction); |
yusuke_robocup | 1:3b61675573b1 | 349 | |
yusuke_robocup | 1:3b61675573b1 | 350 | |
yusuke_robocup | 1:3b61675573b1 | 351 | |
yusuke_robocup | 1:3b61675573b1 | 352 | for(uint8_t i = 0;i < 6;i++) |
akudohune | 0:bde8ed56b164 | 353 | { |
akudohune | 0:bde8ed56b164 | 354 | AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */ |
akudohune | 0:bde8ed56b164 | 355 | irDistance[i] = ((adcIn.read_u16() >> 4) - stand[i]); |
yusuke_robocup | 1:3b61675573b1 | 356 | if(irDistance[i] >= 30) |
akudohune | 0:bde8ed56b164 | 357 | { |
akudohune | 0:bde8ed56b164 | 358 | whiteFlag = 1; |
yusuke_robocup | 1:3b61675573b1 | 359 | movein = 1; |
yusuke_robocup | 1:3b61675573b1 | 360 | whiteout = 10; |
yusuke_robocup | 1:3b61675573b1 | 361 | } |
yusuke_robocup | 1:3b61675573b1 | 362 | |
yusuke_robocup | 1:3b61675573b1 | 363 | //pc.printf("%d\n",irDistance[0]); |
yusuke_robocup | 1:3b61675573b1 | 364 | } |
yusuke_robocup | 1:3b61675573b1 | 365 | |
yusuke_robocup | 1:3b61675573b1 | 366 | if(lineStateX == XNORMAL){ |
yusuke_robocup | 1:3b61675573b1 | 367 | if((LEFT_SONIC < 350) && (whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 368 | lineStateX = LEFT_OUT; |
akudohune | 0:bde8ed56b164 | 369 | } |
yusuke_robocup | 1:3b61675573b1 | 370 | if((LEFT_SONIC < 350) && (RIGHT_SONIC < 100) && (whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 371 | lineStateX = LEFT_OUT; |
yusuke_robocup | 1:3b61675573b1 | 372 | } |
yusuke_robocup | 1:3b61675573b1 | 373 | |
yusuke_robocup | 1:3b61675573b1 | 374 | if((RIGHT_SONIC < 350) && (whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 375 | lineStateX = RIGHT_OUT; |
yusuke_robocup | 1:3b61675573b1 | 376 | } |
yusuke_robocup | 1:3b61675573b1 | 377 | if((RIGHT_SONIC < 350) && (LEFT_SONIC < 100) && (whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 378 | lineStateX = RIGHT_OUT; |
yusuke_robocup | 1:3b61675573b1 | 379 | } |
yusuke_robocup | 1:3b61675573b1 | 380 | |
yusuke_robocup | 1:3b61675573b1 | 381 | }else if(lineStateX == LEFT_OUT){ |
yusuke_robocup | 1:3b61675573b1 | 382 | /* |
yusuke_robocup | 1:3b61675573b1 | 383 | if((LEFT_SONIC > 450) && (!whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 384 | lineStateX = XNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 385 | whiteFlag = 0; |
yusuke_robocup | 1:3b61675573b1 | 386 | } |
yusuke_robocup | 1:3b61675573b1 | 387 | */ |
yusuke_robocup | 1:3b61675573b1 | 388 | if((LEFT_SONIC > 450)){ |
yusuke_robocup | 1:3b61675573b1 | 389 | lineStateX = XNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 390 | whiteFlag = 0; |
yusuke_robocup | 1:3b61675573b1 | 391 | } |
yusuke_robocup | 1:3b61675573b1 | 392 | }else if(lineStateX == RIGHT_OUT){ |
yusuke_robocup | 1:3b61675573b1 | 393 | /* |
yusuke_robocup | 1:3b61675573b1 | 394 | if((RIGHT_SONIC > 450) && (!whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 395 | lineStateX = XNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 396 | whiteFlag = 0; |
yusuke_robocup | 1:3b61675573b1 | 397 | } |
yusuke_robocup | 1:3b61675573b1 | 398 | */ |
yusuke_robocup | 1:3b61675573b1 | 399 | if((RIGHT_SONIC > 450)){ |
yusuke_robocup | 1:3b61675573b1 | 400 | lineStateX = XNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 401 | whiteFlag = 0; |
yusuke_robocup | 1:3b61675573b1 | 402 | } |
akudohune | 0:bde8ed56b164 | 403 | } |
akudohune | 0:bde8ed56b164 | 404 | |
yusuke_robocup | 1:3b61675573b1 | 405 | |
yusuke_robocup | 1:3b61675573b1 | 406 | if(lineStateY == YNORMAL){ |
yusuke_robocup | 1:3b61675573b1 | 407 | if((FRONT_SONIC < 400) && (whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 408 | lineStateY = FRONT_OUT; |
yusuke_robocup | 1:3b61675573b1 | 409 | } |
yusuke_robocup | 1:3b61675573b1 | 410 | if((FRONT_SONIC < 400)&& (BACK_SONIC < 100) && (whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 411 | lineStateY = FRONT_OUT; |
yusuke_robocup | 1:3b61675573b1 | 412 | } |
yusuke_robocup | 1:3b61675573b1 | 413 | if((BACK_SONIC < 400) && (whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 414 | lineStateY = BACK_OUT; |
yusuke_robocup | 1:3b61675573b1 | 415 | } |
yusuke_robocup | 1:3b61675573b1 | 416 | if((BACK_SONIC < 400) && (FRONT_SONIC < 100) && (whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 417 | lineStateY = BACK_OUT; |
akudohune | 0:bde8ed56b164 | 418 | } |
yusuke_robocup | 1:3b61675573b1 | 419 | }else if(lineStateY == FRONT_OUT){ |
yusuke_robocup | 1:3b61675573b1 | 420 | /* |
yusuke_robocup | 1:3b61675573b1 | 421 | if((FRONT_SONIC > 500) && (!whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 422 | lineStateY = YNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 423 | whiteFlag = 0; |
akudohune | 0:bde8ed56b164 | 424 | } |
yusuke_robocup | 1:3b61675573b1 | 425 | */ |
yusuke_robocup | 1:3b61675573b1 | 426 | if((FRONT_SONIC > 500)){ |
yusuke_robocup | 1:3b61675573b1 | 427 | lineStateY = YNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 428 | whiteFlag = 0; |
akudohune | 0:bde8ed56b164 | 429 | } |
yusuke_robocup | 1:3b61675573b1 | 430 | }else if(lineStateY == BACK_OUT){ |
yusuke_robocup | 1:3b61675573b1 | 431 | /* |
yusuke_robocup | 1:3b61675573b1 | 432 | if((BACK_SONIC > 500) && (!whiteFlag)){ |
yusuke_robocup | 1:3b61675573b1 | 433 | lineStateY = YNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 434 | whiteFlag = 0; |
akudohune | 0:bde8ed56b164 | 435 | } |
yusuke_robocup | 1:3b61675573b1 | 436 | */ |
yusuke_robocup | 1:3b61675573b1 | 437 | if((BACK_SONIC > 500)){ |
yusuke_robocup | 1:3b61675573b1 | 438 | lineStateY = YNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 439 | whiteFlag = 0; |
akudohune | 0:bde8ed56b164 | 440 | } |
akudohune | 0:bde8ed56b164 | 441 | } |
akudohune | 0:bde8ed56b164 | 442 | |
akudohune | 0:bde8ed56b164 | 443 | |
yusuke_robocup | 1:3b61675573b1 | 444 | if((state_off == ATTACK)&&(Distance == 10)){ |
yusuke_robocup | 1:3b61675573b1 | 445 | state = 1; |
akudohune | 0:bde8ed56b164 | 446 | }else{ |
yusuke_robocup | 1:3b61675573b1 | 447 | state = 0; |
yusuke_robocup | 1:3b61675573b1 | 448 | } |
yusuke_robocup | 1:3b61675573b1 | 449 | |
yusuke_robocup | 1:3b61675573b1 | 450 | if(((direction == 0)||(direction == 1)||(direction == 15))){ |
yusuke_robocup | 1:3b61675573b1 | 451 | state_off = ATTACK; |
yusuke_robocup | 1:3b61675573b1 | 452 | } |
yusuke_robocup | 1:3b61675573b1 | 453 | if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 90)){ |
yusuke_robocup | 1:3b61675573b1 | 454 | if((past_state_off != SNAKE)){ |
yusuke_robocup | 1:3b61675573b1 | 455 | accelera_Distance = Distance; |
akudohune | 0:bde8ed56b164 | 456 | } |
yusuke_robocup | 1:3b61675573b1 | 457 | state_off = SNAKE; |
akudohune | 0:bde8ed56b164 | 458 | } |
yusuke_robocup | 1:3b61675573b1 | 459 | if(Distance >= 120){ |
yusuke_robocup | 1:3b61675573b1 | 460 | state_off = SEARCH; |
yusuke_robocup | 1:3b61675573b1 | 461 | } |
yusuke_robocup | 1:3b61675573b1 | 462 | |
yusuke_robocup | 1:3b61675573b1 | 463 | past_state_off = state_off; |
akudohune | 0:bde8ed56b164 | 464 | |
yusuke_robocup | 1:3b61675573b1 | 465 | if(IR_found){ |
yusuke_robocup | 1:3b61675573b1 | 466 | if(state_off == SNAKE){ |
yusuke_robocup | 1:3b61675573b1 | 467 | if((Distance == 30)||(Distance == 90)){ |
yusuke_robocup | 1:3b61675573b1 | 468 | x_dista = 12*ball_sankaku[direction][0]; |
yusuke_robocup | 1:3b61675573b1 | 469 | y_dista = 12*ball_sankaku[direction][1]; |
yusuke_robocup | 1:3b61675573b1 | 470 | |
yusuke_robocup | 1:3b61675573b1 | 471 | x_turn = 18*(turn_sankaku[direction][0]); |
yusuke_robocup | 1:3b61675573b1 | 472 | y_turn = 18*(turn_sankaku[direction][1]); |
yusuke_robocup | 1:3b61675573b1 | 473 | |
yusuke_robocup | 1:3b61675573b1 | 474 | if((direction == 2)||(direction == 14)){ |
yusuke_robocup | 1:3b61675573b1 | 475 | //x_turn *= 0.7; |
yusuke_robocup | 1:3b61675573b1 | 476 | y_turn *= 0.7; |
akudohune | 0:bde8ed56b164 | 477 | } |
yusuke_robocup | 1:3b61675573b1 | 478 | |
yusuke_robocup | 1:3b61675573b1 | 479 | if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){ |
yusuke_robocup | 1:3b61675573b1 | 480 | x_turn = 7*(turn_sankaku[direction][0]); |
yusuke_robocup | 1:3b61675573b1 | 481 | y_turn = 7*(turn_sankaku[direction][1]); |
yusuke_robocup | 1:3b61675573b1 | 482 | |
yusuke_robocup | 1:3b61675573b1 | 483 | x_dista = 15*ball_sankaku[direction][0]; |
yusuke_robocup | 1:3b61675573b1 | 484 | y_dista = 10*ball_sankaku[direction][1]; |
yusuke_robocup | 1:3b61675573b1 | 485 | |
yusuke_robocup | 1:3b61675573b1 | 486 | } |
yusuke_robocup | 1:3b61675573b1 | 487 | |
yusuke_robocup | 1:3b61675573b1 | 488 | if(direction == 1){ |
yusuke_robocup | 1:3b61675573b1 | 489 | vx = 15; |
akudohune | 0:bde8ed56b164 | 490 | vy = 0; |
akudohune | 0:bde8ed56b164 | 491 | } |
yusuke_robocup | 1:3b61675573b1 | 492 | |
yusuke_robocup | 1:3b61675573b1 | 493 | if(direction == 15){ |
akudohune | 0:bde8ed56b164 | 494 | vx = -15; |
yusuke_robocup | 1:3b61675573b1 | 495 | vy = 0; |
akudohune | 0:bde8ed56b164 | 496 | } |
yusuke_robocup | 1:3b61675573b1 | 497 | |
yusuke_robocup | 1:3b61675573b1 | 498 | if(direction == 2){ |
yusuke_robocup | 1:3b61675573b1 | 499 | vx = 20; |
akudohune | 0:bde8ed56b164 | 500 | vy = -10; |
yusuke_robocup | 1:3b61675573b1 | 501 | } |
yusuke_robocup | 1:3b61675573b1 | 502 | |
yusuke_robocup | 1:3b61675573b1 | 503 | if(direction == 14){ |
yusuke_robocup | 1:3b61675573b1 | 504 | vx = -20; |
akudohune | 0:bde8ed56b164 | 505 | vy = -10; |
akudohune | 0:bde8ed56b164 | 506 | } |
yusuke_robocup | 1:3b61675573b1 | 507 | |
akudohune | 0:bde8ed56b164 | 508 | } |
yusuke_robocup | 1:3b61675573b1 | 509 | |
yusuke_robocup | 1:3b61675573b1 | 510 | if(Distance == 10){ |
yusuke_robocup | 1:3b61675573b1 | 511 | x_dista = 8*(-ball_sankaku[direction][0]); |
yusuke_robocup | 1:3b61675573b1 | 512 | y_dista = 8*(-ball_sankaku[direction][1]); |
yusuke_robocup | 1:3b61675573b1 | 513 | |
yusuke_robocup | 1:3b61675573b1 | 514 | x_turn = 22*(turn_sankaku[direction][0]); |
yusuke_robocup | 1:3b61675573b1 | 515 | y_turn = 22*(turn_sankaku[direction][1]); |
yusuke_robocup | 1:3b61675573b1 | 516 | |
yusuke_robocup | 1:3b61675573b1 | 517 | |
yusuke_robocup | 1:3b61675573b1 | 518 | if((direction == 2)||(direction == 14)){ |
yusuke_robocup | 1:3b61675573b1 | 519 | y_turn *= 0.7; |
akudohune | 0:bde8ed56b164 | 520 | } |
yusuke_robocup | 1:3b61675573b1 | 521 | |
yusuke_robocup | 1:3b61675573b1 | 522 | if(direction == 2){ |
yusuke_robocup | 1:3b61675573b1 | 523 | vx = 20; |
yusuke_robocup | 1:3b61675573b1 | 524 | vy = -15; |
akudohune | 0:bde8ed56b164 | 525 | } |
yusuke_robocup | 1:3b61675573b1 | 526 | if(direction == 14){ |
yusuke_robocup | 1:3b61675573b1 | 527 | vx = -20; |
yusuke_robocup | 1:3b61675573b1 | 528 | vy = -15; |
yusuke_robocup | 1:3b61675573b1 | 529 | } |
akudohune | 0:bde8ed56b164 | 530 | } |
yusuke_robocup | 1:3b61675573b1 | 531 | |
yusuke_robocup | 1:3b61675573b1 | 532 | if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){ |
yusuke_robocup | 1:3b61675573b1 | 533 | x_dista = 0; |
yusuke_robocup | 1:3b61675573b1 | 534 | y_dista = 0; |
akudohune | 0:bde8ed56b164 | 535 | } |
akudohune | 0:bde8ed56b164 | 536 | |
yusuke_robocup | 1:3b61675573b1 | 537 | if(direction == 2){ |
akudohune | 0:bde8ed56b164 | 538 | vx = 20; |
yusuke_robocup | 1:3b61675573b1 | 539 | vy = -15; |
akudohune | 0:bde8ed56b164 | 540 | } |
yusuke_robocup | 1:3b61675573b1 | 541 | if(direction == 14){ |
yusuke_robocup | 1:3b61675573b1 | 542 | vx = -20; |
yusuke_robocup | 1:3b61675573b1 | 543 | vy = -15; |
yusuke_robocup | 1:3b61675573b1 | 544 | } |
yusuke_robocup | 1:3b61675573b1 | 545 | |
yusuke_robocup | 1:3b61675573b1 | 546 | vx = x_turn + x_dista; |
yusuke_robocup | 1:3b61675573b1 | 547 | vy = y_turn + y_dista; |
yusuke_robocup | 1:3b61675573b1 | 548 | |
yusuke_robocup | 1:3b61675573b1 | 549 | /* |
yusuke_robocup | 1:3b61675573b1 | 550 | if((accelera_Distance == 90)){ |
yusuke_robocup | 1:3b61675573b1 | 551 | if(Distance == 10){ |
yusuke_robocup | 1:3b61675573b1 | 552 | vx *= 0.3; |
yusuke_robocup | 1:3b61675573b1 | 553 | vy *= 0.3; |
yusuke_robocup | 1:3b61675573b1 | 554 | } |
yusuke_robocup | 1:3b61675573b1 | 555 | |
yusuke_robocup | 1:3b61675573b1 | 556 | if(Distance == 30){ |
yusuke_robocup | 1:3b61675573b1 | 557 | vx *= 0.4; |
yusuke_robocup | 1:3b61675573b1 | 558 | vy *= 0.4; |
yusuke_robocup | 1:3b61675573b1 | 559 | } |
yusuke_robocup | 1:3b61675573b1 | 560 | }*/ |
yusuke_robocup | 1:3b61675573b1 | 561 | /* |
yusuke_robocup | 1:3b61675573b1 | 562 | if((accelera_Distance == 10)){ |
yusuke_robocup | 1:3b61675573b1 | 563 | if((direction == 4)||(direction == 12)){ |
yusuke_robocup | 1:3b61675573b1 | 564 | vx = 0; |
yusuke_robocup | 1:3b61675573b1 | 565 | vy = -10; |
yusuke_robocup | 1:3b61675573b1 | 566 | } |
yusuke_robocup | 1:3b61675573b1 | 567 | }*/ |
yusuke_robocup | 1:3b61675573b1 | 568 | |
yusuke_robocup | 1:3b61675573b1 | 569 | }else if(state_off == ATTACK){ |
yusuke_robocup | 1:3b61675573b1 | 570 | if(direction == 0){ |
yusuke_robocup | 1:3b61675573b1 | 571 | vx = 10*ball_sankaku[direction][0]; |
yusuke_robocup | 1:3b61675573b1 | 572 | vy =20*ball_sankaku[direction][1]; |
yusuke_robocup | 1:3b61675573b1 | 573 | /* |
yusuke_robocup | 1:3b61675573b1 | 574 | if(ultrasonicVal[1] < 380){ |
yusuke_robocup | 1:3b61675573b1 | 575 | vy = 10; |
yusuke_robocup | 1:3b61675573b1 | 576 | vx = -20; |
yusuke_robocup | 1:3b61675573b1 | 577 | } |
yusuke_robocup | 1:3b61675573b1 | 578 | |
yusuke_robocup | 1:3b61675573b1 | 579 | if(ultrasonicVal[3] < 380){ |
yusuke_robocup | 1:3b61675573b1 | 580 | vy = 10; |
yusuke_robocup | 1:3b61675573b1 | 581 | vx = 20; |
yusuke_robocup | 1:3b61675573b1 | 582 | } */ |
yusuke_robocup | 1:3b61675573b1 | 583 | } |
yusuke_robocup | 1:3b61675573b1 | 584 | if(direction == 1){ |
yusuke_robocup | 1:3b61675573b1 | 585 | vx = 15*1.3; |
yusuke_robocup | 1:3b61675573b1 | 586 | vy = 20*1.3; |
yusuke_robocup | 1:3b61675573b1 | 587 | } |
yusuke_robocup | 1:3b61675573b1 | 588 | if(direction == 15){ |
yusuke_robocup | 1:3b61675573b1 | 589 | vx = -15*1.3; |
yusuke_robocup | 1:3b61675573b1 | 590 | vy = 20*1.3; |
yusuke_robocup | 1:3b61675573b1 | 591 | } |
yusuke_robocup | 1:3b61675573b1 | 592 | if(direction == 2){ |
yusuke_robocup | 1:3b61675573b1 | 593 | vx = 25; |
yusuke_robocup | 1:3b61675573b1 | 594 | vy = 0; |
yusuke_robocup | 1:3b61675573b1 | 595 | } |
yusuke_robocup | 1:3b61675573b1 | 596 | if(direction == 14){ |
yusuke_robocup | 1:3b61675573b1 | 597 | vx = -25; |
yusuke_robocup | 1:3b61675573b1 | 598 | vy = 0; |
akudohune | 0:bde8ed56b164 | 599 | } |
akudohune | 0:bde8ed56b164 | 600 | |
yusuke_robocup | 1:3b61675573b1 | 601 | if(Distance > 30){ |
yusuke_robocup | 1:3b61675573b1 | 602 | if(direction == 2){ |
yusuke_robocup | 1:3b61675573b1 | 603 | vx = 20; |
yusuke_robocup | 1:3b61675573b1 | 604 | vy = 10; |
yusuke_robocup | 1:3b61675573b1 | 605 | } |
yusuke_robocup | 1:3b61675573b1 | 606 | if(direction == 14){ |
yusuke_robocup | 1:3b61675573b1 | 607 | vx = -20; |
yusuke_robocup | 1:3b61675573b1 | 608 | vy = 10; |
yusuke_robocup | 1:3b61675573b1 | 609 | } |
yusuke_robocup | 1:3b61675573b1 | 610 | } |
akudohune | 0:bde8ed56b164 | 611 | |
yusuke_robocup | 1:3b61675573b1 | 612 | }else if(state_off == SEARCH){ |
yusuke_robocup | 1:3b61675573b1 | 613 | vx = 24*ball_sankaku[direction][0]; |
yusuke_robocup | 1:3b61675573b1 | 614 | vy = 24*ball_sankaku[direction][1]; |
akudohune | 0:bde8ed56b164 | 615 | |
yusuke_robocup | 1:3b61675573b1 | 616 | if(direction == 2){ |
yusuke_robocup | 1:3b61675573b1 | 617 | vx = 20; |
yusuke_robocup | 1:3b61675573b1 | 618 | } |
yusuke_robocup | 1:3b61675573b1 | 619 | if(direction == 14){ |
yusuke_robocup | 1:3b61675573b1 | 620 | vx = -20; |
yusuke_robocup | 1:3b61675573b1 | 621 | } |
yusuke_robocup | 1:3b61675573b1 | 622 | if(direction == 0){ |
yusuke_robocup | 1:3b61675573b1 | 623 | vx = 20*ball_sankaku[direction][0]; |
yusuke_robocup | 1:3b61675573b1 | 624 | vy = 15*ball_sankaku[direction][1]; |
yusuke_robocup | 1:3b61675573b1 | 625 | } |
yusuke_robocup | 1:3b61675573b1 | 626 | if(direction == 1){ |
yusuke_robocup | 1:3b61675573b1 | 627 | vx = 20*ball_sankaku[direction][0]; |
yusuke_robocup | 1:3b61675573b1 | 628 | vy = 10*ball_sankaku[direction][1]; |
yusuke_robocup | 1:3b61675573b1 | 629 | } |
yusuke_robocup | 1:3b61675573b1 | 630 | if(direction == 15){ |
yusuke_robocup | 1:3b61675573b1 | 631 | vx = 20*ball_sankaku[direction][0]; |
yusuke_robocup | 1:3b61675573b1 | 632 | vy = 10*ball_sankaku[direction][1]; |
yusuke_robocup | 1:3b61675573b1 | 633 | } |
yusuke_robocup | 1:3b61675573b1 | 634 | if(direction == 4){ |
yusuke_robocup | 1:3b61675573b1 | 635 | vx = 0; |
yusuke_robocup | 1:3b61675573b1 | 636 | vy = -15; |
yusuke_robocup | 1:3b61675573b1 | 637 | } |
yusuke_robocup | 1:3b61675573b1 | 638 | if(direction == 12){ |
yusuke_robocup | 1:3b61675573b1 | 639 | vx = 0; |
yusuke_robocup | 1:3b61675573b1 | 640 | vy = -15; |
yusuke_robocup | 1:3b61675573b1 | 641 | } |
yusuke_robocup | 1:3b61675573b1 | 642 | } |
akudohune | 0:bde8ed56b164 | 643 | |
yusuke_robocup | 1:3b61675573b1 | 644 | if(direction == 2){ |
akudohune | 0:bde8ed56b164 | 645 | vx = 20; |
yusuke_robocup | 1:3b61675573b1 | 646 | vy = 0; |
yusuke_robocup | 1:3b61675573b1 | 647 | |
yusuke_robocup | 1:3b61675573b1 | 648 | } |
yusuke_robocup | 1:3b61675573b1 | 649 | if(direction == 14){ |
akudohune | 0:bde8ed56b164 | 650 | vx = -20; |
akudohune | 0:bde8ed56b164 | 651 | vy = 0; |
yusuke_robocup | 1:3b61675573b1 | 652 | } |
yusuke_robocup | 1:3b61675573b1 | 653 | |
yusuke_robocup | 1:3b61675573b1 | 654 | }else{ |
yusuke_robocup | 1:3b61675573b1 | 655 | vx = 0; |
yusuke_robocup | 1:3b61675573b1 | 656 | vy = 0; |
akudohune | 0:bde8ed56b164 | 657 | } |
akudohune | 0:bde8ed56b164 | 658 | |
yusuke_robocup | 1:3b61675573b1 | 659 | vx *= 1.3* 0.8; |
yusuke_robocup | 1:3b61675573b1 | 660 | vy *= 0.7 * 0.8; |
yusuke_robocup | 1:3b61675573b1 | 661 | |
yusuke_robocup | 1:3b61675573b1 | 662 | |
yusuke_robocup | 1:3b61675573b1 | 663 | |
yusuke_robocup | 1:3b61675573b1 | 664 | if(lineStateX == LEFT_OUT){ |
yusuke_robocup | 1:3b61675573b1 | 665 | vx += 20; |
yusuke_robocup | 1:3b61675573b1 | 666 | }else if(lineStateX == RIGHT_OUT){ |
yusuke_robocup | 1:3b61675573b1 | 667 | vx -= 20; |
akudohune | 0:bde8ed56b164 | 668 | } |
akudohune | 0:bde8ed56b164 | 669 | |
yusuke_robocup | 1:3b61675573b1 | 670 | if(lineStateY == FRONT_OUT){ |
yusuke_robocup | 1:3b61675573b1 | 671 | vy -= 15; |
yusuke_robocup | 1:3b61675573b1 | 672 | }else if(lineStateY == BACK_OUT){ |
yusuke_robocup | 1:3b61675573b1 | 673 | vy += 15; |
yusuke_robocup | 1:3b61675573b1 | 674 | } |
yusuke_robocup | 1:3b61675573b1 | 675 | |
yusuke_robocup | 1:3b61675573b1 | 676 | //vx *= 0.8; |
yusuke_robocup | 1:3b61675573b1 | 677 | //vy *= 0.8; |
yusuke_robocup | 1:3b61675573b1 | 678 | |
yusuke_robocup | 1:3b61675573b1 | 679 | direction_past = direction; |
yusuke_robocup | 1:3b61675573b1 | 680 | |
akudohune | 0:bde8ed56b164 | 681 | |
akudohune | 0:bde8ed56b164 | 682 | move(vx,vy,vs); |
akudohune | 0:bde8ed56b164 | 683 | } |
yusuke_robocup | 1:3b61675573b1 | 684 | } |