ank/RBE2002
1
Fork 0
Code Issues Activity

Initial commit

Browse source
This commit is contained in:
github-classroom[bot] 2024-10-22 16:03:16 +00:00 committed by GitHub
commit 0534981d4b
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
25 changed files with 2114 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
.gitattributes vendored Normal file
View file

@ -0,0 +1,2 @@
# Auto detect text files and perform LF normalization
* text=auto

2
.gitignore vendored Normal file
View file

@ -0,0 +1,2 @@
.vscode
.pio

2
README.md Normal file
View file

@ -0,0 +1,2 @@
# Starter-Code

39
include/README Normal file
View file

@ -0,0 +1,39 @@
This directory is intended for project header files.
A header file is a file containing C declarations and macro definitions
to be shared between several project source files. You request the use of a
header file in your project source file (C, C++, etc) located in `src` folder
by including it, with the C preprocessing directive `#include'.
```src/main.c
#include "header.h"
int main (void)
{
...
}
```
Including a header file produces the same results as copying the header file
into each source file that needs it. Such copying would be time-consuming
and error-prone. With a header file, the related declarations appear
in only one place. If they need to be changed, they can be changed in one
place, and programs that include the header file will automatically use the
new version when next recompiled. The header file eliminates the labor of
finding and changing all the copies as well as the risk that a failure to
find one copy will result in inconsistencies within a program.
In C, the usual convention is to give header files names that end with `.h'.
It is most portable to use only letters, digits, dashes, and underscores in
header file names, and at most one dot.
Read more about using header files in official GCC documentation:
* Include Syntax
* Include Operation
* Once-Only Headers
* Computed Includes
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

160
lib/Chassis/src/chassis.cpp Normal file
View file

@ -0,0 +1,160 @@
#include "chassis.h"
#include "Romi32U4MotorTemplate.h"
Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor;
Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor;
void Chassis::SetMotorKp(float kp) {leftMotor.Kp = kp; rightMotor.Kp = kp;}
void Chassis::SetMotorKi(float ki) {leftMotor.Ki = ki; rightMotor.Ki = ki; leftMotor.sumError = 0; rightMotor.sumError = 0;}
void Chassis::SetMotorKd(float kd) {leftMotor.Kd = kd; rightMotor.Kd = kd;}
/**
* Because it's declared static, we initialize Chassis::loopFlag here.
*/
uint8_t Chassis::loopFlag = 0;
void Chassis::Timer4OverflowISRHandler(void)
{
loopFlag++;
leftMotor.speed = leftMotor.CalcEncoderDelta();
rightMotor.speed = rightMotor.CalcEncoderDelta();
}
/**
* Sets up a hardware timer on Timer4 to manage motor control on a precise schedule.
*
* We set the timer to set an interrupt flag on overflow, which is handled
* by ISR(TIMER4_OVF_vect) below.
*/
void Chassis::InitializeMotorControlTimer(void)
{
Serial.println("InitTimer");
// Disable interupts while we mess with the Timer4 registers
cli();
// Set up Timer4
TCCR4A = 0x00; // Disable output to pins
TCCR4B = 0x0A; // Sets the prescaler -- see pp. 167-8 in datasheet
TCCR4C = 0x00; // Disables output to pins (but see below for buzzer)
TCCR4D = 0x00; // Normal mode: count up and roll-over
/**
* Calculate TOP based on prescaler and loop duration. Note that loop is in integer ms --
* there may be some rounding. Multiples of 4 ms will be exact.
*/
uint16_t top = ((CONTROL_LOOP_PERIOD_MS * 16000ul) >> 9) - 1; // divides by 512
/**
* Here we do a little trick to allow full 10-bit register access.
* We have 2 _bits_ in TC4H that we can use to add capacity to TOP.
*
* Note that the maximum period is limited by TOP = 0x3FF. If you want
* a longer period, you'll need to adjust the pre-scaler.
*
* There is no minumum period, but precision breaks down with low values,
* unless you adjust the pre-scaler, but the encoder resolution is limited,
* so you only want to go so fast.
*/
uint8_t highbits = top / 256;
uint8_t lowbits = top - highbits;
TC4H = highbits; OCR4C = lowbits;
// Enable overflow interrupt
TIMSK4 = 0x04;
/**
* Uncommenting the following lines will pipe the timer signal to pin 6,
* which controls the buzzer. The pin will toggle at the loop rate, which
* allows you to check that the timing is correct. It will also make a lot
* of noise, so do so sparingly.
*/
// TCCR4C = 0x04
// pinMode(6, OUTPUT);
// Re-enable interrupts
sei();
Serial.println("/InitTimer");
}
bool Chassis::CheckChassisTimer(void)
{
bool retVal = false;
if(loopFlag)
{
if(loopFlag > 1) Serial.println("Missed an update in Robot::RobotLoop()!");
#ifdef __LOOP_DEBUG__
Serial.print(millis());
Serial.print('\t');
#endif
loopFlag = 0;
retVal = true;
#ifdef __LOOP_DEBUG__
Serial.println();
#endif
}
return retVal;
}
void Chassis::UpdateMotors(void)
{
#ifdef __MOTOR_DEBUG__
leftMotor.ControlMotorSpeed(true);
#else
leftMotor.ControlMotorSpeed();
#endif
rightMotor.ControlMotorSpeed();
}
/**
* SetWheelSpeeds converts the linear wheel speeds (axes relative to ground) to motor speeds.
*/
void Chassis::SetWheelSpeeds(float leftSpeedCMperSec, float rightSpeedCMperSec)
{
/**
* TODO: Add code to convert cm/sec -> encoder ticks/control interval
*/
leftMotor.SetTargetSpeed(leftSpeedCMperSec);
rightMotor.SetTargetSpeed(rightSpeedCMperSec);
}
/**
* ISR for timing. On Timer4 overflow, we take a 'snapshot' of the encoder counts
* and raise a flag to let the program it is time to execute the PID calculations.
*/
ISR(TIMER4_OVF_vect)
{
Chassis::Timer4OverflowISRHandler();
}
void Chassis::InitializeMotors(void)
{
Romi32U4MotorBase::InitializePWMTimerAndInterrupts();
leftMotor.InitializeMotor();
rightMotor.InitializeMotor();
}
void Chassis::SetMotorEfforts(int16_t left, int16_t right)
{
leftMotor.SetMotorEffortDirect(left);
rightMotor.SetMotorEffortDirect(right);
}
/**
* Converts a forward speed and angular velocity to wheel speeds.
*
* fwdSpeed in cm/s; rotSpeed in rad/s.
*/
void Chassis::SetTwist(float fwdSpeed, float rotSpeed)
{
float leftSpeed = fwdSpeed - rotSpeed * ROBOT_RADIUS;
float rightSpeed = fwdSpeed + rotSpeed * ROBOT_RADIUS;
SetWheelSpeeds(leftSpeed, rightSpeed);
}

