137 lines
3.7 KiB
C++
137 lines
3.7 KiB
C++
#pragma once
|
|
#include "chassis.h"
|
|
#include <LineSensor.h>
|
|
#include <LSM6.h>
|
|
#include <openmv.h>
|
|
#include <servo32u4.h>
|
|
|
|
static float apriltag_Kp = 0.043; // rads per second per pixel
|
|
static float lining_kP2 = 0.67;
|
|
static float lining_kP = 0.72;
|
|
static float lining_kD = -1.2;
|
|
|
|
class Robot
|
|
{
|
|
protected:
|
|
/**
|
|
* We define some modes for you. SETUP is used for adjusting gains and so forth. Most
|
|
* of the activities will run in AUTO. You shouldn't need to mess with these.
|
|
*/
|
|
enum ROBOT_CTRL_MODE
|
|
{
|
|
CTRL_TELEOP,
|
|
CTRL_AUTO,
|
|
CTRL_SETUP,
|
|
};
|
|
ROBOT_CTRL_MODE robotCtrlMode = CTRL_TELEOP;
|
|
|
|
enum ROBOT_RAMP_STATE
|
|
{
|
|
RAMP_ASCENDING,
|
|
RAMP_DESCENDING,
|
|
};
|
|
ROBOT_RAMP_STATE robotRampState = RAMP_ASCENDING;
|
|
|
|
/**
|
|
* 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_LINING,
|
|
ROBOT_TURNING,
|
|
ROBOT_SEARCHING,
|
|
ROBOT_APPROACHING,
|
|
};
|
|
ROBOT_STATE robotState = ROBOT_IDLE;
|
|
|
|
/* Define the chassis*/
|
|
Chassis chassis;
|
|
|
|
/* Line sensor */
|
|
LineSensor lineSensor;
|
|
|
|
Servo32U4Pin5 servo;
|
|
|
|
/* To add later: rangefinder, camera, etc.*/
|
|
uint16_t distanceTolerance = 65; //for width of tag
|
|
#define CENTER_THRESHOLD 10
|
|
#define CENTERPOINT_X 80
|
|
uint16_t headingTolerance = 10; //for heading cx of tag
|
|
AprilTagDatum tag;
|
|
OpenMV camera;
|
|
|
|
// For managing key presses
|
|
String keyString;
|
|
|
|
/**
|
|
* The LSM6 IMU that is included on the Romi. We keep track of the orientation through
|
|
* Euler angles (roll, pitch, yaw).
|
|
*/
|
|
LSM6 imu;
|
|
LSM6::vector<float> prevEulerAngles;
|
|
LSM6::vector<float> eulerAngles;
|
|
|
|
/* targetHeading is used for commanding the robot to turn */
|
|
float targetHeading;
|
|
|
|
/* baseSpeed is used to drive at a given speed while, say, line following.*/
|
|
float baseSpeed = 0;
|
|
|
|
float rollingTurnRate = 0;
|
|
float turnRateDamp = 0.975; // the proportion of the turn rate retained each loop
|
|
float KTurnRate = 0.18; // slowdown coefficient
|
|
|
|
/**
|
|
* For tracking the motion of the Romi. We keep track of the intersection we came
|
|
* from and the one we're headed to. You'll program in the map in handleIntersection()
|
|
* and other functions.
|
|
*/
|
|
enum INTERSECTION {NODE_START, NODE_1, NODE_2, NODE_3,};
|
|
INTERSECTION nodeFrom = NODE_START;
|
|
INTERSECTION nodeTo = NODE_1;
|
|
|
|
public:
|
|
Robot(void) {keyString.reserve(8);} //reserve some memory to avoid reallocation
|
|
void InitializeRobot(void);
|
|
void RobotLoop(void);
|
|
|
|
protected:
|
|
/* For managing IR remote key presses*/
|
|
void HandleKeyCode(int16_t keyCode);
|
|
|
|
/* State changes */
|
|
void EnterIdleState(void);
|
|
|
|
/* Mode changes */
|
|
void EnterTeleopMode(void);
|
|
void EnterAutoMode(void);
|
|
void EnterSetupMode(void);
|
|
|
|
/**
|
|
* Line following and navigation routines.
|
|
*/
|
|
void EnterLineFollowing(float speed);
|
|
void LineFollowingUpdate(void);
|
|
|
|
bool CheckIntersection(void) {return lineSensor.CheckIntersection();}
|
|
void HandleIntersection(void);
|
|
|
|
void EnterTurn(float angleInDeg);
|
|
bool CheckTurnComplete(void);
|
|
void HandleTurnComplete(void);
|
|
|
|
/* IMU routines */
|
|
void HandleOrientationUpdate(void);
|
|
|
|
/* For commanding the lifter servo */
|
|
void SetLifter(uint16_t position);
|
|
|
|
uint8_t FindAprilTags(void);
|
|
void HandleAprilTag(const AprilTagDatum &tag);
|
|
void EnterSearchingState(void);
|
|
void EnterApproachingState(void);
|
|
bool CheckApproachComplete(int headingTolerance, int distanceTolerance);
|
|
void HandleApproachComplete();
|
|
};
|