From 68b52a9b1425a4b5dce601595cf341f33ed6d5c1 Mon Sep 17 00:00:00 2001 From: Griffin Tabor Date: Tue, 2 Sep 2025 18:33:50 -0400 Subject: [PATCH] Initial commit --- .gitignore | 2 + include/README | 39 +++ lib/Chassis/src/chassis-params.h | 7 + lib/Chassis/src/chassis.cpp | 183 ++++++++++++ lib/Chassis/src/chassis.h | 61 ++++ lib/Chassis/src/utils.cpp | 10 + lib/Chassis/src/utils.h | 32 +++ lib/README | 46 +++ lib/Romi32U4Motors/README.md | 0 .../src/Romi32U4MotorParameters.h | 5 + .../src/Romi32U4MotorTemplate.cpp | 18 ++ .../src/Romi32U4MotorTemplate.h | 269 ++++++++++++++++++ lib/Servo32u4/src/servo32u4.cpp | 185 ++++++++++++ lib/Servo32u4/src/servo32u4.h | 182 ++++++++++++ platformio.ini | 29 ++ src/main.cpp | 25 ++ src/robot-nav.cpp | 72 +++++ src/robot-remote.cpp | 83 ++++++ src/robot.cpp | 62 ++++ src/robot.h | 64 +++++ test/README | 11 + 21 files changed, 1385 insertions(+) create mode 100644 .gitignore create mode 100644 include/README create mode 100644 lib/Chassis/src/chassis-params.h create mode 100644 lib/Chassis/src/chassis.cpp create mode 100644 lib/Chassis/src/chassis.h create mode 100644 lib/Chassis/src/utils.cpp create mode 100644 lib/Chassis/src/utils.h create mode 100644 lib/README create mode 100644 lib/Romi32U4Motors/README.md create mode 100644 lib/Romi32U4Motors/src/Romi32U4MotorParameters.h create mode 100644 lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp create mode 100644 lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h create mode 100644 lib/Servo32u4/src/servo32u4.cpp create mode 100644 lib/Servo32u4/src/servo32u4.h create mode 100644 platformio.ini create mode 100644 src/main.cpp create mode 100644 src/robot-nav.cpp create mode 100644 src/robot-remote.cpp create mode 100644 src/robot.cpp create mode 100644 src/robot.h create mode 100644 test/README diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..80e7464 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.vscode +.pio 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-params.h b/lib/Chassis/src/chassis-params.h new file mode 100644 index 0000000..09a43bb --- /dev/null +++ b/lib/Chassis/src/chassis-params.h @@ -0,0 +1,7 @@ +/** + * TODO: Adjust these to get good FK updates. They're not horrible, but neither are they good. + */ + +const float ROBOT_RADIUS = 7; +const float LEFT_TICKS_PER_CM = 50; +const float RIGHT_TICKS_PER_CM = 50; diff --git a/lib/Chassis/src/chassis.cpp b/lib/Chassis/src/chassis.cpp new file mode 100644 index 0000000..c30d9a3 --- /dev/null +++ b/lib/Chassis/src/chassis.cpp @@ -0,0 +1,183 @@ +#include "chassis.h" +#include "Romi32U4MotorTemplate.h" + +Romi32U4EncodedMotor leftMotor("L"); +Romi32U4EncodedMotor rightMotor("R"); + +/** + * Because it's declared static, we initialize Chassis::loopFlag here. + */ +uint8_t Chassis::loopFlag = 0; + +/** + * For taking snapshots and raising the flag. + */ +void Chassis::Timer4OverflowISRHandler(void) +{ + loopFlag++; + + leftMotor.speed = leftMotor.CalcEncoderDelta(); + rightMotor.speed = rightMotor.CalcEncoderDelta(); +} + +/** + * 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(); +} + +/** + * 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"); +} + +void Chassis::InititalizeChassis(void) +{ + InitializeMotorControlTimer(); + InitializeMotors(); +} + +/** + * The main Chassis loop. + */ +bool Chassis::ChassisLoop(Twist& velocity) +{ + bool retVal = false; + + if(loopFlag) + { + if(loopFlag > 1) Serial.println("Missed an update in Robot::RobotLoop()!"); + +#ifdef __LOOP_DEBUG__ + Serial.print(millis()); + Serial.print('\n'); +#endif + + // motor updates + UpdateMotors(); + + /* Update the wheel velocity so it gets back to Robot. */ + velocity = CalcOdomFromWheelMotion(); + + loopFlag = 0; + + retVal = true; + } + + return retVal; +} + +/** + * Some motor methods. + */ +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); +} + +void Chassis::UpdateMotors(void) +{ + leftMotor.ControlMotorSpeed(); + rightMotor.ControlMotorSpeed(); +} + +/** + * SetWheelSpeeds converts the linear wheel speeds (axes relative to ground) to motor speeds. + */ +void Chassis::SetWheelSpeeds(float leftSpeedCMperSec, float rightSpeedCMperSec) +{ + /** + * TODO: Check the code below. You did this in Lab 1, so we give you the calcs. + */ + leftMotor.SetTargetSpeed(leftSpeedCMperSec * LEFT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.); + rightMotor.SetTargetSpeed(rightSpeedCMperSec * RIGHT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.); +} + +void Chassis::SetTwist(const Twist& twist) +{ + /** + * TODO: Complete SetTwist() to call SetWheelSpeeds() from target u and omega + */ +} + +Twist Chassis::CalcOdomFromWheelMotion(void) +{ + Twist velocity; + /** + * TODO: Calculate velocities from wheel motion, which are held in leftMotor.spped and rightMotor.speed. + * Note that you might want to calculate the deltas instead of speeds (to save some floating point maths). + * + * In that case, you should return a Pose instead of a Twist. + */ + +#ifdef __NAV_DEBUG__ + TeleplotPrint("u", velocity.u); + TeleplotPrint("omega", velocity.omega); +#endif + + return velocity; +} diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h new file mode 100644 index 0000000..c6ecd50 --- /dev/null +++ b/lib/Chassis/src/chassis.h @@ -0,0 +1,61 @@ +#pragma once + +#include + +#include "chassis-params.h" +#include "utils.h" + +class Chassis +{ +protected: + + /** + * 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; + + /** + * 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); + + /* Where the bulk of the work for the motors gets done. */ + bool ChassisLoop(Twist&); + + /* Needed for managing motors. */ + static void Timer4OverflowISRHandler(void); + +public: + /** + * SetTwist eliminates the need for DriveAt and TurnAt + */ + void SetTwist(const Twist& twist); + Twist CalcOdomFromWheelMotion(void); + + /** + * A utility function for converting robot speed to wheel speed. Left + * public so that you can test more easily. + */ + void SetWheelSpeeds(float, float); + void Stop(void) { SetWheelSpeeds(0, 0); } + +protected: + /** + * Initialization and Setup routines + */ + void InitializeMotorControlTimer(void); + void InitializeMotors(void); + + void UpdateMotors(void); + void SetMotorEfforts(int16_t, int16_t); +}; diff --git a/lib/Chassis/src/utils.cpp b/lib/Chassis/src/utils.cpp new file mode 100644 index 0000000..12f34f0 --- /dev/null +++ b/lib/Chassis/src/utils.cpp @@ -0,0 +1,10 @@ +#include "utils.h" + +void TeleplotPrint(const char* var, float value) +{ + Serial.print('>'); + Serial.print(var); + Serial.print(':'); + Serial.print(value); + Serial.print('\n'); +} diff --git a/lib/Chassis/src/utils.h b/lib/Chassis/src/utils.h new file mode 100644 index 0000000..391aaee --- /dev/null +++ b/lib/Chassis/src/utils.h @@ -0,0 +1,32 @@ +#pragma once +#include + +void TeleplotPrint(const char* var, float value); + +/** + * Pose includes information about the 2D pose of a robot: x, y, and heading. + */ +struct Pose +{ + float x = 0; + float y = 0; + float theta = 0; + + Pose(void) {} + Pose(float x_, float y_, float th_) : x(x_), y(y_), theta(th_) {} +}; + +/** + * Twist is very similar to Pose, but we make a separate struct to avoid confusion. + * + * Whereas Pose is position/heading, Twist contains velocity/ang. vel. + */ +struct Twist +{ + float u = 0; + float v = 0; // This will always be 0 in the robot frame. + float omega = 0; + + Twist(void) {} + Twist(float u_, float v_, float om_) : u(u_), v(v_), omega(om_) {} +}; 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/Romi32U4MotorParameters.h b/lib/Romi32U4Motors/src/Romi32U4MotorParameters.h new file mode 100644 index 0000000..76f2edd --- /dev/null +++ b/lib/Romi32U4Motors/src/Romi32U4MotorParameters.h @@ -0,0 +1,5 @@ +#define KP_MOTOR 5.0 +#define KI_MOTOR 0.5 +#define KD_MOTOR 0.0 // don't use + +#define INTEGRAL_CAP_MOTOR 0.0 // if you want to add an integral cap \ No newline at end of file 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..a7777c7 --- /dev/null +++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h @@ -0,0 +1,269 @@ +#pragma once + +#include +#include +#include "Romi32U4MotorParameters.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: + // For prettier printing + const String name; + + // Used to control the motors in different ways + enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED}; + volatile CTRL_MODE ctrlMode = CTRL_DIRECT; + + // Used to manage PID coefficients; note that the defaults stink for speed control. + float Kp = KP_MOTOR; + float Ki = KI_MOTOR; + float Kd = KD_MOTOR; + + // 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; + + /** + * Sets the duty cycle. Must be declared for specific motor. + */ + 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(void) + { + if(ctrlMode == CTRL_SPEED) + { + /** + * TODO: Implement PI controller. Only P at the moment. + */ + + // Calculate the error in speed + float error = targetSpeed - speed; + + sumError+= error; + + int16_t effort = Kp * error + Ki * sumError; + + // Set the effort for the motor + SetEffort(effort); + +#ifdef __MOTOR_DEBUG__ + Serial.print('>'); + Serial.print(name); + Serial.print("_target:"); + Serial.println(targetSpeed); + Serial.print('>'); + Serial.print(name); + Serial.print("_speed:"); + Serial.println(speed); + Serial.print('>'); + Serial.print(name); + Serial.print("_error:"); + Serial.println(error); +#endif + } + } + + 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: + Romi32U4MotorBase(const String& nm) : name(nm) {} + 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); + + /** + * This line is a little esoteric, but it sets the duty cycle for the appropriate PWM pin. + * Note that TOP for the PWM is 420, so the duty cycle will be (effort / 420) * 100 (in %). + */ + _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: + Romi32U4EncodedMotor(const String& name) : Romi32U4MotorBase(name) {} + 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..3360e18 --- /dev/null +++ b/lib/Servo32u4/src/servo32u4.cpp @@ -0,0 +1,185 @@ +#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 + + 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 &= ~0x05; + + 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 512, so 1 timer count = 32 us + //but be sure to set TC4H first! + TC4H = 0; + OCR4D = (microseconds >> 5) - 1; // divides by 32 +} + +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() + // If you're not using it for anything else, you can safely just set TCCR4A = 0, + // but we'll cancel the bits we set above + TCCR4A &= ~0x82; + + 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 512, so 1 timer count = 32 us + //but be sure to set TC4H first! + TC4H = 0; + OCR4A = (microseconds >> 5) - 1; // divides by 32 +} + +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; + + //Invert the output on pin 12 + TCCR4B |= 0x80; + + sei(); + + isAttached = true; +} + +void Servo32U4Pin12::detach(void) +{ + cli(); + + // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() + TCCR4C = 0x00; + TCCR4B &= ~0x80; + + 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 512, so 1 timer count = 32 us + //but be sure to set TC4H first! + TC4H = 0; + OCR4D = (microseconds >> 5) - 1; // divides by 32 +} diff --git a/lib/Servo32u4/src/servo32u4.h b/lib/Servo32u4/src/servo32u4.h new file mode 100644 index 0000000..3ef5f1b --- /dev/null +++ b/lib/Servo32u4/src/servo32u4.h @@ -0,0 +1,182 @@ +#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!). + * + * THIS IS DESIGNED TO WORK WITH THE ROMI SET UP WITH THE CHASSIS CLASS + * + * The Chassis class sets up Timer4, which is needed for pins 6, 12, and 13! + * + * 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. + * + * writeMicroseconds is protected so that you don't call it directly (which will make it move + * really fast). Instead, 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;} + + /** + * update() moves the servo towards the target position. You will want to + * change it to return a bool to detect the event of reaching the target. + */ + void update(void) + { + if(targetPos == currentPos) {} // 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..293d59e --- /dev/null +++ b/platformio.ini @@ -0,0 +1,29 @@ +; 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 = 115200 + +lib_deps = + Wire + https://github.com/gcl8a/IRDecoder + https://github.com/gcl8a/Button + https://github.com/gcl8a/event_timer + https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities + +build_flags = +; -D__MOTOR_DEBUG__ +; -D__LOOP_DEBUG__ +; -D__IMU_DEBUG__ + -D__NAV_DEBUG__ diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..84d72f8 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,25 @@ +#include +#include "robot.h" + +Robot robot; + +void setup() +{ + Serial.begin(115200); + +/** + * If you define __SETUP_DEBUG__ in your .ini file, this line will make the program wait + * until the Serial is set up so you can debug. WARNING: If you do that, you _must_ open + * the Serial Monitor for anything to happen! + */ +#ifdef __SETUP_DEBUG__ + while(!Serial){delay(5);} +#endif + + robot.InitializeRobot(); +} + +void loop() +{ + robot.RobotLoop(); +} diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp new file mode 100644 index 0000000..82c1a46 --- /dev/null +++ b/src/robot-nav.cpp @@ -0,0 +1,72 @@ +/** + * robot-nav.cpp is where you should put navigation routines. + */ + +#include "robot.h" + +void Robot::UpdatePose(const Twist& twist) +{ + /** + * TODO: Add your FK algorithm to update currPose here. + */ + +#ifdef __NAV_DEBUG__ + TeleplotPrint("x", currPose.x); + TeleplotPrint("y", currPose.y); + TeleplotPrint("theta", currPose.theta); +#endif + +} + +/** + * Sets a destination in the lab frame. + */ +void Robot::SetDestination(const Pose& dest) +{ + /** + * TODO: Turn on LED, as well. + */ + Serial.print("Setting dest to: "); + Serial.print(dest.x); + Serial.print(", "); + Serial.print(dest.y); + Serial.print('\n'); + + destPose = dest; + robotState = ROBOT_DRIVE_TO_POINT; +} + +bool Robot::CheckReachedDestination(void) +{ + bool retVal = false; + /** + * TODO: Add code to check if you've reached destination here. + */ + + return retVal; +} + +void Robot::DriveToPoint(void) +{ + if(robotState == ROBOT_DRIVE_TO_POINT) + { + /** + * TODO: Add your IK algorithm here. + */ + +#ifdef __NAV_DEBUG__ + // Print useful stuff here. +#endif + + /** + * TODO: Call chassis.SetTwist() to command the motion, based on your calculations above. + */ + } +} + +void Robot::HandleDestination(void) +{ + /** + * TODO: Stop and change state. Turn off LED. + */ +} \ No newline at end of file diff --git a/src/robot-remote.cpp b/src/robot-remote.cpp new file mode 100644 index 0000000..73bb953 --- /dev/null +++ b/src/robot-remote.cpp @@ -0,0 +1,83 @@ +/** + * 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 == ENTER_SAVE) { EnterIdleState(); keyString = "";} + + + // If STOP/MODE is pressed, it toggles control mode (auto <-> teleop) + else if(keyCode == STOP_MODE) + { + if(robotCtrlMode == CTRL_AUTO) {EnterTeleopMode(); } + else if(robotCtrlMode == CTRL_TELEOP) {EnterAutoMode(); } + EnterIdleState(); // Idle to avoid surprises + keyString = ""; + } + + if(robotCtrlMode == CTRL_AUTO) + { + /** + * TODO: Add destinations. + */ + switch(keyCode) + { + case NUM_2: + SetDestination(Pose(60, 0, 0)); + break; + case NUM_4: + break; + case NUM_6: + break; + case NUM_8: + break; + } + keyString = ""; + } + + else if(robotCtrlMode == CTRL_TELEOP) + { + switch(keyCode) + { + case UP_ARROW: + chassis.SetTwist(Twist(20, 0, 0)); + break; + case RIGHT_ARROW: + break; + case DOWN_ARROW: + break; + case LEFT_ARROW: + break; + } + keyString = ""; + } +} + +void Robot::EnterTeleopMode(void) +{ + Serial.println("-> TELEOP"); + robotCtrlMode = CTRL_TELEOP; +} + +void Robot::EnterAutoMode(void) +{ + Serial.println("-> AUTO"); + robotCtrlMode = CTRL_AUTO; +} diff --git a/src/robot.cpp b/src/robot.cpp new file mode 100644 index 0000000..03d5e61 --- /dev/null +++ b/src/robot.cpp @@ -0,0 +1,62 @@ +#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(); + + /** + * TODO: Set pin 13 HIGH when navigating and LOW when destination is reached. + * Need to set as OUTPUT here. + */ +} + +void Robot::EnterIdleState(void) +{ + chassis.Stop(); + + Serial.println("-> IDLE"); + robotState = ROBOT_IDLE; +} + +/** + * The main loop for your robot. Process both synchronous events (motor control), + * and asynchronous events (IR presses, distance readings, etc.). +*/ +void Robot::RobotLoop(void) +{ + /** + * Handle any IR remote keypresses. + */ + int16_t keyCode = decoder.getKeyCode(); + if(keyCode != -1) HandleKeyCode(keyCode); + + /** + * Run the chassis loop, which handles low-level control. + */ + Twist velocity; + if(chassis.ChassisLoop(velocity)) + { + // We do FK regardless of state + UpdatePose(velocity); + + /** + * Here, we break with tradition and only call these functions if we're in the + * DRIVE_TO_POINT state. CheckReachedDestination() is expensive, so we don't want + * to do all the maths when we don't need to. + * + * While we're at it, we'll toss DriveToPoint() in, as well. + */ + if(robotState == ROBOT_DRIVE_TO_POINT) + { + DriveToPoint(); + if(CheckReachedDestination()) HandleDestination(); + } + } +} diff --git a/src/robot.h b/src/robot.h new file mode 100644 index 0000000..eee7c0a --- /dev/null +++ b/src/robot.h @@ -0,0 +1,64 @@ +#pragma once + +#include "chassis.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, + }; + ROBOT_CTRL_MODE robotCtrlMode = CTRL_AUTO; + + /** + * 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_DRIVE_TO_POINT, + }; + ROBOT_STATE robotState = ROBOT_IDLE; + + /* Define the chassis*/ + Chassis chassis; + + // For managing key presses + String keyString; + + /** + * For tracking current pose and the destination. + */ + Pose currPose; + Pose destPose; + +public: + Robot(void) {keyString.reserve(10);} + 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); + + // /* Navigation methods.*/ + void UpdatePose(const Twist& u); + void SetDestination(const Pose& destination); + void DriveToPoint(void); + bool CheckReachedDestination(void); + void HandleDestination(void); +}; 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