64
lib/Chassis/src/chassis.h Normal file
View file

@ -0,0 +1,64 @@
#pragma once
#include <Arduino.h>
#include "event_timer.h"
class Chassis
{
protected:
/**
* Kinematic parameters default to the spec sheet from Pololu. You'll need to fine
* tune them.
*/
const float ROBOT_RADIUS = 14.7 / 2;
const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0);
/**
* You can change the control loop period, but you should use multiples of 4 ms to
* avoid rounding errors.
*/
const uint16_t CONTROL_LOOP_PERIOD_MS = 20;
protected:
/**
* loopFlag is used to tell the program when to update. It is set when Timer4
* overflows (see InitializeMotorControlTimer). Some of the calculations are too
* lengthy for an ISR, so we set a flag and use that to key the calculations.
*
* Note that we use in integer so we can see if we've missed a loop. If loopFlag is
* more than 1, then we missed a cycle.
*/
static uint8_t loopFlag;
public:
Chassis(void) {}
void InititalizeChassis(void)
{
InitializeMotorControlTimer();
InitializeMotors();
}
void SetMotorKp(float kp);
void SetMotorKi(float ki);
void SetMotorKd(float kd);
bool CheckChassisTimer(void);
static void Timer4OverflowISRHandler(void);
public:
void Stop(void) {SetMotorEfforts(0, 0);}
void SetTwist(float fwdSpeed, float rotSpeed);
void SetWheelSpeeds(float, float);
void UpdateMotors(void);
protected:
/**
* Initialization and Setup routines
*/
void InitializeMotorControlTimer(void);
void InitializeMotors(void);
void SetMotorEfforts(int16_t, int16_t);
};

2
lib/LSM6/README.md Normal file
View file

@ -0,0 +1,2 @@
# LSM6

355
lib/LSM6/src/LSM6.cpp Normal file
View file

