62 lines
1.6 KiB
C++
62 lines
1.6 KiB
C++
#include "robot.h"
|
|
#include <IRdecoder.h>
|
|
|
|
void Robot::InitializeRobot(void)
|
|
{
|
|
chassis.InititalizeChassis();
|
|
|
|
/**
|
|
* Initialize the IR decoder. Declared extern in IRdecoder.h; see robot-remote.cpp
|
|
* for instantiation and setting the pin.
|
|
*/
|
|
decoder.init();
|
|
|
|
/**
|
|
* TODO: Set pin 13 HIGH when navigating and LOW when destination is reached.
|
|
* Need to set as OUTPUT here.
|
|
*/
|
|
}
|
|
|
|
void Robot::EnterIdleState(void)
|
|
{
|
|
chassis.Stop();
|
|
|
|
Serial.println("-> IDLE");
|
|
robotState = ROBOT_IDLE;
|
|
}
|
|
|
|
/**
|
|
* The main loop for your robot. Process both synchronous events (motor control),
|
|
* and asynchronous events (IR presses, distance readings, etc.).
|
|
*/
|
|
void Robot::RobotLoop(void)
|
|
{
|
|
/**
|
|
* Handle any IR remote keypresses.
|
|
*/
|
|
int16_t keyCode = decoder.getKeyCode();
|
|
if(keyCode != -1) HandleKeyCode(keyCode);
|
|
|
|
/**
|
|
* Run the chassis loop, which handles low-level control.
|
|
*/
|
|
Twist velocity;
|
|
if(chassis.ChassisLoop(velocity))
|
|
{
|
|
// We do FK regardless of state
|
|
UpdatePose(velocity);
|
|
|
|
/**
|
|
* Here, we break with tradition and only call these functions if we're in the
|
|
* DRIVE_TO_POINT state. CheckReachedDestination() is expensive, so we don't want
|
|
* to do all the maths when we don't need to.
|
|
*
|
|
* While we're at it, we'll toss DriveToPoint() in, as well.
|
|
*/
|
|
if(robotState == ROBOT_DRIVE_TO_POINT)
|
|
{
|
|
DriveToPoint();
|
|
if(CheckReachedDestination()) HandleDestination();
|
|
}
|
|
}
|
|
}
|