343 lines
8.6 KiB
C++
343 lines
8.6 KiB
C++
#include "robot.h"
|
|
#include <IRdecoder.h>
|
|
#include <limits.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();
|
|
|
|
/**
|
|
* Initialize the IMU and set the rate and scale to reasonable values.
|
|
*/
|
|
imu.init();
|
|
|
|
/**
|
|
* TODO: Add code to set the data rate and scale of IMU (or edit LSM6::setDefaults())
|
|
*/
|
|
|
|
// The line sensor elements default to INPUTs, but we'll initialize anyways, for completeness
|
|
lineSensor.Initialize();
|
|
}
|
|
|
|
void Robot::EnterIdleState(void)
|
|
{
|
|
Serial.println("-> IDLE");
|
|
chassis.Stop();
|
|
keyString = "";
|
|
robotState = ROBOT_IDLE;
|
|
}
|
|
|
|
/**
|
|
* Functions related to the IMU (turning; ramp detection)
|
|
*/
|
|
void Robot::EnterTurn(float angleInDeg)
|
|
{
|
|
Serial.println(" -> TURN");
|
|
robotState = ROBOT_TURNING;
|
|
Serial.println(" -> TURN");
|
|
robotState = ROBOT_TURNING;
|
|
targetDirection = (currDirection+2)%4;
|
|
chassis.SetTwist(0.0,0.5);
|
|
}
|
|
|
|
bool Robot::CheckTurnComplete(void)
|
|
{
|
|
bool retVal = false;
|
|
|
|
if (targetDirection == currDirection) {return retVal;};
|
|
|
|
// unsound
|
|
if (lineSensor.LineDetected() && eulerAngles.z > targetDirection * 90) {
|
|
retVal = true;
|
|
}
|
|
|
|
return retVal;
|
|
}
|
|
|
|
void Robot::HandleTurnComplete(void)
|
|
{
|
|
currDirection = targetDirection;
|
|
chassis.Stop();
|
|
|
|
if (robotRampState == RAMP_ASCENDING) {
|
|
robotRampState = RAMP_DESCENDING;
|
|
Robot::EnterLineFollowing(20.);
|
|
} else {
|
|
Robot::EnterIdleState();
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Here is a good example of handling information differently, depending on the state.
|
|
* If the Romi is not moving, we can update the bias (but be careful when you first start up!).
|
|
* When it's moving, then we update the heading.
|
|
*/
|
|
void Robot::HandleOrientationUpdate(void)
|
|
{
|
|
prevEulerAngles = eulerAngles;
|
|
if(robotState == ROBOT_IDLE)
|
|
{
|
|
// TODO: You'll need to add code to LSM6 to update the bias
|
|
imu.updateGyroBias();
|
|
}
|
|
|
|
else // update orientation
|
|
{
|
|
float z_rot = ((float)imu.g.z - imu.gyroBias.z) / (float) INT_MAX;
|
|
z_rot /= 104.0;
|
|
z_rot *= 245.0;
|
|
eulerAngles.z += z_rot;
|
|
|
|
}
|
|
|
|
float g_lsb = 0.061 / 1000.;
|
|
float acc_x = asin(imu.a.x * g_lsb) * 180 / PI;
|
|
if (isnan(acc_x)) {
|
|
acc_x = 90; // pointing straight down will never occur
|
|
}
|
|
float gyrodelt_x = (float)imu.g.x / (float)INT_MAX;
|
|
gyrodelt_x *= 245.0 / 104.0;
|
|
float kappa = 0.1;
|
|
eulerAngles.x = (1-kappa) * (eulerAngles.x + gyrodelt_x) + kappa * acc_x;
|
|
|
|
float acc_y = asin(imu.a.y * g_lsb) * 180 / PI;
|
|
if (isnan(acc_y)) {
|
|
acc_y = 90; // pointing straight down will never occur
|
|
}
|
|
float gyrodelt_y = (float)imu.g.y / (float)INT_MAX;
|
|
gyrodelt_y *= 245.0 / 104.0;
|
|
eulerAngles.y = (1-kappa) * (eulerAngles.y + gyrodelt_y) + kappa * acc_y;
|
|
#ifdef __IMU_DEBUG__
|
|
Serial.print(imu.gyroBias.z);
|
|
Serial.print('\t');
|
|
Serial.print(eulerAngles.z);
|
|
Serial.print('\t');
|
|
//Serial.print(imu.g.x);
|
|
//Serial.print('\t');
|
|
//Serial.print(imu.g.y);
|
|
//Serial.print('\t');
|
|
Serial.println(imu.g.z);
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
* Functions related to line following and intersection detection.
|
|
*/
|
|
void Robot::EnterLineFollowing(float speed)
|
|
{
|
|
Serial.println(" -> LINING");
|
|
baseSpeed = speed;
|
|
robotState = ROBOT_LINING;
|
|
}
|
|
|
|
int lineLostFrames = 24;
|
|
|
|
float lastError = 0;
|
|
|
|
int leveltimer = 0;
|
|
|
|
void Robot::LineFollowingUpdate(void)
|
|
{
|
|
if(robotState == ROBOT_LINING)
|
|
{
|
|
#ifdef DARK
|
|
float setpoint = rollingTurnRate > 0.1 ? 4 : 3;
|
|
#endif
|
|
#ifndef DARK
|
|
float setpoint = 3.5;
|
|
#endif
|
|
|
|
float lineError = setpoint - lineSensor.CalcError();
|
|
|
|
|
|
float powErr = (lineError*lineError);
|
|
powErr = (lineError < 0 ? -powErr : powErr);
|
|
|
|
float errDiff = lastError - lineError;
|
|
if (lineSensor.LineDetected()) {
|
|
lastError = lineError;
|
|
} else {
|
|
errDiff = 0;
|
|
lineError = lastError > 0 ? 7:-7;
|
|
}
|
|
|
|
float turnEffort = powErr * lining_kP2 + lineError * lining_kP + errDiff * lining_kD;
|
|
|
|
#ifdef __LINING_DEBUG__
|
|
Serial.print(lineSensor.CalcError());
|
|
Serial.print(' ');
|
|
Serial.print(lineError * lining_kP);
|
|
Serial.print(' ');
|
|
Serial.print(errDiff * lining_kD);
|
|
Serial.print(' ');
|
|
Serial.print(turnEffort);
|
|
Serial.print(' ');
|
|
Serial.println(lineSensor.AverageReflectance());
|
|
#endif
|
|
|
|
rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);
|
|
|
|
float speed = baseSpeed;
|
|
|
|
speed *= 1 - (abs(rollingTurnRate) * KTurnRate);
|
|
|
|
if (lineSensor.CheckIntersection()) {
|
|
turnEffort = 0;
|
|
speed *= 0.8;
|
|
}
|
|
|
|
if (!lineSensor.LineDetected()) {
|
|
lineLostFrames -= 1;
|
|
if (lineLostFrames < 0) {
|
|
rollingTurnRate = 0;
|
|
turnEffort = 0;
|
|
speed = 0;
|
|
chassis.Stop();
|
|
return;
|
|
} else {
|
|
speed *= 0.97;
|
|
}
|
|
}
|
|
else {
|
|
lineLostFrames = 24;
|
|
}
|
|
|
|
chassis.SetTwist(speed, turnEffort);
|
|
|
|
if (eulerAngles.x > 10.) {
|
|
Serial.println("climbing");
|
|
} else if (eulerAngles.x <= 10.) {
|
|
Serial.println("level");
|
|
}
|
|
|
|
if (eulerAngles.x > 10.) { // activate timer when on ramp
|
|
leveltimer = 30;
|
|
}
|
|
if (leveltimer > 0) {
|
|
if (eulerAngles.x <= 5.) {
|
|
leveltimer--; // count down when off ramp
|
|
if (leveltimer == 1) {
|
|
leveltimer = 0;
|
|
if (robotRampState == RAMP_ASCENDING) {
|
|
Robot::EnterTurn(180);
|
|
} else {
|
|
EnterIdleState();
|
|
}
|
|
}
|
|
} else {
|
|
leveltimer = 30;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
/**
|
|
* As coded, HandleIntersection will make the robot drive out 3 intersections, turn around,
|
|
* and stop back at the start. You will need to change the behaviour accordingly.
|
|
*/
|
|
void Robot::HandleIntersection(void)
|
|
{
|
|
Serial.print("X: ");
|
|
if(robotState == ROBOT_LINING)
|
|
{
|
|
switch(nodeTo)
|
|
{
|
|
case NODE_START:
|
|
if(nodeFrom == NODE_1)
|
|
EnterIdleState();
|
|
break;
|
|
case NODE_1:
|
|
// By default, we'll continue on straight
|
|
if(nodeFrom == NODE_START)
|
|
{
|
|
nodeTo = NODE_2;
|
|
}
|
|
else if(nodeFrom == NODE_2)
|
|
{
|
|
nodeTo = NODE_START;
|
|
}
|
|
nodeFrom = NODE_1;
|
|
break;
|
|
case NODE_2:
|
|
// By default, we'll continue on straight
|
|
if(nodeFrom == NODE_1)
|
|
{
|
|
nodeTo = NODE_3;
|
|
}
|
|
else if(nodeFrom == NODE_3)
|
|
{
|
|
nodeTo = NODE_1;
|
|
}
|
|
nodeFrom = NODE_2;
|
|
break;
|
|
case NODE_3:
|
|
// By default, we'll bang a u-ey
|
|
if(nodeFrom == NODE_2)
|
|
{
|
|
nodeTo = NODE_2;
|
|
nodeFrom = NODE_3;
|
|
EnterTurn(180);
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
Serial.print(nodeFrom);
|
|
Serial.print("->");
|
|
Serial.print(nodeTo);
|
|
Serial.print('\n');
|
|
}
|
|
}
|
|
|
|
void Robot::RobotLoop(void)
|
|
{
|
|
/**
|
|
* The main loop for your robot. Process both synchronous events (motor control),
|
|
* and asynchronous events (IR presses, distance readings, etc.).
|
|
*/
|
|
|
|
/**
|
|
* Handle any IR remote keypresses.
|
|
*/
|
|
int16_t keyCode = decoder.getKeyCode();
|
|
if(keyCode != -1) HandleKeyCode(keyCode);
|
|
|
|
/**
|
|
* Check the Chassis timer, which is used for executing motor control
|
|
*/
|
|
if(chassis.CheckChassisTimer())
|
|
{
|
|
// add synchronous, pre-motor-update actions here
|
|
if(robotState == ROBOT_LINING)
|
|
{
|
|
LineFollowingUpdate();
|
|
}
|
|
|
|
chassis.UpdateMotors();
|
|
|
|
// add synchronous, post-motor-update actions here
|
|
|
|
}
|
|
|
|
/**
|
|
* Check for any intersections
|
|
*/
|
|
if(lineSensor.CheckIntersection()) HandleIntersection();
|
|
if(CheckTurnComplete()) HandleTurnComplete();
|
|
|
|
/**
|
|
* Check for an IMU update
|
|
*/
|
|
if(imu.checkForNewData())
|
|
{
|
|
HandleOrientationUpdate();
|
|
}
|
|
}
|
|
|