@ -0,0 +1,355 @@
#include <Wire.h>
#include <LSM6.h>
#include <math.h>
// Defines ////////////////////////////////////////////////////////////////
// The Arduino two-wire interface uses a 7-bit number for the address,
// and sets the last bit correctly based on reads and writes
#define DS33_SA0_HIGH_ADDRESS 0b1101011
#define DS33_SA0_LOW_ADDRESS 0b1101010
#define TEST_REG_ERROR -1
#define DS33_WHO_ID 0x69
// Constructors ////////////////////////////////////////////////////////////////
LSM6::LSM6(void)
{
_device = device_auto;
io_timeout = 0; // 0 = no timeout
did_timeout = false;
}
// Public Methods //////////////////////////////////////////////////////////////
#define KAPPA 0.01
#define EPSILON 0.00005
bool LSM6::checkForNewData(void)
{
bool retVal = false;
if(getStatus() & 0x01)
{
read();
retVal = true;
}
return retVal;
}
// Did a timeout occur in readAcc(), readGyro(), or read() since the last call to timeoutOccurred()?
bool LSM6::timeoutOccurred()
{
bool tmp = did_timeout;
did_timeout = false;
return tmp;
}
void LSM6::setTimeout(uint16_t timeout)
{
io_timeout = timeout;
}
uint16_t LSM6::getTimeout()
{
return io_timeout;
}
void LSM6::setFullScaleGyro(GYRO_FS gfs)
{
uint8_t settings = readReg(LSM6::CTRL2_G);
settings &= 0xf0; //clear sensitivity bits; can't use 125 setting; bit 0 must be 0
switch (gfs)
{
case GYRO_FS245:
writeReg(LSM6::CTRL2_G, settings | 0b00000000);
mdpsPerLSB = 8.75;
break;
case GYRO_FS500:
writeReg(LSM6::CTRL2_G, settings | 0b00000100);
mdpsPerLSB = 17.5;
break;
case GYRO_FS1000:
writeReg(LSM6::CTRL2_G, settings | 0b00001000);
mdpsPerLSB = 35;
break;
case GYRO_FS2000:
writeReg(LSM6::CTRL2_G, settings | 0b00001100);
mdpsPerLSB = 70;
break;
default:
Serial.println("Error setting gyro!");
}
Serial.print("Set Gyro FS to ");
Serial.print(mdpsPerLSB);
Serial.println(" mdps/LSB.");
}
void LSM6::setFullScaleAcc(ACC_FS afs)
{
uint8_t settings = readReg(LSM6::CTRL1_XL);
settings &= 0xf3; //clear sensitivity bits
switch (afs)
{
case ACC_FS2:
writeReg(LSM6::CTRL1_XL, settings | 0b00000000);
mgPerLSB = 0.061;
break;
case ACC_FS4:
writeReg(LSM6::CTRL1_XL, settings | 0b00001000);
mgPerLSB = 0.122;
break;
case ACC_FS8:
writeReg(LSM6::CTRL1_XL, settings | 0b00001100);
mgPerLSB = 0.244;
break;
case ACC_FS16:
writeReg(LSM6::CTRL1_XL, settings | 0b00000100);
mgPerLSB = 0.488;
break;
default:
Serial.println("Error setting acc!");
}
Serial.print("Set Acc FS to ");
Serial.print(mgPerLSB, 3);
Serial.println(" mg/LSB.");
}
void LSM6::setGyroDataOutputRate(ODR rate)
{
if(rate < 0 || rate > ODR166k)
{
Serial.println(F("Illegal gyro ODR"));
return;
}
uint8_t settings = readReg(LSM6::CTRL2_G);
settings &= 0x0f; //clear ODR bits
writeReg(LSM6::CTRL2_G, settings | (rate << 4));
// rate in this case is just a flag for the ODR [1 <= rate <= 8]
// corresponding to [13, 26, 52, ..., 1664] Hz
gyroODR = 13 * pow(2, rate - 1);
Serial.print("Set Gyro ODR to ");
Serial.print(gyroODR);
Serial.println(" Hz.");
}
void LSM6::setAccDataOutputRate(ODR rate)
{
if(rate < 0 || rate > ODR166k)
{
Serial.println(F("Illegal acc ODR"));
return;
}
uint8_t settings = readReg(LSM6::CTRL1_XL);
settings &= 0x0f; //clear ODR bits
writeReg(LSM6::CTRL1_XL, settings | (rate << 4));
// rate in this case is just a flag for the ODR [1 <= rate <= 8]
// corresponding to [13, 26, 52, ..., 1664] Hz
accODR = 13 * pow(2, rate - 1);
Serial.print("Set Acc ODR to ");
Serial.print(accODR);
Serial.println(" Hz.");
}
bool LSM6::init(deviceType device, sa0State sa0)
{
Wire.begin();
// perform auto-detection unless device type and SA0 state were both specified
if (device == device_auto || sa0 == sa0_auto)
{
// check for LSM6DS33 if device is unidentified or was specified to be this type
if (device == device_auto || device == device_DS33)
{
// check SA0 high address unless SA0 was specified to be low
if (sa0 != sa0_low && testReg(DS33_SA0_HIGH_ADDRESS, WHO_AM_I) == DS33_WHO_ID)
{
sa0 = sa0_high;
if (device == device_auto)
{
device = device_DS33;
}
}
// check SA0 low address unless SA0 was specified to be high
else if (sa0 != sa0_high && testReg(DS33_SA0_LOW_ADDRESS, WHO_AM_I) == DS33_WHO_ID)
{
sa0 = sa0_low;
if (device == device_auto)
{
device = device_DS33;
}
}
}
// make sure device and SA0 were successfully detected; otherwise, indicate failure
if (device == device_auto || sa0 == sa0_auto)
{
return false;
}
}
_device = device;
switch (device)
{
case device_DS33: // only this IMU for now...
address = (sa0 == sa0_high) ? DS33_SA0_HIGH_ADDRESS : DS33_SA0_LOW_ADDRESS;
break;
default:
break;
}
enableDefault();
return true;
}
/*
Enables the LSM6's accelerometer and gyro. Also:
- Sets sensor full scales (gain) to default power-on values, which are
+/- 2 g for accelerometer and 245 dps for gyro
- Selects 13 Hz ODR (VERY SLOW!) for accelerometer and gyro
- Enables automatic increment of register address during multiple byte access
- Note that this function will also reset other settings controlled by
the registers it writes to.
*/
void LSM6::enableDefault(void)
{
if (_device == device_DS33)
{
// Set the gyro full scale and data rate
setFullScaleGyro(GYRO_FS245);
setGyroDataOutputRate(ODR13);
// Set the accelerometer full scale and data rate
setFullScaleAcc(ACC_FS2);
setAccDataOutputRate(ODR13);
// Auto-increment
writeReg(CTRL3_C, 0x04);
}
}
void LSM6::writeReg(uint8_t reg, uint8_t value)
{
Wire.beginTransmission(address);
Wire.write(reg);
Wire.write(value);
last_status = Wire.endTransmission();
}
uint8_t LSM6::readReg(uint8_t reg)
{
uint8_t value;
Wire.beginTransmission(address);
Wire.write(reg);
last_status = Wire.endTransmission();
Wire.requestFrom(address, (uint8_t)1);
value = Wire.read();
Wire.endTransmission();
return value;
}
// Reads the 3 accelerometer channels and stores them in vector a
void LSM6::readAcc(void)
{
Wire.beginTransmission(address);
// automatic increment of register address is enabled by default (IF_INC in CTRL3_C)
Wire.write(OUTX_L_XL);
Wire.endTransmission();
Wire.requestFrom(address, (uint8_t)6);
uint16_t millis_start = millis();
while (Wire.available() < 6)
{
if (io_timeout > 0 && ((uint16_t)millis() - millis_start) > io_timeout)
{
did_timeout = true;
return;
}
}
uint8_t xla = Wire.read();
uint8_t xha = Wire.read();
uint8_t yla = Wire.read();
uint8_t yha = Wire.read();
uint8_t zla = Wire.read();
uint8_t zha = Wire.read();
// combine high and low bytes
a.x = (int16_t)(xha << 8 | xla);
a.y = (int16_t)(yha << 8 | yla);
a.z = (int16_t)(zha << 8 | zla);
}
// Reads the 3 gyro channels and stores them in vector g
void LSM6::readGyro(void)
{
Wire.beginTransmission(address);
// automatic increment of register address is enabled by default (IF_INC in CTRL3_C)
Wire.write(OUTX_L_G);
Wire.endTransmission();
Wire.requestFrom(address, (uint8_t)6);
uint16_t millis_start = millis();
while (Wire.available() < 6)
{
if (io_timeout > 0 && ((uint16_t)millis() - millis_start) > io_timeout)
{
did_timeout = true;
return;
}
}
uint8_t xlg = Wire.read();
uint8_t xhg = Wire.read();
uint8_t ylg = Wire.read();
uint8_t yhg = Wire.read();
uint8_t zlg = Wire.read();
uint8_t zhg = Wire.read();
// combine high and low bytes
g.x = (int16_t)(xhg << 8 | xlg);
g.y = (int16_t)(yhg << 8 | ylg);
g.z = (int16_t)(zhg << 8 | zlg);
}
// Reads all 6 channels of the LSM6 and stores them in the object variables
void LSM6::read(void)
{
readAcc();
readGyro();
}
// Private Methods //////////////////////////////////////////////////////////////
int16_t LSM6::testReg(uint8_t address, regAddr reg)
{
Wire.beginTransmission(address);
Wire.write((uint8_t)reg);
if (Wire.endTransmission() != 0)
{
return TEST_REG_ERROR;
}
Wire.requestFrom(address, (uint8_t)1);
if (Wire.available())
{
return Wire.read();
}
else
{
return TEST_REG_ERROR;
}
}

175
lib/LSM6/src/LSM6.h Normal file
View file

