Revision dfb90929 arm/main.c
| b/arm/main.c | ||
|---|---|---|
| 19 | 19 |
// the number of steps to jump up when it is trying to speed up |
| 20 | 20 |
#define SPEED_UP_STEPS (50) |
| 21 | 21 |
|
| 22 |
// the amount to time to spend in the 'speedup' phase before |
|
| 23 |
// entering a 'cooldown' phase to realign the motor and stuff |
|
| 24 |
#define SPEEDUP_TIMEOUT ((uint32_t)10.0e8) |
|
| 25 |
#define SPEEDUP_TIMEOUT_COOLDOWN ((uint32_t)0.080e8) |
|
| 26 |
|
|
| 22 | 27 |
// the max speed the pipe will try to accelerate to |
| 23 | 28 |
const int32_t RETURN_SPEED_UPPER = 3200; |
| 24 | 29 |
// when the pipe reaches this speed, it will enter acceleration mode. |
| ... | ... | |
| 35 | 40 |
const int32_t BRAKING_UPPER_SLOWDOWN = 800; |
| 36 | 41 |
|
| 37 | 42 |
|
| 43 |
|
|
| 44 |
|
|
| 38 | 45 |
enum states {WAIT,
|
| 39 | 46 |
ROLLING_AWAY, |
| 40 | 47 |
TRACK_RETURN, |
| 41 | 48 |
TRACK_SPEEDUP, |
| 42 | 49 |
PUSH_BACK_TRANSITION, |
| 43 |
BRAKING}; |
|
| 50 |
BRAKING, |
|
| 51 |
TRACK_SPEEDUP_TIMEOUT}; |
|
| 44 | 52 |
|
| 45 | 53 |
|
| 46 | 54 |
// Global data structures |
| ... | ... | |
| 137 | 145 |
|
| 138 | 146 |
// indicates how far 'ahead' or 'behind' the weight is from the ideal |
| 139 | 147 |
int32_t stepsOffset = 0; |
| 148 |
int32_t timer_reading = 0; |
|
| 140 | 149 |
|
| 141 | 150 |
while(1) |
| 142 | 151 |
{
|
| ... | ... | |
| 190 | 199 |
// exits to: PUSH_BACK_TRANSITION - when the speed drops bellow the threshold |
| 191 | 200 |
*/ |
| 192 | 201 |
case ROLLING_AWAY: |
| 193 |
A4988_turnMotorOff(); |
|
| 202 |
//A4988_turnMotorOff();
|
|
| 194 | 203 |
// tracking code |
| 195 |
if( abs(degreesSpunSinceLast) > 2.0 ) |
|
| 196 |
{
|
|
| 197 |
int turns = degreesSpunSinceLast / 1.8; |
|
| 204 |
//if( abs(degreesSpunSinceLast) > 2.0 )
|
|
| 205 |
//{
|
|
| 206 |
//int turns = degreesSpunSinceLast / 1.8;
|
|
| 198 | 207 |
|
| 199 | 208 |
//A4988_rotRel(turns, ROLLING_AWAY_MOTOR_DELAY); |
| 200 | 209 |
|
| 201 |
__asm("cpsid i");
|
|
| 202 |
degreesSpunSinceLast -= 1.8 * turns; |
|
| 203 |
__asm("cpsie i");
|
|
| 204 |
} |
|
| 210 |
//__asm("cpsid i");
|
|
| 211 |
//degreesSpunSinceLast -= 1.8 * turns;
|
|
| 212 |
//__asm("cpsie i");
|
|
| 213 |
//}
|
|
| 205 | 214 |
|
| 206 | 215 |
// exits state when leaving speed is too low |
| 207 | 216 |
if( abs(g.x) < LEAVING_SPEED_MIN) |
| ... | ... | |
| 230 | 239 |
// TRACK_SPEEDUP - if it needs to speedup, rotates motor and goes here. |
| 231 | 240 |
*/ |
| 232 | 241 |
case PUSH_BACK_TRANSITION: |
| 233 |
A4988_turnMotorOff(); |
|
| 242 |
//A4988_turnMotorOff();
|
|
| 234 | 243 |
// if it entered this state in order to speed up |
| 235 | 244 |
if( abs(g.x) < RETURN_SPEED_LOWER ) |
| 236 | 245 |
{
|
| 237 | 246 |
A4988_turnMotorOn(); |
| 238 | 247 |
|
| 248 |
// set the reading so it will know how long it has been |
|
| 249 |
// in that state |
|
| 250 |
timer_reading = current_time; |
|
| 251 |
|
|
| 239 | 252 |
// turn the motor in the direction it need to speed up in. |
| 240 | 253 |
// (the direction to get back to the home area) |
| 241 | 254 |
if(integrated < 0.0) |
| ... | ... | |
| 249 | 262 |
} |
| 250 | 263 |
else |
| 251 | 264 |
{
|
| 265 |
A4988_turnMotorOff(); |
|
| 252 | 266 |
// it must be going fast enough, so it will go to TRACK_ROLLING |
| 253 | 267 |
cur_state = TRACK_RETURN; |
| 254 | 268 |
} |
| ... | ... | |
| 265 | 279 |
// BRAKE - if while speeding up it reaches home |
| 266 | 280 |
*/ |
| 267 | 281 |
case TRACK_SPEEDUP: |
| 268 |
A4988_turnMotorOn(); |
|
| 282 |
//A4988_turnMotorOn();
|
|
| 269 | 283 |
// tracking phase |
| 270 | 284 |
if( abs(degreesSpunSinceLast) > 2.0 ) |
| 271 | 285 |
{
|
| ... | ... | |
| 280 | 294 |
// this means it is close enough to home for the pipe to feel 'safe' |
| 281 | 295 |
if(abs(integrated) < HOME_THRESHOLD_CLOSE) |
| 282 | 296 |
{
|
| 283 |
//A4988_turnMotorOff(); |
|
| 284 | 297 |
A4988_rotRel(-stepsOffset, ROLLING_BACK_MOTOR_DELAY); |
| 285 | 298 |
stepsOffset = 0; |
| 286 | 299 |
cur_state = BRAKING; |
| ... | ... | |
| 289 | 302 |
else if(abs(g.x) > RETURN_SPEED_UPPER) |
| 290 | 303 |
{
|
| 291 | 304 |
A4988_turnMotorOff(); |
| 292 |
//A4988_rotRel(-stepsOffset, ROLLING_BACK_MOTOR_DELAY); |
|
| 293 | 305 |
stepsOffset = 0; |
| 294 | 306 |
cur_state = PUSH_BACK_TRANSITION; |
| 295 | 307 |
} |
| 308 |
// check to see if it has been in this state continuously too long |
|
| 309 |
else if(timer_reading - current_time > SPEEDUP_TIMEOUT) |
|
| 310 |
{
|
|
| 311 |
A4988_turnMotorOff(); |
|
| 312 |
timer_reading = current_time; |
|
| 313 |
cur_state = TRACK_SPEEDUP_TIMEOUT; |
|
| 314 |
} |
|
| 315 |
|
|
| 296 | 316 |
// otherwise, it should retain the current state of speeding up |
| 297 | 317 |
break; |
| 298 | 318 |
|
| ... | ... | |
| 384 | 404 |
|
| 385 | 405 |
A4988_rotRel(stepsOffset, ROLLING_BACK_MOTOR_DELAY); |
| 386 | 406 |
|
| 387 |
//cur_state = TRACK_SPEEDUP; |
|
| 388 | 407 |
} |
| 389 |
// I thought it would be cool to try to have the braking weight |
|
| 390 |
// 'drift' down as it got slower and slower. This code is supposed |
|
| 391 |
// to do that by adjusting the stepsOffset depending on the rotational |
|
| 392 |
// speed. This code is not necessary, but it might look cool |
|
| 393 |
// else if(abs(g.x) < BRAKING_UPPER_SLOWDOWN // already know abs(g.x) > BRAKING_MIN |
|
| 394 |
// && abs(stepsOffset) != 30 |
|
| 395 |
// ) |
|
| 396 |
// {
|
|
| 397 |
// if(stepsOffset > 50) |
|
| 398 |
// {
|
|
| 399 |
// stepsOffset = -30; |
|
| 400 |
// A4988_rotRel(20, DELAY); |
|
| 401 |
// } |
|
| 402 |
// else |
|
| 403 |
// {
|
|
| 404 |
// stepsOffset = 30; |
|
| 405 |
// A4988_rotRel(-20, DELAY); |
|
| 406 |
// } |
|
| 407 |
|
|
| 408 |
//int32_t testedStepOffset = |
|
| 409 |
// ((float) 40)/(BRAKING_UPPER_SLOWDOWN - BRAKING_MINIMUM)*(-g.x) + 10; |
|
| 410 |
//if(testedStepOffset != stepsOffset) |
|
| 411 |
//{
|
|
| 412 |
// A4988_rotRel(-(stepsOffset - testedStepOffset), DELAY); |
|
| 413 |
// stepsOffset = testedStepOffset; |
|
| 414 |
//} |
|
| 415 |
// } |
|
| 408 |
break; |
|
| 409 |
|
|
| 410 |
/* TRACK_SPEEDUP_TIMEOUT: |
|
| 411 |
// This state happens when it has been speeding up for a long time. |
|
| 412 |
// when in this state is just has to wait for some time before exiting. |
|
| 413 |
// The hope is that it will have the motor drop to be perpendicular to |
|
| 414 |
// the ground, so the speedup phase can have a fresh start. |
|
| 415 |
// NOTE: it will return to TRACK_RETURN. If it is going very slow |
|
| 416 |
// (which is expected) it will then move back into PUSHBACK_TRANSITION. |
|
| 417 |
// if it somehow magically found itself in the home state it will figure |
|
| 418 |
// that out at that time. |
|
| 419 |
// |
|
| 420 |
// ENTERS FROM: TRACK_SPEEDUP - after it has spent a long time in that |
|
| 421 |
// section continuously |
|
| 422 |
// EXITS TO: TRACK_RETURN - after it has 'cooled down' in this state |
|
| 423 |
*/ |
|
| 424 |
case TRACK_SPEEDUP_TIMEOUT: |
|
| 425 |
if(timer_reading - current_time > SPEEDUP_TIMEOUT_COOLDOWN) |
|
| 426 |
cur_state = TRACK_RETURN; |
|
| 416 | 427 |
break; |
| 417 | 428 |
|
| 418 | 429 |
// DEFAULT |
Also available in: Unified diff