static void joust(void *picked_winner_as_vp, lights_t *l, unsigned pin)
{
unsigned picked_winner = (unsigned) picked_winner_as_vp;
unsigned want_winner = picked_winner == WIN_2_PIN;
unsigned winner_id = random_number_in_range(0, 1);
stop_stop(pick_stop);
fprintf(stderr, "Starting joust\n");
lights_blink_one(l, pin);
track_play_asynchronously(jousting, stop);
motor_forward(0, 1);
motor_forward(1, 1);
ms_sleep(WIN_MS);
stop_stop(stop);
track_play_asynchronously(crash, stop);
motor_forward(0, TROT_DUTY);
motor_forward(1, TROT_DUTY);
ms_sleep(TROT_MS);
stop_stop(stop);
motor_stop(0);
motor_stop(1);
fprintf(stderr, "wanted %d got %d\n", want_winner, winner_id);
if (winner_id == 0) {
wb_set(MOTOR_BANK, WINNER_LIGHT_1, 1);
} else {
wb_set(MOTOR_BANK, WINNER_LIGHT_2, 1);
}
if (winner_id == want_winner) {
track_play(winner);
} else {
track_play(loser);
}
ms_sleep(1000);
wb_set(MOTOR_BANK, WINNER_LIGHT_1, 0);
wb_set(MOTOR_BANK, WINNER_LIGHT_2, 0);
lights_off(l);
track_play_asynchronously(beeping, stop);
go_to_start_position();
stop_stop(stop);
}
void attack() {
int left, right;
clear_screen();
print_string("Attack");
motor_forward();
while(1) {
left = read_ir_sensor(LEFT);
right = read_ir_sensor(RIGHT);
if (left >= BLACK && right < BLACK) {
first_led_to_hit = LEFT;
break;
}
else if (right >= BLACK && left < BLACK) {
first_led_to_hit = RIGHT;
break;
}
else if (left >= BLACK && right >= BLACK) {
first_led_to_hit = 47;
break;
}
}
motor_stop();
state = &straighten_out;
}
void find_target() {
clear_screen();
print_string("Targg");
motor_spin_left();
left_count = 0;
while (get_range() > MAX_RANGE && left_count < 5 * NINETY_DEGREES) {
clear_screen();
print_num(get_range());
_delay_ms(20);
}
motor_stop();
if (left_count >= 5 * NINETY_DEGREES) {
motor_forward();
left_count = 0;
while (left_count < 90);
motor_stop();
}
else {
motor_spin_right();
_delay_ms(20);
motor_stop();
state = &attack;
}
}
int
main(int argc, char **argv)
{
if (wb_init() < 0) {
fprintf(stderr, "Failed to initialize wb\n");
exit(1);
}
pthread_mutex_init(&lock, NULL);
stop = stop_new();
pick_stop = stop_new();
stop_stopped(pick_stop);
winner = track_new_fatal("jousters_winner.wav");
loser = track_new_fatal("jousters_loser.wav");
jousting = track_new_fatal("jousters_jousting.wav");
crash = track_new_fatal("jousters_crash.wav");
beeping = track_new_fatal("jousters_beeping.wav");
wb_set_pull_up(HOME_1_PIN, WB_PULL_UP_UP);
wb_set_pull_up(WIN_1_PIN, WB_PULL_UP_UP);
wb_set_pull_up(HOME_2_PIN, WB_PULL_UP_UP);
wb_set_pull_up(WIN_2_PIN, WB_PULL_UP_UP);
motor_forward(0, 0.5);
motor_forward(1, 0.5);
ms_sleep(1000);
motor_stop(0);
motor_stop(1);
ms_sleep(500);
go_to_start_position();
animation_main(stations);
return 0;
}
void loop() {
if (Serial.available()) {
String readString = "";
while (Serial.available()) {
delay(3);
if (Serial.available() > 0) {
char c = Serial.read();
readString += c;
}
}
if (readString.charAt(0) == 'M') {
switch (readString.charAt(1)) {
case 'f' : case 'F':
motor_forward();
break;
case 'b': case 'B':
motor_backward();
break;
case 'l': case 'L':
motor_turnLeft();
break;
case 'r': case 'R':
motor_turnRight();
break;
case 's': case 'S':
motor_stop();
break;
case 'm': case 'A':
isUltra = !isUltra;
break;
case 'p': case 'P':
char c = readString.charAt(2);
ChangeSpeed((c - '0') * factor + minSpeed);
break;
}
}
} else {
if (isUltra) {
//ultraCarProcess();
}
else {
//motor_stop();
}
}
}
int window_fan() {
rcc_config();
delay_config();
led_debug_config();
motor_config();
led_blue_off();
led_green_off();
servo_config();
servo_set_pos(0);
servo_start();
u32 i;
u32 from = 0;
u32 to = 180;
u32 delay = 2000;
while(1) {
motor_forward();
led_blue_on();
led_green_off();
for(i=from; i<to; i++) {
servo_set_pos(i);
delay_ms(delay);
}
led_blue_off();
led_green_on();
for(i=to; i>from; i--) {
servo_set_pos(i);
delay_ms(delay);
}
motor_stop();
delay_ms(10000);
}
}
int reader_test() {
rcc_config();
led_debug_config();
motor_config();
reed_config();
led_blue_off();
led_green_off();
uint8_t i;
while(1) {
// forward
motor_forward();
led_blue_on();
reed_delay_left();
// stop
motor_stop();
led_blue_off();
bigDelay();
bigDelay();
bigDelay();
// backward
motor_back();
led_green_on();
reed_delay_right();
// stop
motor_stop();
led_green_off();
bigDelay();
bigDelay();
bigDelay();
}
}
/**
* Controller::goToPosition
*
* @param double x desired waypoint x co-ord
* @param double y desired waypoing y co-ord
* @param double fPosXStated believed current x co-ord
* @param double fPosYStated believed current y co-ord
*
* @return void
*/
void Controller::goToPosition(double x, double y, double fPosXStated, double fPosYStated)
{
double fTargetVectorMagnitude; // current distance between where we think we are and the waypoint
double fTargetVectorMagnitudeLast = 0; // last distance between where we think we were and the waypoint
double fTargetVectorMagnitudeInitial = 0; // initial distance between where we think we are and the waypoint
double fTargetVectorMagnitudeAtStartOfDrift; // if we begin to get further from the target (rather than closer, perhaps due to a turning circle) what was our distance to the target when this started?
unsigned int nConsecutiveIncreasingDistance = 0; // number of times the distance has increased rather than decrease (used to stop out of controlness)
bool bApproachingTarget = false; // once we start approaching the target don't forget it
double fVelocityLeft = 0, fVelocityRight = 0; // current velocity of left and right wheels
Led ledHealth(1), ledProximity(3);
ledHealth.off();
ledProximity.off();
// Reset the distance counters that are used in the PID loop, they are relative to our last (this) waypoint
resetDistance();
_logger->notice("goToPosition: --- START ---\nGoing to position (%.2f, %.2f) from current position (%.2f, %.2f) and heading [%.2f] ...", x, y, _currentPose.x, _currentPose.y, _currentPose.heading);
_fPosXRef = x;
_fPosYRef = y;
// We need to keep our heading as close to this reference heading as possible to reach the waypoint
_fHeadingRef = getHeading(_fPosXRef, _fPosYRef, _currentPose.x, _currentPose.y, _currentPose.heading);
// _fHeadingRef = getHeading(_fPosXRef, _fPosYRef, fPosXStated, fPosYStated, _fHeadingCurrent);
_logger->notice("goToPosition: new heading required is [%.2f]", _fHeadingRef);
// Start (and reset) the odometry thread
_odo->reset();
int iteration = 0;
bool bFirstIteration = true;
struct timeval tvNow, tvLast;
int nsec;
unsigned long delayms, totaldelayms = 0; // time since last iteration and total time in this waypoint
gettimeofday(&tvLast, NULL);
while (true)
{
/**
* Recalculate our reference heading every now and again as our current position changes.
*/
if (iteration % 10 == 0)
{
_logger->notice("goToPosition: recalculating heading based on current position...");
_fHeadingRef = getHeading(_fPosXRef, _fPosYRef, _currentPose.x, _currentPose.y, _currentPose.heading);
_logger->notice("new heading is %.2f", _fHeadingRef);
}
gettimeofday(&tvNow, NULL);
if (tvNow.tv_usec < tvLast.tv_usec)
{
nsec = (tvLast.tv_usec - tvNow.tv_usec) / 1000000 + 1;
tvLast.tv_usec -= 1000000 * nsec;
tvLast.tv_sec += nsec;
}
if (tvNow.tv_usec - tvLast.tv_usec > 1000000)
{
nsec = (tvLast.tv_usec - tvNow.tv_usec) / 1000000;
tvLast.tv_usec += 1000000 * nsec;
tvLast.tv_sec -= nsec;
}
// How long did we sleep since the last iteration? Required for derivatives.
delayms = ((tvNow.tv_sec - tvLast.tv_sec) * 1000) + ((tvNow.tv_usec - tvLast.tv_usec) / 1000);
totaldelayms += delayms; // total time in this waypoint segment
_totalTime += delayms; // total time transiting altogether
tvLast = tvNow;
_currentPose.timestamp = _totalTime;
// How long have we slept? This is required for our integral and derivative PID values.
double dt = delayms / 1000.0;
ledHealth.strobe();
_logger->notice("goToPosition: %lu --- ITERATION %d --- \nreference: (%.2f, %.2f, distance: %.2f) at heading %.2f (total runtime: %lu) (dt: %.2f)", delayms, iteration, _fPosXRef, _fPosYRef, fTargetVectorMagnitudeInitial, _fHeadingRef, totaldelayms, dt);
// How far has each wheel travelled? This is total distance since odo reset (start of waypoint).
if (_bSimulation)
{
// Don't use the odometer, see what our model predicts. This can be used to determine how accurate the odometer and drive train is.
_fDistLeft += dt * fVelocityLeft;
_fDistRight += dt * fVelocityRight;
}
else
{
_odo->getDistance(&_fDistLeft, &_fDistRight);
}
_fDistTotal = (_fDistLeft + _fDistRight) / 2.0;
_logger->notice("goToPosition: distL[%.2f] distR[%.2f] dist[%.2f] distPrev[%.2f]", _fDistLeft, _fDistRight, _fDistTotal, _fDistTotalPrev);
// How far have we travelled in this last iteration?
double fDistDelta = _fDistTotal - _fDistTotalPrev;
double fDistLeftDelta = _fDistLeft - _fDistLeftPrev;
double fDistRightDelta = _fDistRight - _fDistRightPrev;
// Position has changed based on the distance travelled at the previous heading
_currentPose.x += fDistDelta * cos(_currentPose.heading);
_currentPose.y += fDistDelta * sin(_currentPose.heading);
// Update the heading as it has changed based on the distance travelled too
_currentPose.heading += ((fDistRightDelta - fDistLeftDelta) / CONTROLLER_WHEELBASE);
// Ensure our heading remains sane
_currentPose.heading = atan2(sin(_currentPose.heading), cos(_currentPose.heading));
_fDistTotalPrev = _fDistTotal;
_fDistLeftPrev = _fDistLeft;
_fDistRightPrev = _fDistRight;
// What's the error between our required heading and our heading?
double fHeadingErrorRaw = _fHeadingRef - _currentPose.heading;
_fHeadingError = atan2(sin(fHeadingErrorRaw), cos(fHeadingErrorRaw));
// How fast should we proceed forward? (Anything below 5 results in non-movement due to inertia)
double fForwardVelocity = 9.0; // 5.0 works as well but undershoots
// What is the magnitude of the vector between us and our target?
fTargetVectorMagnitude = sqrt(((_fPosXRef - _currentPose.x)*(_fPosXRef - _currentPose.x)) + ((_fPosYRef - _currentPose.y)*(_fPosYRef - _currentPose.y)));
if (bFirstIteration)
{
fTargetVectorMagnitudeInitial = fTargetVectorMagnitude;
}
_logger->notice("goToPosition: heading[%.4f, e: %.6f] posX[%.2f] posY[%.2f] distToTarget[%.2f] distToTargetLast[%.2f]", _currentPose.heading, _fHeadingError, _currentPose.x, _currentPose.y, fTargetVectorMagnitude, fTargetVectorMagnitudeLast);
/**
* @todo: Come up with a better way of determining whether we should consider ourself "close enough" to the waypoint.
*/
if ( ! bFirstIteration)
{
if (fTargetVectorMagnitude >= fTargetVectorMagnitudeLast)
{
if (nConsecutiveIncreasingDistance == 0)
{
fTargetVectorMagnitudeAtStartOfDrift = fTargetVectorMagnitude;
}
nConsecutiveIncreasingDistance++;
double fDistanceCoveredSinceStartOfDrift = fTargetVectorMagnitude - fTargetVectorMagnitudeAtStartOfDrift;
if (fDistanceCoveredSinceStartOfDrift >= (0.25 * fTargetVectorMagnitudeInitial))
// if (nConsecutiveIncreasingDistance > MAX_ITERATIONS_OF_INCREASING_TARGET_VECTOR_BEFORE_TERMINATION)
{
_logger->notice("goToPosition: Distance to target has increased!\ngoToPosition: --- END ABNORMAL ---\n");
break;
}
}
else
{
nConsecutiveIncreasingDistance = 0;
}
}
if (fTargetVectorMagnitude < 10 || bApproachingTarget)
{
bApproachingTarget = true;
ledProximity.on();
_logger->notice("goToPosition: Approaching target, slowing down.");
fForwardVelocity = 3.0; // 3.0 works as well, safer
}
// _logger->notice("goToPosition: x(%.2f)\ty(%.2f)\ttheta(%.2f)", _fPosXCurrent, _fPosYCurrent, _fHeadingCurrent);
if (fTargetVectorMagnitude <= 5)
{
_logger->notice("goToPosition: You have arrived at your destination!\ngoToPosition: --- END ---\n");
_dotLogPosition->log((_totalTime / 1000.0), _currentPose.x, _currentPose.y, DotLog::DotLogPositionColour::RED, true);
break;
}
else
{
_dotLogPosition->log((_totalTime / 1000.0), _currentPose.x, _currentPose.y, bApproachingTarget ? DotLog::DotLogPositionColour::RED : DotLog::DotLogPositionColour::BLACK);
}
fTargetVectorMagnitudeLast = fTargetVectorMagnitude;
// if (fabs(fPosXCurrent - fPosXRef) < (CONTROLLER_WHEELBASE/2.0) && fabs(fPosYCurrent - fPosYRef) < (CONTROLLER_WHEELBASE/2.0))
// {
// printf("You have arrived at your destination (non-vector)!\n");
// break;
// }
// Maintain the PID variables
double fHeadingErrorDerivative = (_fHeadingError - _fHeadingErrorPrev) / dt;
_fHeadingErrorIntegral += (_fHeadingError * dt);
_fHeadingErrorPrev = _fHeadingError;
double pidP = CONTROLLER_PID_PROPORTIONAL * _fHeadingError;
double pidI = CONTROLLER_PID_INTEGRAL * _fHeadingErrorIntegral;
double pidD = CONTROLLER_PID_DERIVATIVE * fHeadingErrorDerivative;
// PID, this gives us the control signal, u, this is required angular velocity
double u = pidP + pidI + pidD;
// Angular velocity gives us new wheel velocities. We assume a constant forward velocity for simplicity.
fVelocityRight = ((2.0*fForwardVelocity) + (u*CONTROLLER_WHEELBASE)) / (2.0*CONTROLLER_WHEELRADIUS); // cm/s
fVelocityLeft = ((2.0*fForwardVelocity) - (u*CONTROLLER_WHEELBASE)) / (2.0*CONTROLLER_WHEELRADIUS); // cm/s
_logger->notice("goToPosition: P[%.6f] I[%.6f] D[%.6f] u[%.6f] -> required velocities (left,right) are (%.2f,%.2f)", pidP, pidI, pidD, u, fVelocityLeft, fVelocityRight);
// These velocities are relative, work out the required velocity per wheel.
// float vLeft = (fVelocityLeft / (fabs(fVelocityLeft) + fabs(fVelocityRight))) * 100.0;
// float vRight = (fVelocityRight / (fabs(fVelocityLeft) + fabs(fVelocityRight))) * 100.0;
//
// int dLeft = static_cast<int>(fabs(vLeft));
// int dRight = static_cast<int>(fabs(vRight));
//
// printf("Required relative velocities (left,right) are (%.2f,%.2f) -> (%d,%d)\n", vLeft, vRight, dLeft, dRight);
int nLeftPWM = convertVelocityToPWMPercentage(true, fVelocityLeft);
int nRightPWM = convertVelocityToPWMPercentage(false, fVelocityRight);
// Apply the new velocities to the motors
if ( ! _bSimulation)
{
if (fVelocityLeft < 0.0)
{
motor_reverse(MOTOR_LEFT, pwm_speed(nLeftPWM));
}
else
{
motor_forward(MOTOR_LEFT, pwm_speed(nLeftPWM));
}
if (fVelocityRight < 0.0)
{
motor_reverse(MOTOR_RIGHT, pwm_speed(nRightPWM));
}
else
{
motor_forward(MOTOR_RIGHT, pwm_speed(nRightPWM));
}
}
usleep(50000); // 50ms
// usleep(300000); // 300 ms
// usleep(1000000); // 1s
/**
* time passes ... wheels respond to new control signal and begin moving at new velocities
*
* These wheel velocities turn the wheels and influence the distance travelled and heading
* which is recalculated in the next iteration.
*/
bFirstIteration = false;
iteration++;
}
bot_stop();
ledHealth.off();
}