@ -0,0 +1,175 @@
#pragma once
// Datasheet: https://www.pololu.com/file/0J1087/LSM6DS33.pdf
#include <Arduino.h>
class LSM6
{
public:
template <typename T> struct vector
{
T x, y, z;
};
enum deviceType { device_DS33, device_auto };
enum sa0State { sa0_low, sa0_high, sa0_auto };
// full-scale factors
enum ACC_FS {ACC_FS2, ACC_FS4, ACC_FS8, ACC_FS16};
enum GYRO_FS {GYRO_FS245, GYRO_FS500, GYRO_FS1000, GYRO_FS2000};
// output data rate options
enum ODR
{
ODR13 = 0x1,
ODR26 = 0x2,
ODR52 = 0x3,
ODR104 = 0x4,
ODR208 = 0x5,
ODR416 = 0x6,
ODR833 = 0x7,
ODR166k = 0x8
};
// register addresses
enum regAddr
{
FUNC_CFG_ACCESS = 0x01,
FIFO_CTRL1 = 0x06,
FIFO_CTRL2 = 0x07,
FIFO_CTRL3 = 0x08,
FIFO_CTRL4 = 0x09,
FIFO_CTRL5 = 0x0A,
ORIENT_CFG_G = 0x0B,
INT1_CTRL = 0x0D,
INT2_CTRL = 0x0E,
WHO_AM_I = 0x0F,
CTRL1_XL = 0x10,
CTRL2_G = 0x11,
CTRL3_C = 0x12,
CTRL4_C = 0x13,
CTRL5_C = 0x14,
CTRL6_C = 0x15,
CTRL7_G = 0x16,
CTRL8_XL = 0x17,
CTRL9_XL = 0x18,
CTRL10_C = 0x19,
WAKE_UP_SRC = 0x1B,
TAP_SRC = 0x1C,
D6D_SRC = 0x1D,
STATUS_REG = 0x1E,
OUT_TEMP_L = 0x20,
OUT_TEMP_H = 0x21,
OUTX_L_G = 0x22,
OUTX_H_G = 0x23,
OUTY_L_G = 0x24,
OUTY_H_G = 0x25,
OUTZ_L_G = 0x26,
OUTZ_H_G = 0x27,
OUTX_L_XL = 0x28,
OUTX_H_XL = 0x29,
OUTY_L_XL = 0x2A,
OUTY_H_XL = 0x2B,
OUTZ_L_XL = 0x2C,
OUTZ_H_XL = 0x2D,
FIFO_STATUS1 = 0x3A,
FIFO_STATUS2 = 0x3B,
FIFO_STATUS3 = 0x3C,
FIFO_STATUS4 = 0x3D,
FIFO_DATA_OUT_L = 0x3E,
FIFO_DATA_OUT_H = 0x3F,
TIMESTAMP0_REG = 0x40,
TIMESTAMP1_REG = 0x41,
TIMESTAMP2_REG = 0x42,
STEP_TIMESTAMP_L = 0x49,
STEP_TIMESTAMP_H = 0x4A,
STEP_COUNTER_L = 0x4B,
STEP_COUNTER_H = 0x4C,
FUNC_SRC = 0x53,
TAP_CFG = 0x58,
TAP_THS_6D = 0x59,
INT_DUR2 = 0x5A,
WAKE_UP_THS = 0x5B,
WAKE_UP_DUR = 0x5C,
FREE_FALL = 0x5D,
MD1_CFG = 0x5E,
MD2_CFG = 0x5F,
};
vector<int16_t> a; // accelerometer readings
vector<int16_t> g; // gyro readings
const float SIGMA = 0.95; // for updating bias
public:
LSM6(void);
bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);
deviceType getDeviceType(void) { return _device; }
void enableDefault(void);
void writeReg(uint8_t reg, uint8_t value);
uint8_t readReg(uint8_t reg);
void readAcc(void);
void readGyro(void);
void read(void);
void setFullScaleGyro(GYRO_FS gfs);
void setFullScaleAcc(ACC_FS afs);
void setGyroDataOutputRate(ODR);
void setAccDataOutputRate(ODR);
void setTimeout(uint16_t timeout);
uint16_t getTimeout(void);
bool timeoutOccurred(void);
uint8_t getStatus(void) {return readReg(LSM6::STATUS_REG);}
bool checkForNewData(void);
vector<float> updateGyroBias(void)
{
/**
* TODO: add a low-pass filter to update the bias
*/
return gyroBias;
}
protected:
deviceType _device; // chip type
uint8_t address;
uint16_t io_timeout;
bool did_timeout;
int16_t testReg(uint8_t address, regAddr reg);
//conversion factors are set when you change ODR or FS
float mdpsPerLSB = 0;
float mgPerLSB = 0;
float accODR = 0; // Hz
float gyroODR = 0; // Hz
uint8_t last_status; // status of last I2C transmission
// for the complementary filter
// float estimatedPitchAngle = 0;
// vector<float> eulerAngles;
vector<float> gyroBias;
/* We make Robot a friend to avoid all the setters and getters. */
friend class Robot;
};

0
lib/LineSensor/readme.md Normal file
View file

30
lib/LineSensor/src/LineSensor.cpp Normal file
View file

@ -0,0 +1,30 @@
#include "LineSensor.h"
#define DARK_THRESHOLD 500;
void LineSensor::Initialize(void)
{
pinMode(leftSensorPin, INPUT);
pinMode(rightSensorPin, INPUT);
}
int16_t LineSensor::CalcError(void)
{
return analogRead(rightSensorPin) - analogRead(leftSensorPin);
}
bool LineSensor::CheckIntersection(void)
{
bool retVal = false;
bool isLeftDark = analogRead(leftSensorPin) > DARK_THRESHOLD;
bool isRightDark = analogRead(rightSensorPin) > DARK_THRESHOLD;
bool onIntersection = isLeftDark && isRightDark;
if(onIntersection && !prevOnIntersection) retVal = true;
prevOnIntersection = onIntersection;
return retVal;
}

21
lib/LineSensor/src/LineSensor.h Normal file
View file

@ -0,0 +1,21 @@
#pragma once
#include <Arduino.h>
#define LEFT_LINE_SENSOR A0
#define RIGHT_LINE_SENSOR A4
class LineSensor
{
protected:
uint8_t leftSensorPin = LEFT_LINE_SENSOR;
uint8_t rightSensorPin = RIGHT_LINE_SENSOR;
bool prevOnIntersection = false;
public:
LineSensor(void) {}
void Initialize(void);
int16_t CalcError(void);
bool CheckIntersection(void);
};

46
lib/README Normal file
View file

