61 lines
1.5 KiB
C++
61 lines
1.5 KiB
C++
#pragma once
|
|
|
|
#include "chassis.h"
|
|
|
|
const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (cm)
|
|
const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)
|
|
|
|
class Robot
|
|
{
|
|
protected:
|
|
/**
|
|
* robotState is used to track the current task of the robot. You will add new states as
|
|
* the term progresses.
|
|
*/
|
|
enum ROBOT_STATE
|
|
{
|
|
ROBOT_IDLE,
|
|
ROBOT_DRIVE_TO_POINT,
|
|
ROBOT_CALIB_TURN,
|
|
ROBOT_CALIB_DRIVE,
|
|
};
|
|
ROBOT_STATE robotState = ROBOT_IDLE;
|
|
|
|
/* Define the chassis*/
|
|
Chassis chassis;
|
|
|
|
// For managing key presses
|
|
String keyString;
|
|
|
|
/**
|
|
* For tracking current pose and the destination.
|
|
*/
|
|
Pose currPose;
|
|
Pose destPose;
|
|
|
|
public:
|
|
Robot(void) {keyString.reserve(10);}
|
|
void InitializeRobot(void);
|
|
void RobotLoop(void);
|
|
|
|
protected:
|
|
/* State changes */
|
|
void EnterIdleState(void);
|
|
void EnterCalibTurn(void);
|
|
void EnterCalibDrive(void);
|
|
|
|
// /* Navigation methods.*/
|
|
void UpdatePose(const Pose& u);
|
|
void SetDestination(const Pose& destination);
|
|
void DriveToPoint(void);
|
|
void CalibTurn(void);
|
|
void CalibDrive(void);
|
|
bool CheckReachedDestination(void);
|
|
void HandleDestination(void);
|
|
|
|
// Square distance between current and target pose
|
|
float SquareDistance(void);
|
|
|
|
// minimum radians to transform current to setpoint
|
|
float RadError(float current, float setpoint);
|
|
};
|