108 lines
2.5 KiB
C++
108 lines
2.5 KiB
C++
/**
|
|
* robot-nav.cpp is where you should put navigation routines.
|
|
*/
|
|
|
|
#include "robot.h"
|
|
|
|
void Robot::UpdatePose(const Pose& delta)
|
|
{
|
|
/**
|
|
* TODO: Add your FK algorithm to update currPose here.
|
|
*/
|
|
|
|
float transitHeading = currPose.theta + (delta.theta * 0.5);
|
|
|
|
currPose.x += delta.x * cos(transitHeading);
|
|
currPose.y += delta.x * sin(transitHeading);
|
|
|
|
currPose.theta += delta.theta;
|
|
|
|
#ifdef __NAV_DEBUG__
|
|
TeleplotPrint("x", currPose.x);
|
|
TeleplotPrint("y", currPose.y);
|
|
TeleplotPrint("theta", currPose.theta * 180.0 / PI);
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
* Sets a destination in the lab frame.
|
|
*/
|
|
void Robot::SetDestination(const Pose& dest)
|
|
{
|
|
/**
|
|
* TODO: Turn on LED, as well.
|
|
*/
|
|
Serial.print("Setting dest to: ");
|
|
Serial.print(dest.x);
|
|
Serial.print(", ");
|
|
Serial.print(dest.y);
|
|
Serial.print('\n');
|
|
|
|
destPose = dest;
|
|
robotState = ROBOT_DRIVE_TO_POINT;
|
|
}
|
|
|
|
float Robot::SquareDistance(void) {
|
|
return pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
|
|
}
|
|
|
|
#define TAU (float) TWO_PI
|
|
|
|
float Robot::RadError(float current, float destination) {
|
|
float diff = destination-current;
|
|
diff += HALF_PI;
|
|
diff = fmod(diff,TAU); // floating point modulo
|
|
diff -= HALF_PI;
|
|
|
|
if (diff > 180) {
|
|
diff -= TAU;
|
|
}
|
|
return diff;
|
|
}
|
|
|
|
bool Robot::CheckReachedDestination(void)
|
|
{
|
|
/**
|
|
* TODO: Add code to check if you've reached destination here.
|
|
*/
|
|
|
|
return SquareDistance() < TARGET_SQ_DIST
|
|
&& RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
|
|
}
|
|
|
|
void Robot::DriveToPoint(void)
|
|
{
|
|
if(robotState == ROBOT_DRIVE_TO_POINT)
|
|
{
|
|
float target_heading;
|
|
|
|
if (SquareDistance() < TARGET_SQ_DIST) {
|
|
target_heading = destPose.theta;
|
|
} else {
|
|
target_heading = atan2(destPose.x - currPose.x, destPose.y - currPose.y);
|
|
}
|
|
|
|
float heading_err = RadError(currPose.theta, target_heading);
|
|
float forward_effort = cos(heading_err) * sqrt(SquareDistance());
|
|
|
|
|
|
/**
|
|
* TODO: Add your IK algorithm here.
|
|
*/
|
|
|
|
#ifdef __NAV_DEBUG__
|
|
// Print useful stuff here.
|
|
#endif
|
|
|
|
/**
|
|
* TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above.
|
|
*/
|
|
}
|
|
}
|
|
|
|
void Robot::HandleDestination(void)
|
|
{
|
|
/**
|
|
* TODO: Stop and change state. Turn off LED.
|
|
*/
|
|
}
|