@ -0,0 +1,46 @@
This directory is intended for project specific (private) libraries.
PlatformIO will compile them to static libraries and link into executable file.
The source code of each library should be placed in an own separate directory
("lib/your_library_name/[here are source files]").
For example, see a structure of the following two libraries `Foo` and `Bar`:
|--lib
| |
| |--Bar
| | |--docs
| | |--examples
| | |--src
| | |- Bar.c
| | |- Bar.h
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
| |
| |--Foo
| | |- Foo.c
| | |- Foo.h
| |
| |- README --> THIS FILE
|
|- platformio.ini
|--src
|- main.c
and a contents of `src/main.c`:
```
#include <Foo.h>
#include <Bar.h>
int main (void)
{
...
}
```
PlatformIO Library Dependency Finder will find automatically dependent
libraries scanning project source files.
More information about PlatformIO Library Dependency Finder
- https://docs.platformio.org/page/librarymanager/ldf.html

0
lib/Romi32U4Motors/README.md Normal file
View file

18
lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp Normal file
View file

@ -0,0 +1,18 @@
#include "Romi32U4MotorTemplate.h"
#include <PCint.h>
void leftISR(void)
{
leftMotor.ProcessEncoderTick();
}
void rightISR(void)
{
rightMotor.ProcessEncoderTick();
}
void Romi32U4MotorBase::AttachInterrupts(void)
{
attachPCInt(digitalPinToPCInterrupt(LEFT_XOR), leftISR);
attachInterrupt(digitalPinToInterrupt(RIGHT_XOR), rightISR, CHANGE);
}

254
lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h Normal file
View file

@ -0,0 +1,254 @@
#pragma once
#include <Arduino.h>
#include <FastGPIO.h>
// define the motor pins here
#define PWM_L 10
#define PWM_R 9
#define DIR_L 16
#define DIR_R 15
#define LEFT_XOR 8
#define LEFT_B IO_E2
#define RIGHT_XOR 7
#define RIGHT_B 23
#define OCR_L 0x8A
#define OCR_R 0x88
class Romi32U4MotorBase
{
protected:
// Used to control the motors in different ways
enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
// TODO: After you tune your motors, set the gains here.
float Kp = 1;
float Ki = 0;
float Kd = 0;
// Used to keep track of the target speed, in counts / interval.
float targetSpeed = 0;
/**
* This is the speed of the motor, in "encoder counts / encoder interval".
* The encoder interval is set in Robot::InitializeMotorControlTimer.
*/
volatile int16_t speed = 0;
// For tracking the error integral (sum)
float sumError = 0;
// For tracking derivative (difference)
float prevError = 0;
// Maximum effort
int16_t maxEffort = 420;
// Keeps track of encoder changes
volatile int16_t prevCount;
volatile int16_t encCount;
volatile int8_t lastA;
volatile int8_t lastB;
/**
*
*/
virtual void SetEffort(int16_t effort) = 0;
/**
* Used to set the motor _effort_ directly, which is mostly used for testing.
*
* Calling this function will switch the control mode to DIRECT, if needed, meaning speed control is lost.
*/
void SetMotorEffortDirect(int16_t effort)
{
ctrlMode = CTRL_DIRECT;
SetEffort(effort);
}
/**
* calcEncoderDelta() takes a 'snapshot of the encoders and
* stores the change since the last call, which has units of "encoder ticks/motor interval"
*
* We also use the function for zeroing the delta (for example, when switching control modes),
* so interrupts are cleared when accessing encCount.
*/
int16_t CalcEncoderDelta(void)
{
cli();
int16_t currCount = encCount;
sei();
int16_t speed = currCount - prevCount;
prevCount = currCount;
return speed;
}
/**
* Sets the target speed in "encoder ticks/16 ms interval"
* */
void SetTargetSpeed(float target)
{
targetSpeed = target;
if(ctrlMode != CTRL_SPEED)
{
// Reset the error integral if we are switching from another mode
// Otherwise, the robot may jump due to residual integral
sumError = 0;
// Also set prevCount to encCount so to avoid speed jumps when switching mode
CalcEncoderDelta();
}
ctrlMode = CTRL_SPEED;
}
/**
* ControlMotorSpeed implements the PID controller. It should _not_ be called by user code.
* Instead, ControlMotorSpeed is called from Chassis::UpdateMotors, which is on a timer schedule.
*/
void ControlMotorSpeed(bool debug = false)
{
if(ctrlMode == CTRL_SPEED)
{
/**
* TODO: Implement integral, derivative control for the motors here!
*/
// Calculate the error in speed
float error = targetSpeed - speed;
// Calculate the effort from the PID gains
int16_t effort = Kp * error;
// Set the effort for the motor
SetEffort(effort);
if(debug)
{
Serial.print(targetSpeed);
Serial.print('\t');
Serial.print(speed);
Serial.print('\t');
Serial.print(error);
Serial.print('\t');
Serial.print(effort / 10.0); // N.B. that we divide by 10 to make the graph cleaner
Serial.print('\n');
}
}
}
static void AttachInterrupts(void);
/**
* InitializeMotorPWMTimer() should be called near the beginning of the program.
* It sets up Timer4 to run at 38 kHz, which is used to both drive the PWM signal for the motors
* and (tangentially) allow for a 38 kHz signal on pin 11, which can be used, say, to drive an
* IR LED at a common rate.
*
* Timer 1 has the following configuration:
* prescaler of 1
* outputs enabled on channels A (pin 9), B (pin 10) and C (pin 11)
* fast PWM mode
* top of 420, which will be the max speed
* frequency is then: 16 MHz / [1 (prescaler) / (420 + 1)] = 38.005 kHz
* */
static void InitializePWMTimerAndInterrupts(void)
{
Serial.println("InitMotor()");
noInterrupts(); //disable interrupts while we set Timer1 registers
TCCR1A = 0xA2; //0b10100010; //Fast PWM + enable A and B; change to 0xAA to enable C on pin 11
TCCR1B = 0x19; //0b00011001; //Fast PWM
ICR1 = 420; //runs at 38kHz; lowers speed for given effort by 5% from Pololu version
//set all three outputs to zero
OCR1A = 0;
OCR1B = 0;
OCR1C = 0; //can be used to create 38 kHz signal on pin 11; must enable output in TCCR1A above
/**
* Call a static function to set up the left and right motor interrupts
*/
AttachInterrupts();
interrupts(); //re-enable interrupts
Serial.println("/InitMotor()");
}
public:
void SetPIDCoeffs(float p, float i, float d) {Kp = p; Ki = i; Kd = d; sumError = 0;}
};
template <uint8_t encXOR, uint8_t encB, uint8_t PWM, uint8_t DIR, uint8_t OCR>
class Romi32U4EncodedMotor : public Romi32U4MotorBase
{
protected:
void InitializeMotor(void)
{
FastGPIO::Pin<PWM>::setOutputLow();
FastGPIO::Pin<DIR>::setOutputLow();
InitializeEncoder();
}
/**
* SetEffort is used internally to set the motor effort without changing the control mode.
*/
void SetEffort(int16_t effort)
{
bool reverse = 0;
if (effort < 0)
{
effort = -effort; // Make speed a positive quantity.
reverse = 1; // Reverse the direction.
}
if (effort > maxEffort)
{
effort = maxEffort;
}
FastGPIO::Pin<DIR>::setOutput(reverse);
_SFR_MEM16(OCR) = effort;
}
void InitializeEncoder(void)
{
Serial.println("InitEnc()");
// Set the pins as pulled-up inputs.
FastGPIO::Pin<encXOR>::setInputPulledUp();
FastGPIO::Pin<encB>::setInputPulledUp();
// Initialize the variables so that the speed will start as 0
lastB = FastGPIO::Pin<encB>::isInputHigh();
lastA = FastGPIO::Pin<encXOR>::isInputHigh() ^ lastB;
Serial.println("/InitEnc()");
}
public:
void ProcessEncoderTick(void)
{
bool newB = FastGPIO::Pin<encB>::isInputHigh();
bool newA = FastGPIO::Pin<encXOR>::isInputHigh() ^ newB;
encCount += (lastA ^ newB) - (newA ^ lastB);
lastA = newA;
lastB = newB;
}
friend class Chassis; // Allow Chassis to call protected methods directly
};
extern Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor;
extern Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor;

