k
Dependents: 3rdcompfixstart 2ndcomp 4thcomp 6th33222_copy
Fork of Move by
move.cpp@15:39c7e97b37c4, 2016-09-17 (annotated)
- Committer:
- choutin
- Date:
- Sat Sep 17 23:20:56 2016 +0000
- Revision:
- 15:39c7e97b37c4
- Parent:
- 14:138af628d979
?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sakanakuuun | 0:d7ff86f25eaa | 1 | #include "mbed.h" |
sakanakuuun | 0:d7ff86f25eaa | 2 | #include "move.h" |
sakanakuuun | 0:d7ff86f25eaa | 3 | #include "locate.h" |
sakanakuuun | 2:f25a09c5e113 | 4 | #include "stdlib.h" |
choutin | 13:4501c9202500 | 5 | #include "math.h" |
choutin | 14:138af628d979 | 6 | #include "servo.h" |
choutin | 14:138af628d979 | 7 | |
choutin | 13:4501c9202500 | 8 | /*************/ |
choutin | 14:138af628d979 | 9 | int rightspeed = 70; |
choutin | 15:39c7e97b37c4 | 10 | int leftspeed = (int)(rightspeed)+7; |
choutin | 15:39c7e97b37c4 | 11 | const int hosei_turnspeed = 14; |
choutin | 13:4501c9202500 | 12 | const int max_disorder = 3; |
choutin | 15:39c7e97b37c4 | 13 | const float ratio = 1.0/7.0; |
choutin | 15:39c7e97b37c4 | 14 | const int t_spe[] = {50,50}; |
choutin | 13:4501c9202500 | 15 | /*************/ |
choutin | 14:138af628d979 | 16 | |
sakanakuuun | 0:d7ff86f25eaa | 17 | PwmOut M1cw(PA_11); |
sakanakuuun | 0:d7ff86f25eaa | 18 | PwmOut M1ccw(PB_15); |
sakanakuuun | 0:d7ff86f25eaa | 19 | PwmOut M2ccw(PB_14); |
sakanakuuun | 0:d7ff86f25eaa | 20 | PwmOut M2cw(PB_13); |
sakanakuuun | 7:7f1721542753 | 21 | |
sakanakuuun | 7:7f1721542753 | 22 | DigitalOut green (PC_2); |
sakanakuuun | 7:7f1721542753 | 23 | DigitalOut yellow(PC_3); |
sakanakuuun | 7:7f1721542753 | 24 | DigitalOut red (PC_0); |
choutin | 14:138af628d979 | 25 | |
choutin | 15:39c7e97b37c4 | 26 | //Serial pc(SERIAL_TX, SERIAL_RX); //pcと通信 |
choutin | 15:39c7e97b37c4 | 27 | |
choutin | 15:39c7e97b37c4 | 28 | |
sakanakuuun | 2:f25a09c5e113 | 29 | |
choutin | 13:4501c9202500 | 30 | int tcolor; |
sakanakuuun | 0:d7ff86f25eaa | 31 | |
choutin | 13:4501c9202500 | 32 | void initmotor(int team) |
sakanakuuun | 0:d7ff86f25eaa | 33 | { |
choutin | 13:4501c9202500 | 34 | tcolor = team; |
choutin | 14:138af628d979 | 35 | if(tcolor == -1) { |
choutin | 14:138af628d979 | 36 | int tspeed = rightspeed; |
choutin | 14:138af628d979 | 37 | rightspeed = leftspeed; |
choutin | 14:138af628d979 | 38 | leftspeed = tspeed; |
choutin | 14:138af628d979 | 39 | } |
sakanakuuun | 0:d7ff86f25eaa | 40 | M1cw.period_us(256); |
sakanakuuun | 0:d7ff86f25eaa | 41 | M1ccw.period_us(256); |
sakanakuuun | 0:d7ff86f25eaa | 42 | M2cw.period_us(256); |
sakanakuuun | 0:d7ff86f25eaa | 43 | M2ccw.period_us(256); |
sakanakuuun | 0:d7ff86f25eaa | 44 | } |
choutin | 14:138af628d979 | 45 | |
sakanakuuun | 0:d7ff86f25eaa | 46 | void move(int left,int right) |
sakanakuuun | 0:d7ff86f25eaa | 47 | { |
sakanakuuun | 0:d7ff86f25eaa | 48 | float rightduty,leftduty; |
choutin | 14:138af628d979 | 49 | |
choutin | 13:4501c9202500 | 50 | if(right>256) |
sakanakuuun | 0:d7ff86f25eaa | 51 | right=256; |
choutin | 13:4501c9202500 | 52 | else if(right<-256) |
choutin | 13:4501c9202500 | 53 | right=-256; |
choutin | 14:138af628d979 | 54 | |
choutin | 13:4501c9202500 | 55 | if(left>256) |
sakanakuuun | 0:d7ff86f25eaa | 56 | left=256; |
choutin | 13:4501c9202500 | 57 | else if(left<-256) |
choutin | 13:4501c9202500 | 58 | left=-256; |
choutin | 14:138af628d979 | 59 | |
choutin | 14:138af628d979 | 60 | if(tcolor == -1) { |
choutin | 13:4501c9202500 | 61 | int t = right; |
choutin | 13:4501c9202500 | 62 | right = left; |
choutin | 13:4501c9202500 | 63 | left = t; |
sakanakuuun | 0:d7ff86f25eaa | 64 | } |
choutin | 14:138af628d979 | 65 | |
sakanakuuun | 0:d7ff86f25eaa | 66 | rightduty=right/256.0; |
sakanakuuun | 0:d7ff86f25eaa | 67 | leftduty=left/256.0; |
choutin | 14:138af628d979 | 68 | |
sakanakuuun | 0:d7ff86f25eaa | 69 | if(right>0) { |
sakanakuuun | 0:d7ff86f25eaa | 70 | M1cw.write(1-rightduty); |
sakanakuuun | 0:d7ff86f25eaa | 71 | M1ccw.write(1); |
sakanakuuun | 0:d7ff86f25eaa | 72 | } else { |
sakanakuuun | 0:d7ff86f25eaa | 73 | M1cw.write(1); |
sakanakuuun | 0:d7ff86f25eaa | 74 | M1ccw.write(1+rightduty); |
sakanakuuun | 0:d7ff86f25eaa | 75 | } |
choutin | 14:138af628d979 | 76 | |
sakanakuuun | 0:d7ff86f25eaa | 77 | if(left>0) { |
sakanakuuun | 0:d7ff86f25eaa | 78 | M2cw.write(1-leftduty); |
sakanakuuun | 0:d7ff86f25eaa | 79 | M2ccw.write(1); |
sakanakuuun | 0:d7ff86f25eaa | 80 | } else { |
sakanakuuun | 0:d7ff86f25eaa | 81 | M2cw.write(1); |
sakanakuuun | 0:d7ff86f25eaa | 82 | M2ccw.write(1+leftduty); |
sakanakuuun | 0:d7ff86f25eaa | 83 | } |
sakanakuuun | 0:d7ff86f25eaa | 84 | } |
choutin | 14:138af628d979 | 85 | |
sakanakuuun | 0:d7ff86f25eaa | 86 | void hosei_turn(float pt, bool cw, float rad) |
sakanakuuun | 0:d7ff86f25eaa | 87 | { |
sakanakuuun | 0:d7ff86f25eaa | 88 | int np; |
sakanakuuun | 0:d7ff86f25eaa | 89 | if(cw) np = 1; |
sakanakuuun | 0:d7ff86f25eaa | 90 | else np = -1; |
sakanakuuun | 0:d7ff86f25eaa | 91 | while(1) { |
choutin | 13:4501c9202500 | 92 | update_np(); |
sakanakuuun | 0:d7ff86f25eaa | 93 | if(pt-coordinateTheta() < np * rad - ALLOW_RAD) { |
choutin | 13:4501c9202500 | 94 | move(-hosei_turnspeed, hosei_turnspeed); |
sakanakuuun | 0:d7ff86f25eaa | 95 | } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) { |
choutin | 13:4501c9202500 | 96 | move(hosei_turnspeed, -hosei_turnspeed); |
sakanakuuun | 0:d7ff86f25eaa | 97 | } else { |
sakanakuuun | 0:d7ff86f25eaa | 98 | move(0,0); |
sakanakuuun | 0:d7ff86f25eaa | 99 | return; |
sakanakuuun | 0:d7ff86f25eaa | 100 | } |
sakanakuuun | 0:d7ff86f25eaa | 101 | } |
choutin | 14:138af628d979 | 102 | |
sakanakuuun | 0:d7ff86f25eaa | 103 | } |
choutin | 14:138af628d979 | 104 | |
choutin | 13:4501c9202500 | 105 | void turnrad(float rad) |
sakanakuuun | 0:d7ff86f25eaa | 106 | { |
sakanakuuun | 7:7f1721542753 | 107 | green = 1; |
choutin | 14:138af628d979 | 108 | |
choutin | 13:4501c9202500 | 109 | update_np(); |
choutin | 14:138af628d979 | 110 | |
sakanakuuun | 1:405e28b64fdb | 111 | int np; |
sakanakuuun | 1:405e28b64fdb | 112 | if(coordinateTheta() > rad) np = 1; |
sakanakuuun | 1:405e28b64fdb | 113 | else if(coordinateTheta() < rad) np = -1; |
sakanakuuun | 1:405e28b64fdb | 114 | else return; |
choutin | 14:138af628d979 | 115 | |
choutin | 15:39c7e97b37c4 | 116 | //move((-np)*rightspeed, (np)*leftspeed); |
choutin | 15:39c7e97b37c4 | 117 | move((-np) * t_spe[0], (np) * t_spe[1]); |
choutin | 13:4501c9202500 | 118 | while(1) { |
choutin | 15:39c7e97b37c4 | 119 | update_np();\ |
choutin | 13:4501c9202500 | 120 | if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) { |
choutin | 13:4501c9202500 | 121 | move(0,0); |
choutin | 13:4501c9202500 | 122 | break; |
choutin | 13:4501c9202500 | 123 | } |
choutin | 13:4501c9202500 | 124 | } |
choutin | 14:138af628d979 | 125 | |
choutin | 13:4501c9202500 | 126 | hosei_turn(0, false, rad); |
choutin | 13:4501c9202500 | 127 | wait(0.2); |
choutin | 13:4501c9202500 | 128 | hosei_turn(0, false, rad); |
choutin | 13:4501c9202500 | 129 | wait(0.2); |
choutin | 13:4501c9202500 | 130 | green = 0; |
choutin | 13:4501c9202500 | 131 | } |
choutin | 14:138af628d979 | 132 | |
choutin | 14:138af628d979 | 133 | |
choutin | 13:4501c9202500 | 134 | void turnrad_ccw(float rad, int team) |
choutin | 13:4501c9202500 | 135 | { |
choutin | 14:138af628d979 | 136 | if(team == 0) { |
choutin | 13:4501c9202500 | 137 | turnrad_cw(rad, 1); |
choutin | 13:4501c9202500 | 138 | return; |
choutin | 13:4501c9202500 | 139 | } |
choutin | 14:138af628d979 | 140 | |
choutin | 13:4501c9202500 | 141 | green = 1; |
choutin | 14:138af628d979 | 142 | |
choutin | 13:4501c9202500 | 143 | update(); |
choutin | 14:138af628d979 | 144 | |
choutin | 15:39c7e97b37c4 | 145 | //move(rightspeed, -leftspeed); |
choutin | 15:39c7e97b37c4 | 146 | move(t_spe[0], -t_spe[1]); |
choutin | 15:39c7e97b37c4 | 147 | |
choutin | 14:138af628d979 | 148 | while(1) { |
choutin | 14:138af628d979 | 149 | |
choutin | 14:138af628d979 | 150 | |
choutin | 14:138af628d979 | 151 | update(); |
choutin | 14:138af628d979 | 152 | if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) { |
choutin | 14:138af628d979 | 153 | move(0,0); |
choutin | 14:138af628d979 | 154 | break; |
choutin | 14:138af628d979 | 155 | } |
choutin | 14:138af628d979 | 156 | } |
choutin | 14:138af628d979 | 157 | |
choutin | 14:138af628d979 | 158 | hosei_turn(0, false, rad); |
choutin | 14:138af628d979 | 159 | wait(0.2); |
choutin | 14:138af628d979 | 160 | hosei_turn(0, false, rad); |
choutin | 14:138af628d979 | 161 | wait(0.2); |
choutin | 14:138af628d979 | 162 | green = 0; |
choutin | 14:138af628d979 | 163 | } |
choutin | 14:138af628d979 | 164 | |
choutin | 14:138af628d979 | 165 | void turnrad_cw(float rad, int team) |
choutin | 14:138af628d979 | 166 | { |
choutin | 14:138af628d979 | 167 | if(team == 0) { |
choutin | 14:138af628d979 | 168 | turnrad_ccw(rad, 1); |
choutin | 14:138af628d979 | 169 | return; |
choutin | 14:138af628d979 | 170 | } |
choutin | 14:138af628d979 | 171 | |
choutin | 14:138af628d979 | 172 | green = 1; |
choutin | 14:138af628d979 | 173 | |
choutin | 14:138af628d979 | 174 | update(); |
choutin | 14:138af628d979 | 175 | |
choutin | 15:39c7e97b37c4 | 176 | //move((-1)*rightspeed, leftspeed); |
choutin | 15:39c7e97b37c4 | 177 | move(-t_spe[0], t_spe[1]); |
sakanakuuun | 0:d7ff86f25eaa | 178 | while(1) { |
sakanakuuun | 0:d7ff86f25eaa | 179 | update(); |
sakanakuuun | 2:f25a09c5e113 | 180 | if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) { |
sakanakuuun | 0:d7ff86f25eaa | 181 | move(0,0); |
sakanakuuun | 0:d7ff86f25eaa | 182 | break; |
sakanakuuun | 0:d7ff86f25eaa | 183 | } |
sakanakuuun | 0:d7ff86f25eaa | 184 | } |
choutin | 14:138af628d979 | 185 | |
choutin | 13:4501c9202500 | 186 | hosei_turn(0, false, rad); |
choutin | 13:4501c9202500 | 187 | wait(0.2); |
choutin | 13:4501c9202500 | 188 | hosei_turn(0, false, rad); |
choutin | 13:4501c9202500 | 189 | wait(0.2); |
choutin | 13:4501c9202500 | 190 | green = 0; |
choutin | 13:4501c9202500 | 191 | } |
sakanakuuun | 1:405e28b64fdb | 192 | |
sakanakuuun | 1:405e28b64fdb | 193 | |
choutin | 15:39c7e97b37c4 | 194 | |
choutin | 13:4501c9202500 | 195 | void movelength(int length) |
choutin | 13:4501c9202500 | 196 | { |
choutin | 13:4501c9202500 | 197 | int px,py,pt; |
choutin | 13:4501c9202500 | 198 | update(); |
choutin | 13:4501c9202500 | 199 | px=coordinateX(); |
choutin | 13:4501c9202500 | 200 | py=coordinateY(); |
choutin | 13:4501c9202500 | 201 | pt=coordinateTheta(); |
sakanakuuun | 1:405e28b64fdb | 202 | |
choutin | 13:4501c9202500 | 203 | move(rightspeed,leftspeed); |
choutin | 13:4501c9202500 | 204 | while(1) { |
sakanakuuun | 1:405e28b64fdb | 205 | |
choutin | 13:4501c9202500 | 206 | update(); |
choutin | 13:4501c9202500 | 207 | //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta()); |
choutin | 13:4501c9202500 | 208 | if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) { |
choutin | 13:4501c9202500 | 209 | break; |
choutin | 13:4501c9202500 | 210 | } |
sakanakuuun | 7:7f1721542753 | 211 | |
choutin | 13:4501c9202500 | 212 | } |
choutin | 15:39c7e97b37c4 | 213 | move(-32,-30); |
choutin | 15:39c7e97b37c4 | 214 | wait(0.05); |
choutin | 15:39c7e97b37c4 | 215 | move(0,0); |
choutin | 15:39c7e97b37c4 | 216 | |
choutin | 15:39c7e97b37c4 | 217 | } |
choutin | 15:39c7e97b37c4 | 218 | |
choutin | 15:39c7e97b37c4 | 219 | |
choutin | 15:39c7e97b37c4 | 220 | void movelengthnoprintf(int length) |
choutin | 15:39c7e97b37c4 | 221 | { |
choutin | 15:39c7e97b37c4 | 222 | int px,py,pt; |
choutin | 15:39c7e97b37c4 | 223 | update(); |
choutin | 15:39c7e97b37c4 | 224 | px=coordinateX(); |
choutin | 15:39c7e97b37c4 | 225 | py=coordinateY(); |
choutin | 15:39c7e97b37c4 | 226 | pt=coordinateTheta(); |
choutin | 15:39c7e97b37c4 | 227 | |
choutin | 15:39c7e97b37c4 | 228 | move(rightspeed,leftspeed); |
choutin | 15:39c7e97b37c4 | 229 | while(1) { |
choutin | 15:39c7e97b37c4 | 230 | |
choutin | 15:39c7e97b37c4 | 231 | update_np(); |
choutin | 15:39c7e97b37c4 | 232 | if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) { |
choutin | 15:39c7e97b37c4 | 233 | break; |
choutin | 15:39c7e97b37c4 | 234 | } |
choutin | 15:39c7e97b37c4 | 235 | |
choutin | 15:39c7e97b37c4 | 236 | } |
choutin | 13:4501c9202500 | 237 | move(0,0); |
sakanakuuun | 1:405e28b64fdb | 238 | } |
sakanakuuun | 1:405e28b64fdb | 239 | |
choutin | 13:4501c9202500 | 240 | |
sakanakuuun | 5:0e18cf25291a | 241 | void pmove(int x, int y) |
sakanakuuun | 7:7f1721542753 | 242 | { |
sakanakuuun | 7:7f1721542753 | 243 | yellow = 1; |
choutin | 14:138af628d979 | 244 | |
choutin | 13:4501c9202500 | 245 | float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数 |
choutin | 13:4501c9202500 | 246 | int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数 |
choutin | 14:138af628d979 | 247 | |
sakanakuuun | 4:4c574be6325c | 248 | int length, dx, dy; |
sakanakuuun | 2:f25a09c5e113 | 249 | int *d_length, *disorder; |
sakanakuuun | 5:0e18cf25291a | 250 | int absd_length; |
sakanakuuun | 4:4c574be6325c | 251 | float dtheta, ptheta; |
sakanakuuun | 2:f25a09c5e113 | 252 | float daikei; |
choutin | 14:138af628d979 | 253 | |
sakanakuuun | 7:7f1721542753 | 254 | int direction; |
choutin | 14:138af628d979 | 255 | |
sakanakuuun | 5:0e18cf25291a | 256 | if(abs(x - coordinateX()) > abs(y - coordinateY())) { |
sakanakuuun | 5:0e18cf25291a | 257 | y = coordinateY(); |
sakanakuuun | 5:0e18cf25291a | 258 | direction = X_PLUS; |
sakanakuuun | 5:0e18cf25291a | 259 | length = abs(x - coordinateX()); |
sakanakuuun | 5:0e18cf25291a | 260 | d_length = &dx; |
sakanakuuun | 5:0e18cf25291a | 261 | disorder = &dy; |
sakanakuuun | 7:7f1721542753 | 262 | } else { |
sakanakuuun | 5:0e18cf25291a | 263 | x = coordinateX(); |
sakanakuuun | 5:0e18cf25291a | 264 | direction = Y_PLUS; |
sakanakuuun | 5:0e18cf25291a | 265 | length = abs(y - coordinateY()); |
sakanakuuun | 5:0e18cf25291a | 266 | d_length = &dy; |
sakanakuuun | 5:0e18cf25291a | 267 | disorder = &dx; |
sakanakuuun | 5:0e18cf25291a | 268 | } |
choutin | 14:138af628d979 | 269 | |
sakanakuuun | 7:7f1721542753 | 270 | update(); |
sakanakuuun | 7:7f1721542753 | 271 | dx = x - coordinateX(); |
sakanakuuun | 7:7f1721542753 | 272 | dy = y - coordinateY(); |
choutin | 14:138af628d979 | 273 | |
sakanakuuun | 7:7f1721542753 | 274 | if(*d_length < 0) //x,y減少方向なら、*d_length<0 |
sakanakuuun | 5:0e18cf25291a | 275 | direction *= -1; |
choutin | 14:138af628d979 | 276 | |
sakanakuuun | 2:f25a09c5e113 | 277 | switch(direction) { |
sakanakuuun | 2:f25a09c5e113 | 278 | case X_PLUS: |
sakanakuuun | 2:f25a09c5e113 | 279 | ptheta = 0; |
sakanakuuun | 2:f25a09c5e113 | 280 | break; |
sakanakuuun | 2:f25a09c5e113 | 281 | case Y_PLUS: |
sakanakuuun | 2:f25a09c5e113 | 282 | ptheta = PI/2; |
sakanakuuun | 2:f25a09c5e113 | 283 | break; |
sakanakuuun | 2:f25a09c5e113 | 284 | case X_MINUS: |
sakanakuuun | 2:f25a09c5e113 | 285 | k *= -1; |
sakanakuuun | 2:f25a09c5e113 | 286 | ptheta = PI; |
sakanakuuun | 2:f25a09c5e113 | 287 | break; |
sakanakuuun | 2:f25a09c5e113 | 288 | case Y_MINUS: |
choutin | 14:138af628d979 | 289 | k *= -1; |
choutin | 11:451b0c1d06b7 | 290 | ptheta = -PI/2; |
sakanakuuun | 2:f25a09c5e113 | 291 | break; |
sakanakuuun | 2:f25a09c5e113 | 292 | default: |
sakanakuuun | 2:f25a09c5e113 | 293 | return; |
sakanakuuun | 2:f25a09c5e113 | 294 | } |
choutin | 14:138af628d979 | 295 | |
choutin | 13:4501c9202500 | 296 | ptheta += nearPi(coordinateTheta() - ptheta); |
choutin | 14:138af628d979 | 297 | |
choutin | 13:4501c9202500 | 298 | turnrad(ptheta); |
choutin | 14:138af628d979 | 299 | |
sakanakuuun | 2:f25a09c5e113 | 300 | if(length == 0) return; |
choutin | 14:138af628d979 | 301 | |
choutin | 13:4501c9202500 | 302 | int i = 0; |
choutin | 14:138af628d979 | 303 | |
sakanakuuun | 1:405e28b64fdb | 304 | while(1) { |
choutin | 14:138af628d979 | 305 | update(); |
sakanakuuun | 5:0e18cf25291a | 306 | dx = x - coordinateX(); |
sakanakuuun | 5:0e18cf25291a | 307 | dy = y - coordinateY(); |
sakanakuuun | 2:f25a09c5e113 | 308 | dtheta = coordinateTheta() - ptheta; |
choutin | 14:138af628d979 | 309 | |
choutin | 13:4501c9202500 | 310 | if(*disorder>max_disorder) { |
choutin | 13:4501c9202500 | 311 | *disorder = max_disorder; |
choutin | 13:4501c9202500 | 312 | } else if(*disorder<-max_disorder) { |
choutin | 13:4501c9202500 | 313 | *disorder = -max_disorder; |
sakanakuuun | 1:405e28b64fdb | 314 | } |
choutin | 14:138af628d979 | 315 | |
sakanakuuun | 5:0e18cf25291a | 316 | absd_length = abs(*d_length); |
choutin | 14:138af628d979 | 317 | |
choutin | 14:138af628d979 | 318 | |
choutin | 13:4501c9202500 | 319 | if(i++ < 5) { |
choutin | 13:4501c9202500 | 320 | daikei = i/5; |
choutin | 13:4501c9202500 | 321 | } else if(absd_length < 300) { |
choutin | 13:4501c9202500 | 322 | daikei = absd_length / 300.0; |
sakanakuuun | 4:4c574be6325c | 323 | } else |
sakanakuuun | 4:4c574be6325c | 324 | daikei = 1; |
choutin | 14:138af628d979 | 325 | |
choutin | 15:39c7e97b37c4 | 326 | move(daikei * (rightspeed*(1-ratio) - k*(*disorder) - k_theta*dtheta) + rightspeed*ratio, |
choutin | 15:39c7e97b37c4 | 327 | daikei * (leftspeed *(1-ratio) + k*(*disorder) + k_theta*dtheta) + leftspeed *ratio); |
choutin | 14:138af628d979 | 328 | |
choutin | 15:39c7e97b37c4 | 329 | //pc.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei); |
sakanakuuun | 5:0e18cf25291a | 330 | if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) { |
sakanakuuun | 2:f25a09c5e113 | 331 | move(0, 0); |
sakanakuuun | 1:405e28b64fdb | 332 | break; |
sakanakuuun | 1:405e28b64fdb | 333 | } |
choutin | 14:138af628d979 | 334 | |
sakanakuuun | 0:d7ff86f25eaa | 335 | } |
choutin | 14:138af628d979 | 336 | |
choutin | 13:4501c9202500 | 337 | wait(0.2); |
choutin | 14:138af628d979 | 338 | |
sakanakuuun | 7:7f1721542753 | 339 | yellow = 0; |
sakanakuuun | 0:d7ff86f25eaa | 340 | } |
choutin | 14:138af628d979 | 341 | |
choutin | 13:4501c9202500 | 342 | void pmove2(int x, int y) |
choutin | 13:4501c9202500 | 343 | { |
choutin | 14:138af628d979 | 344 | yellow = 0.8; |
choutin | 13:4501c9202500 | 345 | red=0; |
choutin | 13:4501c9202500 | 346 | float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数 |
choutin | 13:4501c9202500 | 347 | int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数 |
choutin | 14:138af628d979 | 348 | |
choutin | 13:4501c9202500 | 349 | double length; |
choutin | 13:4501c9202500 | 350 | int d_length, disorder; |
choutin | 13:4501c9202500 | 351 | float dtheta, ptheta; |
choutin | 13:4501c9202500 | 352 | float daikei; |
choutin | 14:138af628d979 | 353 | |
choutin | 15:39c7e97b37c4 | 354 | length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2)) - 10; |
choutin | 14:138af628d979 | 355 | |
choutin | 13:4501c9202500 | 356 | if(length == 0) { |
choutin | 13:4501c9202500 | 357 | red=1; |
choutin | 13:4501c9202500 | 358 | return; |
choutin | 13:4501c9202500 | 359 | } |
choutin | 14:138af628d979 | 360 | |
choutin | 13:4501c9202500 | 361 | ptheta = giveatan(x, y); |
choutin | 14:138af628d979 | 362 | |
choutin | 13:4501c9202500 | 363 | ptheta += nearPi(coordinateTheta() - ptheta); |
choutin | 14:138af628d979 | 364 | |
choutin | 15:39c7e97b37c4 | 365 | //pc.printf("pt:%f", ptheta); |
choutin | 13:4501c9202500 | 366 | turnrad(ptheta); |
choutin | 14:138af628d979 | 367 | |
choutin | 13:4501c9202500 | 368 | virtual_setup(); |
choutin | 14:138af628d979 | 369 | |
choutin | 13:4501c9202500 | 370 | int i = 0; |
choutin | 14:138af628d979 | 371 | |
choutin | 13:4501c9202500 | 372 | while(1) { |
choutin | 14:138af628d979 | 373 | update(); |
choutin | 13:4501c9202500 | 374 | virtual_update(); |
choutin | 14:138af628d979 | 375 | |
choutin | 13:4501c9202500 | 376 | d_length = length - virtual_coordinateX(); |
choutin | 13:4501c9202500 | 377 | disorder = virtual_coordinateY(); |
choutin | 13:4501c9202500 | 378 | dtheta = virtual_coordinateTheta(); |
choutin | 14:138af628d979 | 379 | |
choutin | 13:4501c9202500 | 380 | if(disorder>max_disorder) { |
choutin | 13:4501c9202500 | 381 | disorder = max_disorder; |
choutin | 13:4501c9202500 | 382 | } else if(disorder<-max_disorder) { |
choutin | 13:4501c9202500 | 383 | disorder = -max_disorder; |
choutin | 13:4501c9202500 | 384 | } |
choutin | 14:138af628d979 | 385 | |
choutin | 13:4501c9202500 | 386 | if(i++ < 5) { |
choutin | 13:4501c9202500 | 387 | daikei = i/5; |
choutin | 13:4501c9202500 | 388 | } else if(d_length < 300) { |
choutin | 13:4501c9202500 | 389 | daikei = d_length / 300.0; |
choutin | 13:4501c9202500 | 390 | } else |
choutin | 13:4501c9202500 | 391 | daikei = 1; |
choutin | 14:138af628d979 | 392 | |
choutin | 14:138af628d979 | 393 | move(daikei * (rightspeed*(1-ratio) + k*disorder - k_theta*dtheta) + rightspeed*ratio + k*disorder - k_theta*dtheta, |
choutin | 14:138af628d979 | 394 | daikei * (leftspeed *(1-ratio) - k*disorder + k_theta*dtheta) + leftspeed *ratio - k*disorder + k_theta*dtheta); |
choutin | 14:138af628d979 | 395 | |
choutin | 14:138af628d979 | 396 | |
choutin | 15:39c7e97b37c4 | 397 | //printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY()); |
choutin | 13:4501c9202500 | 398 | if(d_length <= 0) { |
choutin | 13:4501c9202500 | 399 | move(0, 0); |
choutin | 13:4501c9202500 | 400 | break; |
choutin | 13:4501c9202500 | 401 | } |
choutin | 14:138af628d979 | 402 | |
choutin | 13:4501c9202500 | 403 | } |
choutin | 14:138af628d979 | 404 | |
choutin | 13:4501c9202500 | 405 | wait(0.2); |
choutin | 14:138af628d979 | 406 | |
choutin | 13:4501c9202500 | 407 | yellow = 0; |
choutin | 13:4501c9202500 | 408 | red = 0; |
choutin | 13:4501c9202500 | 409 | } |
choutin | 14:138af628d979 | 410 | |
sakanakuuun | 6:0aa97a99c9cb | 411 | void back300() |
sakanakuuun | 7:7f1721542753 | 412 | { |
sakanakuuun | 7:7f1721542753 | 413 | red = 1; |
choutin | 14:138af628d979 | 414 | |
sakanakuuun | 4:4c574be6325c | 415 | float k = 0.9; |
sakanakuuun | 6:0aa97a99c9cb | 416 | int length, px, py, dx, dy; |
choutin | 14:138af628d979 | 417 | |
sakanakuuun | 6:0aa97a99c9cb | 418 | update(); |
choutin | 14:138af628d979 | 419 | |
sakanakuuun | 7:7f1721542753 | 420 | px = coordinateX(); |
sakanakuuun | 7:7f1721542753 | 421 | py = coordinateY(); |
choutin | 14:138af628d979 | 422 | |
sakanakuuun | 6:0aa97a99c9cb | 423 | length = 300; |
choutin | 14:138af628d979 | 424 | |
sakanakuuun | 4:4c574be6325c | 425 | while(1) { |
choutin | 13:4501c9202500 | 426 | update_np(); |
sakanakuuun | 6:0aa97a99c9cb | 427 | dx = coordinateX() - px; |
sakanakuuun | 6:0aa97a99c9cb | 428 | dy = coordinateY() - py; |
choutin | 14:138af628d979 | 429 | |
choutin | 13:4501c9202500 | 430 | if(dy>max_disorder) { |
choutin | 13:4501c9202500 | 431 | dy = max_disorder; |
choutin | 13:4501c9202500 | 432 | } else if(dy<-max_disorder) { |
choutin | 13:4501c9202500 | 433 | dy = -max_disorder; |
sakanakuuun | 6:0aa97a99c9cb | 434 | } |
choutin | 14:138af628d979 | 435 | |
sakanakuuun | 8:3bf4addaaedd | 436 | move(-(30 + k*dy), -(32 - k*dy)); |
choutin | 14:138af628d979 | 437 | |
sakanakuuun | 6:0aa97a99c9cb | 438 | if(dx>length) { |
sakanakuuun | 4:4c574be6325c | 439 | move(0, 0); |
sakanakuuun | 4:4c574be6325c | 440 | break; |
sakanakuuun | 4:4c574be6325c | 441 | } |
sakanakuuun | 4:4c574be6325c | 442 | } |
choutin | 14:138af628d979 | 443 | |
choutin | 13:4501c9202500 | 444 | wait(0.2); |
choutin | 14:138af628d979 | 445 | |
choutin | 13:4501c9202500 | 446 | red = 0; |
choutin | 13:4501c9202500 | 447 | } |
choutin | 14:138af628d979 | 448 | |
choutin | 14:138af628d979 | 449 | |
choutin | 13:4501c9202500 | 450 | void nxback300() |
choutin | 13:4501c9202500 | 451 | { |
choutin | 13:4501c9202500 | 452 | red = 1; |
choutin | 14:138af628d979 | 453 | |
choutin | 13:4501c9202500 | 454 | float k = 0.9; |
choutin | 13:4501c9202500 | 455 | int length, px, py, dx, dy; |
choutin | 14:138af628d979 | 456 | |
choutin | 13:4501c9202500 | 457 | update(); |
choutin | 14:138af628d979 | 458 | |
choutin | 13:4501c9202500 | 459 | px = coordinateX(); |
choutin | 13:4501c9202500 | 460 | py = coordinateY(); |
choutin | 14:138af628d979 | 461 | |
choutin | 13:4501c9202500 | 462 | length = 300; |
choutin | 14:138af628d979 | 463 | |
choutin | 13:4501c9202500 | 464 | turnrad(nearPi(coordinateTheta())); |
choutin | 14:138af628d979 | 465 | |
choutin | 13:4501c9202500 | 466 | while(1) { |
choutin | 13:4501c9202500 | 467 | update_np(); |
choutin | 13:4501c9202500 | 468 | dx = coordinateX() - px; |
choutin | 13:4501c9202500 | 469 | dy = coordinateY() - py; |
choutin | 14:138af628d979 | 470 | |
choutin | 13:4501c9202500 | 471 | if(dy>max_disorder) { |
choutin | 13:4501c9202500 | 472 | dy = max_disorder; |
choutin | 13:4501c9202500 | 473 | } else if(dy<-max_disorder) { |
choutin | 13:4501c9202500 | 474 | dy = -max_disorder; |
choutin | 13:4501c9202500 | 475 | } |
choutin | 14:138af628d979 | 476 | |
choutin | 13:4501c9202500 | 477 | move(-(30 - k*dy), -(32 + k*dy)); |
choutin | 14:138af628d979 | 478 | |
choutin | 13:4501c9202500 | 479 | if(abs(dx)>length) { |
choutin | 13:4501c9202500 | 480 | move(0, 0); |
choutin | 13:4501c9202500 | 481 | break; |
choutin | 13:4501c9202500 | 482 | } |
choutin | 13:4501c9202500 | 483 | } |
choutin | 14:138af628d979 | 484 | |
choutin | 13:4501c9202500 | 485 | wait(0.2); |
choutin | 14:138af628d979 | 486 | |
sakanakuuun | 7:7f1721542753 | 487 | red = 0; |
sakanakuuun | 4:4c574be6325c | 488 | } |
choutin | 14:138af628d979 | 489 | |
choutin | 13:4501c9202500 | 490 | void pyback300(int team) |
choutin | 13:4501c9202500 | 491 | { |
choutin | 13:4501c9202500 | 492 | if(team == 0) { |
choutin | 13:4501c9202500 | 493 | nyback300(1); |
choutin | 13:4501c9202500 | 494 | return; |
choutin | 13:4501c9202500 | 495 | } |
choutin | 14:138af628d979 | 496 | |
choutin | 13:4501c9202500 | 497 | red = 1; |
choutin | 14:138af628d979 | 498 | |
choutin | 13:4501c9202500 | 499 | float k = 0.9; |
choutin | 13:4501c9202500 | 500 | int length, px, py, dx, dy; |
choutin | 14:138af628d979 | 501 | |
choutin | 13:4501c9202500 | 502 | update(); |
choutin | 14:138af628d979 | 503 | |
choutin | 13:4501c9202500 | 504 | px = coordinateX(); |
choutin | 13:4501c9202500 | 505 | py = coordinateY(); |
choutin | 14:138af628d979 | 506 | |
choutin | 13:4501c9202500 | 507 | length = 300; |
choutin | 14:138af628d979 | 508 | |
choutin | 13:4501c9202500 | 509 | turnrad(PI/2 + nearPi(coordinateTheta() - PI/2)); |
choutin | 14:138af628d979 | 510 | |
choutin | 13:4501c9202500 | 511 | while(1) { |
choutin | 13:4501c9202500 | 512 | update_np(); |
choutin | 13:4501c9202500 | 513 | dx = coordinateX() - px; |
choutin | 13:4501c9202500 | 514 | dy = coordinateY() - py; |
choutin | 14:138af628d979 | 515 | |
choutin | 13:4501c9202500 | 516 | if(dx>max_disorder) { |
choutin | 13:4501c9202500 | 517 | dx = max_disorder; |
choutin | 13:4501c9202500 | 518 | } else if(dx<-max_disorder) { |
choutin | 13:4501c9202500 | 519 | dx = -max_disorder; |
choutin | 13:4501c9202500 | 520 | } |
choutin | 14:138af628d979 | 521 | |
choutin | 13:4501c9202500 | 522 | move(-(30 - k*dx), -(32 + k*dx)); |
choutin | 14:138af628d979 | 523 | |
choutin | 13:4501c9202500 | 524 | if(dy>length) { |
choutin | 13:4501c9202500 | 525 | move(0, 0); |
choutin | 13:4501c9202500 | 526 | break; |
choutin | 13:4501c9202500 | 527 | } |
choutin | 14:138af628d979 | 528 | |
choutin | 13:4501c9202500 | 529 | } |
choutin | 14:138af628d979 | 530 | |
choutin | 13:4501c9202500 | 531 | wait(0.2); |
choutin | 14:138af628d979 | 532 | |
choutin | 13:4501c9202500 | 533 | red = 0; |
choutin | 13:4501c9202500 | 534 | } |
choutin | 14:138af628d979 | 535 | |
choutin | 13:4501c9202500 | 536 | void nyback300(int team) |
choutin | 13:4501c9202500 | 537 | { |
choutin | 13:4501c9202500 | 538 | if(team == 0) { |
choutin | 13:4501c9202500 | 539 | pyback300(1); |
choutin | 13:4501c9202500 | 540 | return; |
choutin | 13:4501c9202500 | 541 | } |
choutin | 14:138af628d979 | 542 | |
choutin | 13:4501c9202500 | 543 | red = 1; |
choutin | 14:138af628d979 | 544 | |
choutin | 13:4501c9202500 | 545 | float k = 0.9; |
choutin | 13:4501c9202500 | 546 | int length, px, py, dx, dy; |
choutin | 14:138af628d979 | 547 | |
choutin | 13:4501c9202500 | 548 | update(); |
choutin | 14:138af628d979 | 549 | |
choutin | 13:4501c9202500 | 550 | px = coordinateX(); |
choutin | 13:4501c9202500 | 551 | py = coordinateY(); |
choutin | 14:138af628d979 | 552 | |
choutin | 13:4501c9202500 | 553 | length = 300; |
choutin | 14:138af628d979 | 554 | |
choutin | 13:4501c9202500 | 555 | turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2)); |
choutin | 14:138af628d979 | 556 | |
choutin | 13:4501c9202500 | 557 | while(1) { |
choutin | 15:39c7e97b37c4 | 558 | update();//npの部分を変更した |
choutin | 13:4501c9202500 | 559 | dx = coordinateX() - px; |
choutin | 13:4501c9202500 | 560 | dy = coordinateY() - py; |
choutin | 14:138af628d979 | 561 | |
choutin | 13:4501c9202500 | 562 | if(dx>max_disorder) { |
choutin | 13:4501c9202500 | 563 | dx = max_disorder; |
choutin | 13:4501c9202500 | 564 | } else if(dx<-max_disorder) { |
choutin | 13:4501c9202500 | 565 | dx = -max_disorder; |
choutin | 13:4501c9202500 | 566 | } |
choutin | 14:138af628d979 | 567 | |
choutin | 13:4501c9202500 | 568 | move(-(30 - k*dx), -(32 + k*dx)); |
choutin | 14:138af628d979 | 569 | |
choutin | 13:4501c9202500 | 570 | if(abs(dy)>length) { |
choutin | 13:4501c9202500 | 571 | move(0, 0); |
choutin | 13:4501c9202500 | 572 | break; |
choutin | 13:4501c9202500 | 573 | } |
choutin | 13:4501c9202500 | 574 | } |
choutin | 14:138af628d979 | 575 | |
choutin | 13:4501c9202500 | 576 | wait(0.2); |
choutin | 14:138af628d979 | 577 | |
choutin | 13:4501c9202500 | 578 | red = 0; |
choutin | 13:4501c9202500 | 579 | } |
choutin | 14:138af628d979 | 580 | |
choutin | 13:4501c9202500 | 581 | float nearPi(float rad) |
choutin | 13:4501c9202500 | 582 | { |
choutin | 13:4501c9202500 | 583 | float npi = 0; |
choutin | 14:138af628d979 | 584 | |
choutin | 13:4501c9202500 | 585 | while(1) { |
choutin | 13:4501c9202500 | 586 | if(rad > npi + PI) |
choutin | 13:4501c9202500 | 587 | npi += 2*PI; |
choutin | 13:4501c9202500 | 588 | else if(rad < npi - PI) |
choutin | 13:4501c9202500 | 589 | npi -= 2*PI; |
choutin | 13:4501c9202500 | 590 | else |
choutin | 13:4501c9202500 | 591 | return npi; |
choutin | 13:4501c9202500 | 592 | } |
choutin | 13:4501c9202500 | 593 | } |
choutin | 14:138af628d979 | 594 | |
choutin | 13:4501c9202500 | 595 | float giveatan(int targetx,int targety) |
choutin | 13:4501c9202500 | 596 | { |
choutin | 13:4501c9202500 | 597 | int x,y; |
choutin | 13:4501c9202500 | 598 | float phi; |
choutin | 13:4501c9202500 | 599 | update(); |
choutin | 13:4501c9202500 | 600 | x = coordinateX(); |
choutin | 13:4501c9202500 | 601 | y = coordinateY(); |
choutin | 14:138af628d979 | 602 | |
choutin | 13:4501c9202500 | 603 | if(targetx - x == 0) { |
choutin | 13:4501c9202500 | 604 | if(targety - y > 0) |
choutin | 13:4501c9202500 | 605 | return PI/2; |
choutin | 13:4501c9202500 | 606 | else if(targety - y < 0) |
choutin | 13:4501c9202500 | 607 | return -PI/2; |
choutin | 13:4501c9202500 | 608 | else |
choutin | 13:4501c9202500 | 609 | return 0; |
choutin | 13:4501c9202500 | 610 | } |
choutin | 14:138af628d979 | 611 | |
choutin | 13:4501c9202500 | 612 | phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得 |
choutin | 13:4501c9202500 | 613 | if(targetx - x < 0) { |
choutin | 13:4501c9202500 | 614 | if(targety - y > 0) |
choutin | 13:4501c9202500 | 615 | phi += PI; |
choutin | 13:4501c9202500 | 616 | else if(targety - y < 0) |
choutin | 13:4501c9202500 | 617 | phi -= PI; |
choutin | 13:4501c9202500 | 618 | } |
choutin | 14:138af628d979 | 619 | |
choutin | 13:4501c9202500 | 620 | return phi; |
choutin | 14:138af628d979 | 621 | } |
choutin | 14:138af628d979 | 622 | |
choutin | 14:138af628d979 | 623 | void commandMove(int cx0 = 17, int cx1 = 17, int cx2 = 17) |
choutin | 14:138af628d979 | 624 | { |
choutin | 14:138af628d979 | 625 | if(cx0 >= 10) { |
choutin | 14:138af628d979 | 626 | pmove2(150, 900); |
choutin | 14:138af628d979 | 627 | return; |
choutin | 14:138af628d979 | 628 | } |
choutin | 14:138af628d979 | 629 | yokoMove(cx0 * 100 + 300, true); |
choutin | 14:138af628d979 | 630 | close_hand(); |
choutin | 14:138af628d979 | 631 | wait(0.3); |
choutin | 14:138af628d979 | 632 | lift(); |
choutin | 14:138af628d979 | 633 | open_hand(); |
choutin | 14:138af628d979 | 634 | if(cx1 >= 10) { |
choutin | 14:138af628d979 | 635 | pmove2(150, 900); |
choutin | 14:138af628d979 | 636 | return; |
choutin | 14:138af628d979 | 637 | } |
choutin | 14:138af628d979 | 638 | yokoMove(cx1 * 100 + 300, false); |
choutin | 14:138af628d979 | 639 | close_hand(); |
choutin | 14:138af628d979 | 640 | wait(0.3); |
choutin | 14:138af628d979 | 641 | lift(); |
choutin | 14:138af628d979 | 642 | open_hand(); |
choutin | 14:138af628d979 | 643 | if(cx2 >= 10) { |
choutin | 14:138af628d979 | 644 | pmove2(150, 900); |
choutin | 14:138af628d979 | 645 | return; |
choutin | 14:138af628d979 | 646 | } |
choutin | 14:138af628d979 | 647 | yokoMove(cx2 * 100 + 300, true); |
choutin | 14:138af628d979 | 648 | pmove2(150, 900); |
choutin | 14:138af628d979 | 649 | } |
choutin | 14:138af628d979 | 650 | |
choutin | 14:138af628d979 | 651 | void commandMoveEnemy(int cx0 = 17, int cx1 = 17, int cx2 = 17) |
choutin | 14:138af628d979 | 652 | { |
choutin | 14:138af628d979 | 653 | pmove2(1200, 1200); |
choutin | 14:138af628d979 | 654 | if(cx0 >= 10) return; |
choutin | 14:138af628d979 | 655 | yokoMove(-cx0 * 100 + 2700, false); |
choutin | 14:138af628d979 | 656 | close_hand(); |
choutin | 14:138af628d979 | 657 | wait(0.3); |
choutin | 14:138af628d979 | 658 | lift(); |
choutin | 14:138af628d979 | 659 | open_hand(); |
choutin | 14:138af628d979 | 660 | if(cx1 >= 10) return; |
choutin | 14:138af628d979 | 661 | yokoMove(-cx1 * 100 + 2700, true); |
choutin | 14:138af628d979 | 662 | pmove2(150, 1100); |
choutin | 14:138af628d979 | 663 | } |
choutin | 14:138af628d979 | 664 | |
choutin | 14:138af628d979 | 665 | void yokoMove(int cx, bool Left) |
choutin | 14:138af628d979 | 666 | { |
choutin | 14:138af628d979 | 667 | if(Left) { |
choutin | 15:39c7e97b37c4 | 668 | pmove2(cx, 100); |
choutin | 15:39c7e97b37c4 | 669 | pmove(cx, 1100); |
choutin | 14:138af628d979 | 670 | } else { |
choutin | 15:39c7e97b37c4 | 671 | pmove2(cx, 1100); |
choutin | 15:39c7e97b37c4 | 672 | pmove(cx, 100); |
choutin | 14:138af628d979 | 673 | } |
choutin | 15:39c7e97b37c4 | 674 | } |
choutin | 15:39c7e97b37c4 | 675 | |
choutin | 15:39c7e97b37c4 | 676 | void moveTriangle(int c0, int c1, int c2, int c3) |
choutin | 15:39c7e97b37c4 | 677 | { |
choutin | 15:39c7e97b37c4 | 678 | pmove2(c0 * 100, c1 * 100); |
choutin | 15:39c7e97b37c4 | 679 | pmove2(c2 * 100, c3 * 100); |
choutin | 15:39c7e97b37c4 | 680 | pmove2(0, 900); |
choutin | 13:4501c9202500 | 681 | } |