commit 0534981d4b3de8beb7a69f9ac09814fcc42c58cc Author: github-classroom[bot] <66690702+github-classroom[bot]@users.noreply.github.com> Date: Tue Oct 22 16:03:16 2024 +0000 Initial commit diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..42f8df3 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.vscode +.pio \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..c225540 --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# Starter-Code + diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -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 diff --git a/lib/Chassis/src/chassis.cpp b/lib/Chassis/src/chassis.cpp new file mode 100644 index 0000000..a9fcbe8 --- /dev/null +++ b/lib/Chassis/src/chassis.cpp @@ -0,0 +1,160 @@ +#include "chassis.h" +#include "Romi32U4MotorTemplate.h" + +Romi32U4EncodedMotor leftMotor; +Romi32U4EncodedMotor 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); +} \ No newline at end of file diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h new file mode 100644 index 0000000..1acbc8b --- /dev/null +++ b/lib/Chassis/src/chassis.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#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); +}; diff --git a/lib/LSM6/README.md b/lib/LSM6/README.md new file mode 100644 index 0000000..f5a8c3e --- /dev/null +++ b/lib/LSM6/README.md @@ -0,0 +1,2 @@ +# LSM6 + diff --git a/lib/LSM6/src/LSM6.cpp b/lib/LSM6/src/LSM6.cpp new file mode 100644 index 0000000..ac124c5 --- /dev/null +++ b/lib/LSM6/src/LSM6.cpp @@ -0,0 +1,355 @@ +#include +#include +#include + +// 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; + } +} \ No newline at end of file diff --git a/lib/LSM6/src/LSM6.h b/lib/LSM6/src/LSM6.h new file mode 100644 index 0000000..abff397 --- /dev/null +++ b/lib/LSM6/src/LSM6.h @@ -0,0 +1,175 @@ +#pragma once + +// Datasheet: https://www.pololu.com/file/0J1087/LSM6DS33.pdf + +#include + +class LSM6 +{ + public: + template 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 a; // accelerometer readings + vector 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 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 eulerAngles; + vector gyroBias; + + /* We make Robot a friend to avoid all the setters and getters. */ + friend class Robot; +}; diff --git a/lib/LineSensor/readme.md b/lib/LineSensor/readme.md new file mode 100644 index 0000000..e69de29 diff --git a/lib/LineSensor/src/LineSensor.cpp b/lib/LineSensor/src/LineSensor.cpp new file mode 100644 index 0000000..a92c8ac --- /dev/null +++ b/lib/LineSensor/src/LineSensor.cpp @@ -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; +} \ No newline at end of file diff --git a/lib/LineSensor/src/LineSensor.h b/lib/LineSensor/src/LineSensor.h new file mode 100644 index 0000000..ecba533 --- /dev/null +++ b/lib/LineSensor/src/LineSensor.h @@ -0,0 +1,21 @@ +#pragma once + +#include + +#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); +}; \ No newline at end of file diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/lib/README @@ -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 +#include + +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 diff --git a/lib/Romi32U4Motors/README.md b/lib/Romi32U4Motors/README.md new file mode 100644 index 0000000..e69de29 diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp new file mode 100644 index 0000000..23bf8de --- /dev/null +++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp @@ -0,0 +1,18 @@ +#include "Romi32U4MotorTemplate.h" +#include + +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); +} diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h new file mode 100644 index 0000000..3a27781 --- /dev/null +++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h @@ -0,0 +1,254 @@ +#pragma once + +#include +#include + +// 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 + class Romi32U4EncodedMotor : public Romi32U4MotorBase +{ +protected: + + void InitializeMotor(void) + { + FastGPIO::Pin::setOutputLow(); + FastGPIO::Pin::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::setOutput(reverse); + _SFR_MEM16(OCR) = effort; + } + + void InitializeEncoder(void) + { + Serial.println("InitEnc()"); + + // Set the pins as pulled-up inputs. + FastGPIO::Pin::setInputPulledUp(); + FastGPIO::Pin::setInputPulledUp(); + + // Initialize the variables so that the speed will start as 0 + lastB = FastGPIO::Pin::isInputHigh(); + lastA = FastGPIO::Pin::isInputHigh() ^ lastB; + + Serial.println("/InitEnc()"); + } + +public: + void ProcessEncoderTick(void) + { + bool newB = FastGPIO::Pin::isInputHigh(); + bool newA = FastGPIO::Pin::isInputHigh() ^ newB; + + encCount += (lastA ^ newB) - (newA ^ lastB); + + lastA = newA; + lastB = newB; + } + + friend class Chassis; // Allow Chassis to call protected methods directly +}; + +extern Romi32U4EncodedMotor leftMotor; +extern Romi32U4EncodedMotor rightMotor; diff --git a/lib/Servo32u4/src/servo32u4.cpp b/lib/Servo32u4/src/servo32u4.cpp new file mode 100644 index 0000000..926a49c --- /dev/null +++ b/lib/Servo32u4/src/servo32u4.cpp @@ -0,0 +1,173 @@ +#include + +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 +} diff --git a/lib/Servo32u4/src/servo32u4.h b/lib/Servo32u4/src/servo32u4.h new file mode 100644 index 0000000..27e5e3a --- /dev/null +++ b/lib/Servo32u4/src/servo32u4.h @@ -0,0 +1,172 @@ +#pragma once + +#include + +/** \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); +}; diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..9b7d47b --- /dev/null +++ b/platformio.ini @@ -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__ \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..4f159a5 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,21 @@ +#include +#include +#include "robot.h" + +Robot robot; + +void setup() +{ + Serial.begin(250000); + +#ifdef __LOOP_DEBUG__ + while(!Serial){delay(5);} +#endif + + robot.InitializeRobot(); +} + +void loop() +{ + robot.RobotLoop(); +} diff --git a/src/map.h b/src/map.h new file mode 100644 index 0000000..aa8d31e --- /dev/null +++ b/src/map.h @@ -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 +// #include + +// 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 turns; +// }; + +// class Map +// { +// private: +// TList 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; +} \ No newline at end of file diff --git a/src/robot-remote.cpp b/src/robot-remote.cpp new file mode 100644 index 0000000..1b2a86c --- /dev/null +++ b/src/robot-remote.cpp @@ -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 +#include + +/** + * 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; +} \ No newline at end of file diff --git a/src/robot.cpp b/src/robot.cpp new file mode 100644 index 0000000..7b57679 --- /dev/null +++ b/src/robot.cpp @@ -0,0 +1,214 @@ +#include "robot.h" +#include + +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(); + } +} + diff --git a/src/robot.h b/src/robot.h new file mode 100644 index 0000000..d7f6f33 --- /dev/null +++ b/src/robot.h @@ -0,0 +1,102 @@ +#pragma once +#include "chassis.h" +#include +#include + +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 prevEulerAngles; + LSM6::vector 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); +}; diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/test/README @@ -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