173
lib/Servo32u4/src/servo32u4.cpp Normal file
View file

@ -0,0 +1,173 @@
#include <servo32u4.h>
uint16_t Servo32U4Base::setMinMaxMicroseconds(uint16_t min, uint16_t max)
{
// swap if in the wrong place
if(min > max) {uint16_t temp = min; min = max; max = temp;}
usMin = min;
usMax = max;
return usMax - usMin; //return the range, in case the user wants to do a sanity check
}
void Servo32U4Pin5::attach(void)
{
pinMode(5, OUTPUT); // set pin as OUTPUT
cli();
// clear then set the OCR3A bits (pin 5)
TCCR3A = 0x82; //WGM
TCCR3B = 0x1A; //WGM + CS = 8
ICR3 = 39999; //20ms
OCR3A = 3000;
sei();
isAttached = true;
}
void Servo32U4Pin5::detach(void)
{
cli();
// clear the OCR3A bits
TCCR3A &= 0x7f; //cancel OCR3A
sei();
isAttached = false;
}
void Servo32U4Pin5::writeMicroseconds(uint16_t microseconds)
{
if (!isAttached)
{
attach();
}
microseconds = constrain(microseconds, usMin, usMax);
//prescaler is 8, so 1 timer count = 0.5 us
OCR3A = (microseconds << 1) - 1; // multiplies by 2
}
void Servo32U4Pin6::attach(void)
{
pinMode(6, OUTPUT); // set pin as OUTPUT
cli();
// Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
TCCR4C |= 0x05;
sei();
isAttached = true;
}
void Servo32U4Pin6::detach(void)
{
cli();
// Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
TCCR4C = 0x00;
sei();
isAttached = false;
}
// Resolution is 64 us; not great, but shouldn't be too constraining
void Servo32U4Pin6::writeMicroseconds(uint16_t microseconds)
{
if (!isAttached)
{
attach();
}
microseconds = constrain(microseconds, usMin, usMax);
//prescaler is 8, so 1 timer count = 64 us
OCR4D = (microseconds >> 6) - 1; // divides by 64
}
void Servo32U4Pin13::attach(void)
{
pinMode(13, OUTPUT); // set pin as OUTPUT
cli();
// Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
TCCR4A = 0x82;
sei();
isAttached = true;
}
void Servo32U4Pin13::detach(void)
{
cli();
// Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
TCCR4A = 0x00;
sei();
isAttached = false;
}
// Resolution is 64 us; not great, but shouldn't be too constraining
void Servo32U4Pin13::writeMicroseconds(uint16_t microseconds)
{
if (!isAttached)
{
attach();
}
microseconds = constrain(microseconds, usMin, usMax);
//prescaler is 8, so 1 timer count = 64 us
OCR4A = (microseconds >> 6) - 1; // divides by 64
}
void Servo32U4Pin12::attach(void)
{
pinMode(12, OUTPUT); // set pin as OUTPUT
cli();
// Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
TCCR4C |= 0x05;
sei();
isAttached = true;
}
void Servo32U4Pin12::detach(void)
{
cli();
// Be careful here, since Timer4 is used to manage speed controller. See Chassis::init()
TCCR4C = 0x00;
sei();
isAttached = false;
}
// Resolution is 64 us; not great, but shouldn't be too constraining
void Servo32U4Pin12::writeMicroseconds(uint16_t microseconds)
{
if (!isAttached)
{
attach();
}
microseconds = constrain(microseconds, usMin, usMax);
//prescaler is 8, so 1 timer count = 64 us
OCR4D = 250 - (microseconds >> 6) - 1; // divides by 64
}

172
lib/Servo32u4/src/servo32u4.h Normal file
View file

