int pointTurn(float targetHeading)
{
brake();
float currentHeading = (float)game.coords[0].theta/2047*180; //NEED TO CONVERT TO DEGREES
float deltaHeading;
if(targetHeading > currentHeading)
deltaHeading = targetHeading - currentHeading;
else
deltaHeading = currentHeading - targetHeading;
printf("The deltaHeading is: %f\n", deltaHeading);
float speed;
float timeStart = get_time();
printf("Start Time: %d\n", timeStart);
while (deltaHeading > TURNTOLERANCE)
{
float timeNow = get_time();
printf("Current Time: %d\n", timeNow);
if(timeElapsed(timeNow, timeStart) < TURNTIMEOUT)
{
speed = speedLimit(KT*deltaHeading);
printf("Speed: %f\n", speed);
if (targetHeading > currentHeading + TURNTOLERANCE)
{
setMotorsVelocity(-speed, speed);
printf("Trying to turn left\n");
}
else if (targetHeading < currentHeading - TURNTOLERANCE)
{
setMotorsVelocity(speed, -speed);
printf("Trying to turn right\n");
}
}
else
{
printf("Breaking from turning");
break;
}
update();
currentHeading = (float)game.coords[0].theta/2047*180; //NEED TO CONVERT TO DEGREES
if(targetHeading > currentHeading)
{
deltaHeading = targetHeading - currentHeading;
}
else
{
deltaHeading = currentHeading - targetHeading;
}
}
brake();
printf("Facing the right direction\n");
return 0;
}
// FAST LOOP: 50 Hz-ish
void RAMBLERBot::loop50Hz()
{
// Check if the speed should accelerate or decelerate
if (speedLimit(&motor_left_, motor_left_goal_))
{
motors_.setLeftSpeed(motor_left_);
}
// Check if the speed should accelerate or decelerate
if (speedLimit(&motor_right_, motor_right_goal_))
{
motors_.setRightSpeed(motor_right_);
}
// Reset 50 Hz Count if necessary
if (++count50Hz_ >= 5)
{
count50Hz_ = 0;
}
// Perform Operations on the 50 Hz time
if (count50Hz_ == 1)
{
rand1_ = GetRandomValue();
}
if (count50Hz_ == 2)
{
rand2_ = GetRandomValue();
}
if (count50Hz_ == 3)
{
rand3_ = GetRandomValue();
}
if (count50Hz_ == 4)
{
whiskers_.GetWhiskerValues(&ant_right_, &ant_left_);
}
}
void driveStraight(float dist)
{
float timeStart = get_time();
Point prevPt = {currentPt.x, currentPt.y};
motor_vel = speedLimit(KD*dist);
straight(motor_vel);
pause(500);
update();
if (distanceTo(prevPt) < 0.01) //working too hard, what to do?
{
brake();
wiggle(5);
straight(70); //backward at speed 70
pause(1000);
pointTurn(0);
}
}
void BoidFlock::limitSpeed(Boid *boid) {
float velMag = boid->getVelocity().length();
if (velMag > speedLimit()) {
boid->setVelocity(normalize(boid->getVelocity()) * speedLimit());
}
}