@ -0,0 +1,172 @@
#pragma once
#include <Arduino.h>
/** \file Manages servos on up to three pins:
* pin 5,
* pin 13, and
* either pin 6 OR 12 (don't use 6 and 12 simultaneously!).
*
* Pin 5 uses Timer3 and has the fewest restrictions as well as the highest precision (0.5 us).
*
* The other three have potential conflicts and much lower resolution (64 us).
*
* Pin 13 uses Timer4, but shares functionality with the bootloader, which flashes the LED connected
* to that pin. So, your servo will go a bit nuts when you upload -- be careful!
*
* Pins 6 and 12 use Timer4, but share the same output compare -- except that they are inverted.
* Practically, that means you can use one or the other. Note that pin 6 is shared with the buzzer,
* so you'll need to cut the buzzer trace if you want to use that pin and maintain your sanity.
*
* If using the Chassis class, Timer4 is used for the control loop, so be sure you don't create any
* conflicts.
*
* You can either call writeMicroseconds directly (which will make it move really fast), or
* set the target and call update() regularly.
*
* */
/** \class Servo32U4Base
* \brief Base class class for servos.
*
* Each derived class controls a specific pin (obvious from the name).
*
* Defaults to a range of 1000 - 2000 us, but can be customized.
*/
class Servo32U4Base
{
protected:
uint16_t usMin = 1000;
uint16_t usMax = 2000;
uint8_t feedbackPin = -1;
bool isAttached = false;
uint16_t targetPos = 1500;
uint16_t currentPos = 1500;
public:
// Virtual functions defined for each specific class
virtual void attach(void) = 0;
virtual void detach(void) = 0;
void setTargetPos(uint16_t target) {targetPos = target;}
void update(void)
{
if(targetPos == currentPos) return; // no need to update
else if(abs(targetPos - currentPos) <= 40) currentPos = targetPos;
else if(targetPos > currentPos) currentPos += 40;
else if(targetPos < currentPos) currentPos -= 40;
writeMicroseconds(currentPos);
}
uint16_t setMinMaxMicroseconds(uint16_t min, uint16_t max);
virtual void writeMicroseconds(uint16_t microseconds) = 0;
};
/** \class Servo32U4Pin5
* \brief A servo class to control a servo on pin 5.
*
* Servo32U4 uses output compare on Timer3 to control the pulse to the servo.
* The 16-bit Timer3 is set up with a pre-scaler of 8, TOP of 39999 + 1 => 20 ms interval.
*
* OCR3A controls the pulse on pin 5 -- this servo must be on pin 5!
*
* Defaults to a range of 1000 - 2000 us, but can be customized.
*/
class Servo32U4Pin5 : public Servo32U4Base
{
public:
void attach(void);
void detach(void);
protected:
void writeMicroseconds(uint16_t microseconds);
};
/** \class Servo32U4Pin6
* \brief A servo class to control a servo on pin 6.
*
* Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo.
* _In Chassis::init(),
* The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval.
*
* YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK,
* AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
*
* OCR4D controls the pulse on pin 6 -- this servo must be on pin 6!
*
* Note that pin 6 controls the buzzer, so you'll go crazy if you don't cut the buzzer trace.
* See: https://www.pololu.com/docs/0J69/3.2 for how to cut the trace.
*
* Defaults to a range of 1000 - 2000 us, but can be customized.
*
* Note that because we're using an 8-bit timer, resolution is only 64 us.
*/
class Servo32U4Pin6 : public Servo32U4Base
{
public:
void attach(void);
void detach(void);
protected:
void writeMicroseconds(uint16_t microseconds);
};
/** \class Servo32U4Pin13
* \brief A servo class to control a servo on pin 13.
*
* Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo.
* _In Chassis::init(),
* The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval.
*
* YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK,
* AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
*
* OCR4A controls the pulse on pin 13 -- this servo must be on pin 13!
*
* Note that there is a useful LED on pin 13 -- you'll lose that functionality.
*
* Pin 13 is also used during the upload process, so your servo will go crazy when uploading!
*
* Defaults to a range of 1000 - 2000 us, but can be customized.
*
* Note that because we're using an 8-bit timer, resolution is only 64 us.
*/
class Servo32U4Pin13 : public Servo32U4Base
{
public:
void attach(void);
void detach(void);
protected:
void writeMicroseconds(uint16_t microseconds);
};
/** \class Servo32U4Pin12
* \brief A servo class to control a servo on pin 12.
*
* Servo32U4Pin12 uses output compare on Timer4 to control the pulse to the servo.
* _In Chassis::init(),
*
* YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK,
* AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
*
* ^OCR4D controls the pulse on pin 12 -- this servo _must_ be on pin 12!
*
* DO NOT USE IN CONJUNCTION WITH Servo32U4Pin6
*
* Defaults to a range of 1000 - 2000 us, but can be customized.
*
* Note that because we're using an 8-bit timer, resolution is only 64 us.
*/
class Servo32U4Pin12 : public Servo32U4Base
{
public:
void attach(void);
void detach(void);
protected:
void writeMicroseconds(uint16_t microseconds);
};

28
platformio.ini Normal file
View file

@ -0,0 +1,28 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:a-star32U4]
platform = atmelavr
board = a-star32U4
framework = arduino
monitor_speed = 250000
lib_deps =
Wire
https://github.com/gcl8a/IRDecoder
https://github.com/gcl8a/event_timer
; https://github.com/gcl8a/LSM6
https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities
build_flags =
; -D__MOTOR_DEBUG__
; -D__LOOP_DEBUG__
; -D__IMU_DEBUG__

21
src/main.cpp Normal file
View file

@ -0,0 +1,21 @@
#include <Arduino.h>
#include <LSM6.h>
#include "robot.h"
Robot robot;
void setup()
{
Serial.begin(250000);
#ifdef __LOOP_DEBUG__
while(!Serial){delay(5);}
#endif
robot.InitializeRobot();
}
void loop()
{
robot.RobotLoop();
}

52
src/map.h Normal file
View file

@ -0,0 +1,52 @@
/**
* A Map is a graph of intersections (nodes) and streets (edges). To save memory, we store
* the map as a list of "turns". Every time you get to an intersection, you know where you
* came from and where you want to go. You then calculate the angle at which you need to
* turn from the directions of the roads from the current intersection.
*/
#pragma once
#include <Arduino.h>
// #include <TList.h>
// struct Turn
// {
// Intersection to;
// int16_t direction; //direction to head from the intersection
// };
// struct Intersection
// {
// /**
// * List the reachable intersections and the direction of the street (absolute).
// */
// TList<Turn> turns;
// };
// class Map
// {
// private:
// TList<Intersection> intersections;
// public:
// int16_t calcAngle(Intersection from, Intersection at, Intersection to);
// };
PROGMEM const uint8_t directions[][4] =
{
{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0},
{0, 0, 0, 0}
};
int16_t CalcTurn(uint8_t from, uint8_t at, uint8_t to)
{
int16_t currentHeading = directions[at][from] - 180;
int16_t desiredHeading = directions[at][to];
int16_t turnAngle = desiredHeading - currentHeading;
return turnAngle;
}

171
src/robot-remote.cpp Normal file
View file

@ -0,0 +1,171 @@
/**
* robot-remote.cpp implements features of class Robot that are related to processing
* remote control commands. It also manages modes. You might want to trim away some of
* the code that isn't needed later in the term.
*/
#include "robot.h"
#include <ir_codes.h>
#include <IRdecoder.h>
/**
* IRDecoder decoder is declared extern in IRdecoder.h (for ISR purposes),
* so we _must_ name it decoder.
*/
#define IR_PIN 17
IRDecoder decoder(IR_PIN);
void Robot::HandleKeyCode(int16_t keyCode)
{
Serial.println(keyCode);
// Regardless of current state, if ENTER is pressed, go to idle state
if(keyCode == STOP_MODE) EnterIdleState();
// The SETUP key is used for tuning motor gains
else if(keyCode == SETUP_BTN)
{
if(robotCtrlMode == CTRL_SETUP) {EnterTeleopMode(); EnterIdleState();}
else {EnterSetupMode(); EnterIdleState();}
}
// If PLAY is pressed, it toggles control mode (setup -> teleop)
else if(keyCode == PLAY_PAUSE)
{
if(robotCtrlMode == CTRL_AUTO) {EnterTeleopMode(); EnterIdleState();}
else if(robotCtrlMode == CTRL_TELEOP) {EnterAutoMode(); EnterIdleState();}
}
/**
* AUTO commands
*/
if(robotCtrlMode == CTRL_AUTO)
{
switch(keyCode)
{
case REWIND:
EnterLineFollowing(keyString.toInt());
keyString = "";
break;
case NUM_1:
case NUM_2:
case NUM_3:
keyString += (char)(keyCode + 33);
break;
case NUM_4:
case NUM_5:
case NUM_6:
keyString += (char)(keyCode + 32);
break;
case NUM_7:
case NUM_8:
case NUM_9:
keyString += (char)(keyCode + 31);
break;
case NUM_0_10:
keyString += '0';
break;
}
}
/**
* TELEOP commands
*/
else if(robotCtrlMode == CTRL_TELEOP)
{
switch(keyCode)
{
case UP_ARROW:
chassis.SetTwist(5, 0);
break;
case RIGHT_ARROW:
chassis.SetTwist(0, -0.25);
break;
case DOWN_ARROW:
chassis.SetTwist(-5, 0);
break;
case LEFT_ARROW:
chassis.SetTwist(0, 0.25);
break;
case ENTER_SAVE:
chassis.SetTwist(0, 0);
break;
}
}
/**
* SETUP mode
*/
else if(robotCtrlMode == CTRL_SETUP)
{
float k = 0;
switch(keyCode)
{
case VOLminus:
k = keyString.toInt() / 100.0;
Serial.print("Kp = "); Serial.println(k);
chassis.SetMotorKp(k);
keyString = "";
break;
case PLAY_PAUSE:
k = keyString.toInt() / 100.0;
Serial.print("Ki = "); Serial.println(k);
chassis.SetMotorKi(k);
keyString = "";
break;
case VOLplus:
k = keyString.toInt() / 100.0;
Serial.print("Kd = "); Serial.println(k);
chassis.SetMotorKd(k);
keyString = "";
break;
case UP_ARROW:
if(!keyString.length()) chassis.SetWheelSpeeds(60, 0);
break;
case DOWN_ARROW:
if(!keyString.length()) chassis.SetWheelSpeeds(20, 0);
break;
case ENTER_SAVE:
keyString = "";
chassis.SetWheelSpeeds(0, 0);
break;
case NUM_1:
case NUM_2:
case NUM_3:
keyString += (char)(keyCode + 33);
break;
case NUM_4:
case NUM_5:
case NUM_6:
keyString += (char)(keyCode + 32);
break;
case NUM_7:
case NUM_8:
case NUM_9:
keyString += (char)(keyCode + 31);
break;
case NUM_0_10:
keyString += '0';
break;
}
}
}
void Robot::EnterTeleopMode(void)
{
Serial.println("-> TELEOP");
robotCtrlMode = CTRL_TELEOP;
}
void Robot::EnterAutoMode(void)
{
Serial.println("-> AUTO");
robotCtrlMode = CTRL_AUTO;
}
void Robot::EnterSetupMode(void)
{
Serial.println("-> SETUP");
robotCtrlMode = CTRL_SETUP;
}

214
src/robot.cpp Normal file
View file

@ -0,0 +1,214 @@
#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();
/**
* 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;
/**
* TODO: Add code to initiate the turn and set the target
*/
}
bool Robot::CheckTurnComplete(void)
{
bool retVal = false;
/**
* TODO: add a checker to detect when the turn is complete
*/
return retVal;
}
void Robot::HandleTurnComplete(void)
{
/**
* TODO: Add code to handle the completed turn
*/
}
/**
* 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
{
// TODO: update the orientation
}
#ifdef __IMU_DEBUG__
Serial.println(eulerAngles.z);
#endif
}
/**
* Functions related to line following and intersection detection.
*/
void Robot::EnterLineFollowing(float speed)
{
Serial.println(" -> LINING");
baseSpeed = speed;
robotState = ROBOT_LINING;
}
void Robot::LineFollowingUpdate(void)
{
if(robotState == ROBOT_LINING)
{
// TODO: calculate the error in CalcError(), calc the effort, and update the motion
int16_t lineError = lineSensor.CalcError();
float turnEffort = 0;
chassis.SetTwist(baseSpeed, turnEffort);
}
}
/**
* 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();
}
}

102
src/robot.h Normal file
View file

@ -0,0 +1,102 @@
#pragma once
#include "chassis.h"
#include <LineSensor.h>
#include <LSM6.h>
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;
/**
* 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_STATE robotState = ROBOT_IDLE;
/* Define the chassis*/
Chassis chassis;
/* Line sensor */
LineSensor lineSensor;
/* To add later: rangefinder, camera, etc.*/
// 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;
/**
* 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);
};

11
test/README Normal file
View file

@ -0,0 +1,11 @@
This directory is intended for PlatformIO Test Runner and project tests.
Unit Testing is a software testing method by which individual units of
source code, sets of one or more MCU program modules together with associated
control data, usage procedures, and operating procedures, are tested to
determine whether they are fit for use. Unit testing finds problems early
in the development cycle.
More information about PlatformIO Unit Testing:
- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html