ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

canvas initial

Browse source
This commit is contained in:
Andy Killorin 2026-01-29 08:25:16 -05:00
parent 4a8952a5db
commit a126b577b7
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
18 changed files with 1166 additions and 1166 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
.gitignore vendored
View file

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

78
include/README
View file

@ -1,39 +1,39 @@
This directory is intended for project header files. This directory is intended for project header files.
A header file is a file containing C declarations and macro definitions 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 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 header file in your project source file (C, C++, etc) located in `src` folder
by including it, with the C preprocessing directive `#include'. by including it, with the C preprocessing directive `#include'.
```src/main.c ```src/main.c
#include "header.h" #include "header.h"
int main (void) int main (void)
{ {
... ...
} }
``` ```
Including a header file produces the same results as copying the header file 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 into each source file that needs it. Such copying would be time-consuming
and error-prone. With a header file, the related declarations appear 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 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 place, and programs that include the header file will automatically use the
new version when next recompiled. The header file eliminates the labor of 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 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. 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'. 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 It is most portable to use only letters, digits, dashes, and underscores in
header file names, and at most one dot. header file names, and at most one dot.
Read more about using header files in official GCC documentation: Read more about using header files in official GCC documentation:
* Include Syntax * Include Syntax
* Include Operation * Include Operation
* Once-Only Headers * Once-Only Headers
* Computed Includes * Computed Includes
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

14
lib/Chassis/src/chassis-params.h
View file

@ -1,7 +1,7 @@
/** /**
* TODO: Adjust these to get good FK updates. They're not horrible, but neither are they good. * TODO: Adjust these to get good FK updates. They're not horrible, but neither are they good.
*/ */
const float ROBOT_RADIUS = 7; const float ROBOT_RADIUS = 7;
const float LEFT_TICKS_PER_CM = 50; const float LEFT_TICKS_PER_CM = 50;
const float RIGHT_TICKS_PER_CM = 50; const float RIGHT_TICKS_PER_CM = 50;

312
lib/Chassis/src/chassis.cpp
View file

@ -1,156 +1,156 @@
#include "chassis.h" #include "chassis.h"
#include "Romi32U4MotorTemplate.h" #include "Romi32U4MotorTemplate.h"
Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor("L"); Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor("L");
Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor("R"); Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor("R");
/** /**
* Because it's declared static, we initialize Chassis::loopFlag here. * Because it's declared static, we initialize Chassis::loopFlag here.
*/ */
uint8_t Chassis::loopFlag = 0; uint8_t Chassis::loopFlag = 0;
/** /**
* For taking snapshots and raising the flag. * For taking snapshots and raising the flag.
*/ */
void Chassis::Timer4OverflowISRHandler(void) void Chassis::Timer4OverflowISRHandler(void)
{ {
loopFlag++; loopFlag++;
leftMotor.speed = leftMotor.CalcEncoderDelta(); leftMotor.speed = leftMotor.CalcEncoderDelta();
rightMotor.speed = rightMotor.CalcEncoderDelta(); rightMotor.speed = rightMotor.CalcEncoderDelta();
} }
/** /**
* ISR for timing. On Timer4 overflow, we take a 'snapshot' of the encoder counts * 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. * and raise a flag to let the program it is time to execute the PID calculations.
*/ */
ISR(TIMER4_OVF_vect) ISR(TIMER4_OVF_vect)
{ {
Chassis::Timer4OverflowISRHandler(); Chassis::Timer4OverflowISRHandler();
} }
/** /**
* Sets up a hardware timer on Timer4 to manage motor control on a precise schedule. * 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 * We set the timer to set an interrupt flag on overflow, which is handled
* by ISR(TIMER4_OVF_vect) below. * by ISR(TIMER4_OVF_vect) below.
*/ */
void Chassis::InitializeMotorControlTimer(void) void Chassis::InitializeMotorControlTimer(void)
{ {
Serial.println("InitTimer"); Serial.println("InitTimer");
// Disable interupts while we mess with the Timer4 registers // Disable interupts while we mess with the Timer4 registers
cli(); cli();
// Set up Timer4 // Set up Timer4
TCCR4A = 0x00; // Disable output to pins TCCR4A = 0x00; // Disable output to pins
TCCR4B = 0x0A; // Sets the prescaler -- see pp. 167-8 in datasheet TCCR4B = 0x0A; // Sets the prescaler -- see pp. 167-8 in datasheet
TCCR4C = 0x00; // Disables output to pins (but see below for buzzer) TCCR4C = 0x00; // Disables output to pins (but see below for buzzer)
TCCR4D = 0x00; // Normal mode: count up and roll-over TCCR4D = 0x00; // Normal mode: count up and roll-over
/** /**
* Calculate TOP based on prescaler and loop duration. Note that loop is in integer ms -- * 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. * 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 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. * 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. * 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 * Note that the maximum period is limited by TOP = 0x3FF. If you want
* a longer period, you'll need to adjust the pre-scaler. * a longer period, you'll need to adjust the pre-scaler.
* *
* There is no minumum period, but precision breaks down with low values, * There is no minumum period, but precision breaks down with low values,
* unless you adjust the pre-scaler, but the encoder resolution is limited, * unless you adjust the pre-scaler, but the encoder resolution is limited,
* so you only want to go so fast. * so you only want to go so fast.
*/ */
uint8_t highbits = top / 256; uint8_t highbits = top / 256;
uint8_t lowbits = top - highbits; uint8_t lowbits = top - highbits;
TC4H = highbits; OCR4C = lowbits; TC4H = highbits; OCR4C = lowbits;
// Enable overflow interrupt // Enable overflow interrupt
TIMSK4 = 0x04; TIMSK4 = 0x04;
/** /**
* Uncommenting the following lines will pipe the timer signal to pin 6, * 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 * 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 * allows you to check that the timing is correct. It will also make a lot
* of noise, so do so sparingly. * of noise, so do so sparingly.
*/ */
// TCCR4C = 0x04 // TCCR4C = 0x04
// pinMode(6, OUTPUT); // pinMode(6, OUTPUT);
// Re-enable interrupts // Re-enable interrupts
sei(); sei();
Serial.println("/InitTimer"); Serial.println("/InitTimer");
} }
void Chassis::InititalizeChassis(void) void Chassis::InititalizeChassis(void)
{ {
InitializeMotorControlTimer(); InitializeMotorControlTimer();
InitializeMotors(); InitializeMotors();
} }
/** /**
* The main Chassis loop. * The main Chassis loop.
*/ */
bool Chassis::ChassisLoop(Twist& velocity) bool Chassis::ChassisLoop(Twist& velocity)
{ {
bool retVal = false; bool retVal = false;
if(loopFlag) if(loopFlag)
{ {
if(loopFlag > 1) Serial.println("Missed an update in Robot::RobotLoop()!"); if(loopFlag > 1) Serial.println("Missed an update in Robot::RobotLoop()!");
#ifdef __LOOP_DEBUG__ #ifdef __LOOP_DEBUG__
Serial.print(millis()); Serial.print(millis());
Serial.print('\n'); Serial.print('\n');
#endif #endif
/* Update the wheel velocity so it gets back to Robot. */ /* Update the wheel velocity so it gets back to Robot. */
velocity = CalcOdomFromWheelMotion(); velocity = CalcOdomFromWheelMotion();
loopFlag = 0; loopFlag = 0;
retVal = true; retVal = true;
} }
return retVal; return retVal;
} }
/** /**
* Some motor methods. * Some motor methods.
*/ */
void Chassis::InitializeMotors(void) void Chassis::InitializeMotors(void)
{ {
Romi32U4MotorBase::InitializePWMTimerAndInterrupts(); Romi32U4MotorBase::InitializePWMTimerAndInterrupts();
leftMotor.InitializeMotor(); leftMotor.InitializeMotor();
rightMotor.InitializeMotor(); rightMotor.InitializeMotor();
} }
void Chassis::SetMotorEfforts(int16_t left, int16_t right) void Chassis::SetMotorEfforts(int16_t left, int16_t right)
{ {
leftMotor.SetMotorEffortDirect(left); leftMotor.SetMotorEffortDirect(left);
rightMotor.SetMotorEffortDirect(right); rightMotor.SetMotorEffortDirect(right);
} }
Twist Chassis::CalcOdomFromWheelMotion(void) Twist Chassis::CalcOdomFromWheelMotion(void)
{ {
Twist velocity; Twist velocity;
/** /**
* TODO: Calculate velocities from wheel motion, which are held in leftMotor.spped and rightMotor.speed. * 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). * 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. * In that case, you should return a Pose instead of a Twist.
*/ */
#ifdef __NAV_DEBUG__ #ifdef __NAV_DEBUG__
TeleplotPrint("u", velocity.u); TeleplotPrint("u", velocity.u);
TeleplotPrint("omega", velocity.omega); TeleplotPrint("omega", velocity.omega);
#endif #endif
return velocity; return velocity;
} }

108
lib/Chassis/src/chassis.h
View file

@ -1,54 +1,54 @@
#pragma once #pragma once
#include <Arduino.h> #include <Arduino.h>
#include "chassis-params.h" #include "chassis-params.h"
#include "utils.h" #include "utils.h"
class Chassis class Chassis
{ {
protected: protected:
/** /**
* You can change the control loop period, but you should use multiples of 4 ms to * You can change the control loop period, but you should use multiples of 4 ms to
* avoid rounding errors. * avoid rounding errors.
*/ */
const uint16_t CONTROL_LOOP_PERIOD_MS = 20; const uint16_t CONTROL_LOOP_PERIOD_MS = 20;
/** /**
* loopFlag is used to tell the program when to update. It is set when Timer4 * loopFlag is used to tell the program when to update. It is set when Timer4
* overflows (see InitializeMotorControlTimer). Some of the calculations are too * 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. * 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 * 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. * more than 1, then we missed a cycle.
*/ */
static uint8_t loopFlag; static uint8_t loopFlag;
public: public:
Chassis(void) {} Chassis(void) {}
void InititalizeChassis(void); void InititalizeChassis(void);
/* Where the bulk of the work for the motors gets done. */ /* Where the bulk of the work for the motors gets done. */
bool ChassisLoop(Twist&); bool ChassisLoop(Twist&);
/* Needed for managing motors. */ /* Needed for managing motors. */
static void Timer4OverflowISRHandler(void); static void Timer4OverflowISRHandler(void);
public: public:
Twist CalcOdomFromWheelMotion(void); Twist CalcOdomFromWheelMotion(void);
void SetMotorEfforts(int16_t, int16_t); void SetMotorEfforts(int16_t, int16_t);
void Stop(void) { SetMotorEfforts(0, 0);} void Stop(void) { SetMotorEfforts(0, 0);}
protected: protected:
/** /**
* Initialization and Setup routines * Initialization and Setup routines
*/ */
void InitializeMotorControlTimer(void); void InitializeMotorControlTimer(void);
void InitializeMotors(void); void InitializeMotors(void);
void UpdateMotors(void); void UpdateMotors(void);
}; };

20
lib/Chassis/src/utils.cpp
View file

@ -1,10 +1,10 @@
#include "utils.h" #include "utils.h"
void TeleplotPrint(const char* var, float value) void TeleplotPrint(const char* var, float value)
{ {
Serial.print('>'); Serial.print('>');
Serial.print(var); Serial.print(var);
Serial.print(':'); Serial.print(':');
Serial.print(value); Serial.print(value);
Serial.print('\n'); Serial.print('\n');
} }

64
lib/Chassis/src/utils.h
View file

@ -1,32 +1,32 @@
#pragma once #pragma once
#include <Arduino.h> #include <Arduino.h>
void TeleplotPrint(const char* var, float value); void TeleplotPrint(const char* var, float value);
/** /**
* Pose includes information about the 2D pose of a robot: x, y, and heading. * Pose includes information about the 2D pose of a robot: x, y, and heading.
*/ */
struct Pose struct Pose
{ {
float x = 0; float x = 0;
float y = 0; float y = 0;
float theta = 0; float theta = 0;
Pose(void) {} Pose(void) {}
Pose(float x_, float y_, float th_) : x(x_), y(y_), theta(th_) {} 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. * 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. * Whereas Pose is position/heading, Twist contains velocity/ang. vel.
*/ */
struct Twist struct Twist
{ {
float u = 0; float u = 0;
float v = 0; // This will always be 0 in the robot frame. float v = 0; // This will always be 0 in the robot frame.
float omega = 0; float omega = 0;
Twist(void) {} Twist(void) {}
Twist(float u_, float v_, float om_) : u(u_), v(v_), omega(om_) {} Twist(float u_, float v_, float om_) : u(u_), v(v_), omega(om_) {}
}; };

92
lib/README
View file

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

36
lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp
View file

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

408
lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
View file

@ -1,204 +1,204 @@
#pragma once #pragma once
#include <Arduino.h> #include <Arduino.h>
#include <FastGPIO.h> #include <FastGPIO.h>
// define the motor pins here // define the motor pins here
#define PWM_L 10 #define PWM_L 10
#define PWM_R 9 #define PWM_R 9
#define DIR_L 16 #define DIR_L 16
#define DIR_R 15 #define DIR_R 15
#define LEFT_XOR 8 #define LEFT_XOR 8
#define LEFT_B IO_E2 #define LEFT_B IO_E2
#define RIGHT_XOR 7 #define RIGHT_XOR 7
#define RIGHT_B 23 #define RIGHT_B 23
#define OCR_L 0x8A #define OCR_L 0x8A
#define OCR_R 0x88 #define OCR_R 0x88
class Romi32U4MotorBase class Romi32U4MotorBase
{ {
protected: protected:
// For prettier printing // For prettier printing
const String name; const String name;
/** /**
* This is the speed of the motor, in "encoder counts / encoder interval". * This is the speed of the motor, in "encoder counts / encoder interval".
* The encoder interval is set in Robot::InitializeMotorControlTimer. * The encoder interval is set in Robot::InitializeMotorControlTimer.
*/ */
volatile int16_t speed = 0; volatile int16_t speed = 0;
// Maximum effort // Maximum effort
int16_t maxEffort = 420; int16_t maxEffort = 420;
// Keeps track of encoder changes // Keeps track of encoder changes
volatile int16_t prevCount; volatile int16_t prevCount;
volatile int16_t encCount; volatile int16_t encCount;
volatile int8_t lastA; volatile int8_t lastA;
volatile int8_t lastB; volatile int8_t lastB;
/** /**
* Sets the duty cycle. Must be declared for specific motor. * Sets the duty cycle. Must be declared for specific motor.
*/ */
virtual void SetEffort(int16_t effort) = 0; virtual void SetEffort(int16_t effort) = 0;
/** /**
* Used to set the motor _effort_ directly, which is mostly used for testing. * 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. * Calling this function will switch the control mode to DIRECT, if needed, meaning speed control is lost.
*/ */
void SetMotorEffortDirect(int16_t effort) void SetMotorEffortDirect(int16_t effort)
{ {
SetEffort(effort); SetEffort(effort);
} }
/** /**
* calcEncoderDelta() takes a 'snapshot of the encoders and * calcEncoderDelta() takes a 'snapshot of the encoders and
* stores the change since the last call, which has units of "encoder ticks/motor interval" * 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), * We also use the function for zeroing the delta (for example, when switching control modes),
* so interrupts are cleared when accessing encCount. * so interrupts are cleared when accessing encCount.
*/ */
int16_t CalcEncoderDelta(void) int16_t CalcEncoderDelta(void)
{ {
cli(); cli();
int16_t currCount = encCount; int16_t currCount = encCount;
sei(); sei();
int16_t speed = currCount - prevCount; int16_t speed = currCount - prevCount;
prevCount = currCount; prevCount = currCount;
return speed; return speed;
} }
/** /**
* GetEncoderTotal() returns the total encoder counts" * GetEncoderTotal() returns the total encoder counts"
* *
*/ */
int16_t GetEncoderTotal(void) int16_t GetEncoderTotal(void)
{ {
cli(); cli();
int16_t currCount = encCount; int16_t currCount = encCount;
sei(); sei();
return currCount; return currCount;
} }
static void AttachInterrupts(void); static void AttachInterrupts(void);
/** /**
* InitializeMotorPWMTimer() should be called near the beginning of the program. * 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 * 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 * 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. * IR LED at a common rate.
* *
* Timer 1 has the following configuration: * Timer 1 has the following configuration:
* prescaler of 1 * prescaler of 1
* outputs enabled on channels A (pin 9), B (pin 10) and C (pin 11) * outputs enabled on channels A (pin 9), B (pin 10) and C (pin 11)
* fast PWM mode * fast PWM mode
* top of 420, which will be the max speed * top of 420, which will be the max speed
* frequency is then: 16 MHz / [1 (prescaler) / (420 + 1)] = 38.005 kHz * frequency is then: 16 MHz / [1 (prescaler) / (420 + 1)] = 38.005 kHz
* */ * */
static void InitializePWMTimerAndInterrupts(void) static void InitializePWMTimerAndInterrupts(void)
{ {
Serial.println("InitMotor()"); Serial.println("InitMotor()");
noInterrupts(); //disable interrupts while we set Timer1 registers 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 TCCR1A = 0xA2; //0b10100010; //Fast PWM + enable A and B; change to 0xAA to enable C on pin 11
TCCR1B = 0x19; //0b00011001; //Fast PWM TCCR1B = 0x19; //0b00011001; //Fast PWM
ICR1 = 420; //runs at 38kHz; lowers speed for given effort by 5% from Pololu version ICR1 = 420; //runs at 38kHz; lowers speed for given effort by 5% from Pololu version
//set all three outputs to zero //set all three outputs to zero
OCR1A = 0; OCR1A = 0;
OCR1B = 0; OCR1B = 0;
OCR1C = 0; //can be used to create 38 kHz signal on pin 11; must enable output in TCCR1A above 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 * Call a static function to set up the left and right motor interrupts
*/ */
AttachInterrupts(); AttachInterrupts();
interrupts(); //re-enable interrupts interrupts(); //re-enable interrupts
Serial.println("/InitMotor()"); Serial.println("/InitMotor()");
} }
public: public:
Romi32U4MotorBase(const String& nm) : name(nm) {} Romi32U4MotorBase(const String& nm) : name(nm) {}
}; };
template <uint8_t encXOR, uint8_t encB, uint8_t PWM, uint8_t DIR, uint8_t OCR> template <uint8_t encXOR, uint8_t encB, uint8_t PWM, uint8_t DIR, uint8_t OCR>
class Romi32U4EncodedMotor : public Romi32U4MotorBase class Romi32U4EncodedMotor : public Romi32U4MotorBase
{ {
protected: protected:
void InitializeMotor(void) void InitializeMotor(void)
{ {
FastGPIO::Pin<PWM>::setOutputLow(); FastGPIO::Pin<PWM>::setOutputLow();
FastGPIO::Pin<DIR>::setOutputLow(); FastGPIO::Pin<DIR>::setOutputLow();
InitializeEncoder(); InitializeEncoder();
} }
/** /**
* SetEffort is used internally to set the motor effort without changing the control mode. * SetEffort is used internally to set the motor effort without changing the control mode.
*/ */
void SetEffort(int16_t effort) void SetEffort(int16_t effort)
{ {
bool reverse = 0; bool reverse = 0;
if (effort < 0) if (effort < 0)
{ {
effort = -effort; // Make speed a positive quantity. effort = -effort; // Make speed a positive quantity.
reverse = 1; // Reverse the direction. reverse = 1; // Reverse the direction.
} }
if (effort > maxEffort) if (effort > maxEffort)
{ {
effort = maxEffort; effort = maxEffort;
} }
FastGPIO::Pin<DIR>::setOutput(reverse); FastGPIO::Pin<DIR>::setOutput(reverse);
/** /**
* This line is a little esoteric, but it sets the duty cycle for the appropriate PWM pin. * 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 %). * Note that TOP for the PWM is 420, so the duty cycle will be (effort / 420) * 100 (in %).
*/ */
_SFR_MEM16(OCR) = effort; _SFR_MEM16(OCR) = effort;
} }
void InitializeEncoder(void) void InitializeEncoder(void)
{ {
Serial.println("InitEnc()"); Serial.println("InitEnc()");
// Set the pins as pulled-up inputs. // Set the pins as pulled-up inputs.
FastGPIO::Pin<encXOR>::setInputPulledUp(); FastGPIO::Pin<encXOR>::setInputPulledUp();
FastGPIO::Pin<encB>::setInputPulledUp(); FastGPIO::Pin<encB>::setInputPulledUp();
// Initialize the variables so that the speed will start as 0 // Initialize the variables so that the speed will start as 0
lastB = FastGPIO::Pin<encB>::isInputHigh(); lastB = FastGPIO::Pin<encB>::isInputHigh();
lastA = FastGPIO::Pin<encXOR>::isInputHigh() ^ lastB; lastA = FastGPIO::Pin<encXOR>::isInputHigh() ^ lastB;
Serial.println("/InitEnc()"); Serial.println("/InitEnc()");
} }
public: public:
Romi32U4EncodedMotor(const String& name) : Romi32U4MotorBase(name) {} Romi32U4EncodedMotor(const String& name) : Romi32U4MotorBase(name) {}
void ProcessEncoderTick(void) void ProcessEncoderTick(void)
{ {
bool newB = FastGPIO::Pin<encB>::isInputHigh(); bool newB = FastGPIO::Pin<encB>::isInputHigh();
bool newA = FastGPIO::Pin<encXOR>::isInputHigh() ^ newB; bool newA = FastGPIO::Pin<encXOR>::isInputHigh() ^ newB;
encCount += (lastA ^ newB) - (newA ^ lastB); encCount += (lastA ^ newB) - (newA ^ lastB);
lastA = newA; lastA = newA;
lastB = newB; lastB = newB;
} }
friend class Chassis; // Allow Chassis to call protected methods directly friend class Chassis; // Allow Chassis to call protected methods directly
}; };
extern Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor; extern Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor;
extern Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor; extern Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor;

370
lib/Servo32u4/src/servo32u4.cpp
View file

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

364
lib/Servo32u4/src/servo32u4.h
View file

@ -1,182 +1,182 @@
#pragma once #pragma once
#include <Arduino.h> #include <Arduino.h>
/** \file Manages servos on up to three pins: /** \file Manages servos on up to three pins:
* pin 5, * pin 5,
* pin 13, and * pin 13, and
* either pin 6 OR 12 (don't use 6 and 12 simultaneously!). * 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 * 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! * 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). * 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). * 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 * 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! * 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. * 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, * 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. * 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 * If using the Chassis class, Timer4 is used for the control loop, so be sure you don't create any
* conflicts. * conflicts.
* *
* writeMicroseconds is protected so that you don't call it directly (which will make it move * 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. * really fast). Instead, set the target and call update() regularly.
* *
* */ * */
/** \class Servo32U4Base /** \class Servo32U4Base
* \brief Base class class for servos. * \brief Base class class for servos.
* *
* Each derived class controls a specific pin (obvious from the name). * Each derived class controls a specific pin (obvious from the name).
* *
* Defaults to a range of 1000 - 2000 us, but can be customized. * Defaults to a range of 1000 - 2000 us, but can be customized.
*/ */
class Servo32U4Base class Servo32U4Base
{ {
protected: protected:
uint16_t usMin = 1000; uint16_t usMin = 1000;
uint16_t usMax = 2000; uint16_t usMax = 2000;
uint8_t feedbackPin = -1; uint8_t feedbackPin = -1;
bool isAttached = false; bool isAttached = false;
uint16_t targetPos = 1500; uint16_t targetPos = 1500;
uint16_t currentPos = 1500; uint16_t currentPos = 1500;
public: public:
// Virtual functions defined for each specific class // Virtual functions defined for each specific class
virtual void attach(void) = 0; virtual void attach(void) = 0;
virtual void detach(void) = 0; virtual void detach(void) = 0;
void setTargetPos(uint16_t target) {targetPos = target;} void setTargetPos(uint16_t target) {targetPos = target;}
/** /**
* update() moves the servo towards the target position. You will want to * 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. * change it to return a bool to detect the event of reaching the target.
*/ */
void update(void) void update(void)
{ {
if(targetPos == currentPos) {} // no need to update if(targetPos == currentPos) {} // no need to update
else if(abs(targetPos - currentPos) <= 40) currentPos = targetPos; else if(abs(targetPos - currentPos) <= 40) currentPos = targetPos;
else if(targetPos > currentPos) currentPos += 40; else if(targetPos > currentPos) currentPos += 40;
else if(targetPos < currentPos) currentPos -= 40; else if(targetPos < currentPos) currentPos -= 40;
writeMicroseconds(currentPos); writeMicroseconds(currentPos);
} }
uint16_t setMinMaxMicroseconds(uint16_t min, uint16_t max); uint16_t setMinMaxMicroseconds(uint16_t min, uint16_t max);
virtual void writeMicroseconds(uint16_t microseconds) = 0; virtual void writeMicroseconds(uint16_t microseconds) = 0;
}; };
/** \class Servo32U4Pin5 /** \class Servo32U4Pin5
* \brief A servo class to control a servo on pin 5. * \brief A servo class to control a servo on pin 5.
* *
* Servo32U4 uses output compare on Timer3 to control the pulse to the servo. * 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. * 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! * 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. * Defaults to a range of 1000 - 2000 us, but can be customized.
*/ */
class Servo32U4Pin5 : public Servo32U4Base class Servo32U4Pin5 : public Servo32U4Base
{ {
public: public:
void attach(void); void attach(void);
void detach(void); void detach(void);
protected: protected:
void writeMicroseconds(uint16_t microseconds); void writeMicroseconds(uint16_t microseconds);
}; };
/** \class Servo32U4Pin6 /** \class Servo32U4Pin6
* \brief A servo class to control a servo on pin 6. * \brief A servo class to control a servo on pin 6.
* *
* Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo. * Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo.
* _In Chassis::init(), * _In Chassis::init(),
* The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval. * 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, * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK,
* AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
* *
* OCR4D controls the pulse on pin 6 -- this servo must be on pin 6! * 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. * 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. * 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. * 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. * Note that because we're using an 8-bit timer, resolution is only 64 us.
*/ */
class Servo32U4Pin6 : public Servo32U4Base class Servo32U4Pin6 : public Servo32U4Base
{ {
public: public:
void attach(void); void attach(void);
void detach(void); void detach(void);
protected: protected:
void writeMicroseconds(uint16_t microseconds); void writeMicroseconds(uint16_t microseconds);
}; };
/** \class Servo32U4Pin13 /** \class Servo32U4Pin13
* \brief A servo class to control a servo on pin 13. * \brief A servo class to control a servo on pin 13.
* *
* Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo. * Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo.
* _In Chassis::init(), * _In Chassis::init(),
* The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval. * 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, * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK,
* AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
* *
* OCR4A controls the pulse on pin 13 -- this servo must be on pin 13! * 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. * 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! * 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. * 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. * Note that because we're using an 8-bit timer, resolution is only 64 us.
*/ */
class Servo32U4Pin13 : public Servo32U4Base class Servo32U4Pin13 : public Servo32U4Base
{ {
public: public:
void attach(void); void attach(void);
void detach(void); void detach(void);
protected: protected:
void writeMicroseconds(uint16_t microseconds); void writeMicroseconds(uint16_t microseconds);
}; };
/** \class Servo32U4Pin12 /** \class Servo32U4Pin12
* \brief A servo class to control a servo on pin 12. * \brief A servo class to control a servo on pin 12.
* *
* Servo32U4Pin12 uses output compare on Timer4 to control the pulse to the servo. * Servo32U4Pin12 uses output compare on Timer4 to control the pulse to the servo.
* _In Chassis::init(), * _In Chassis::init(),
* *
* YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK,
* AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach()
* *
* ^OCR4D controls the pulse on pin 12 -- this servo _must_ be on pin 12! * ^OCR4D controls the pulse on pin 12 -- this servo _must_ be on pin 12!
* *
* DO NOT USE IN CONJUNCTION WITH Servo32U4Pin6 * DO NOT USE IN CONJUNCTION WITH Servo32U4Pin6
* *
* Defaults to a range of 1000 - 2000 us, but can be customized. * 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. * Note that because we're using an 8-bit timer, resolution is only 64 us.
*/ */
class Servo32U4Pin12 : public Servo32U4Base class Servo32U4Pin12 : public Servo32U4Base
{ {
public: public:
void attach(void); void attach(void);
void detach(void); void detach(void);
protected: protected:
void writeMicroseconds(uint16_t microseconds); void writeMicroseconds(uint16_t microseconds);
}; };

56
platformio.ini
View file

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

50
src/main.cpp
View file

@ -1,25 +1,25 @@
#include <Arduino.h> #include <Arduino.h>
#include "robot.h" #include "robot.h"
Robot robot; Robot robot;
void setup() void setup()
{ {
Serial.begin(115200); Serial.begin(115200);
/** /**
* If you define __SETUP_DEBUG__ in your .ini file, this line will make the program wait * 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 * 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! * the Serial Monitor for anything to happen!
*/ */
#ifdef __SETUP_DEBUG__ #ifdef __SETUP_DEBUG__
while(!Serial){delay(5);} while(!Serial){delay(5);}
#endif #endif
robot.InitializeRobot(); robot.InitializeRobot();
} }
void loop() void loop()
{ {
robot.RobotLoop(); robot.RobotLoop();
} }

142
src/robot-nav.cpp
View file

@ -1,72 +1,72 @@
/** /**
* robot-nav.cpp is where you should put navigation routines. * robot-nav.cpp is where you should put navigation routines.
*/ */
#include "robot.h" #include "robot.h"
void Robot::UpdatePose(const Twist& twist) void Robot::UpdatePose(const Twist& twist)
{ {
/** /**
* TODO: Add your FK algorithm to update currPose here. * TODO: Add your FK algorithm to update currPose here.
*/ */
#ifdef __NAV_DEBUG__ #ifdef __NAV_DEBUG__
TeleplotPrint("x", currPose.x); TeleplotPrint("x", currPose.x);
TeleplotPrint("y", currPose.y); TeleplotPrint("y", currPose.y);
TeleplotPrint("theta", currPose.theta); TeleplotPrint("theta", currPose.theta);
#endif #endif
} }
/** /**
* Sets a destination in the lab frame. * Sets a destination in the lab frame.
*/ */
void Robot::SetDestination(const Pose& dest) void Robot::SetDestination(const Pose& dest)
{ {
/** /**
* TODO: Turn on LED, as well. * TODO: Turn on LED, as well.
*/ */
Serial.print("Setting dest to: "); Serial.print("Setting dest to: ");
Serial.print(dest.x); Serial.print(dest.x);
Serial.print(", "); Serial.print(", ");
Serial.print(dest.y); Serial.print(dest.y);
Serial.print('\n'); Serial.print('\n');
destPose = dest; destPose = dest;
robotState = ROBOT_DRIVE_TO_POINT; robotState = ROBOT_DRIVE_TO_POINT;
} }
bool Robot::CheckReachedDestination(void) bool Robot::CheckReachedDestination(void)
{ {
bool retVal = false; bool retVal = false;
/** /**
* TODO: Add code to check if you've reached destination here. * TODO: Add code to check if you've reached destination here.
*/ */
return retVal; return retVal;
} }
void Robot::DriveToPoint(void) void Robot::DriveToPoint(void)
{ {
if(robotState == ROBOT_DRIVE_TO_POINT) if(robotState == ROBOT_DRIVE_TO_POINT)
{ {
/** /**
* TODO: Add your IK algorithm here. * TODO: Add your IK algorithm here.
*/ */
#ifdef __NAV_DEBUG__ #ifdef __NAV_DEBUG__
// Print useful stuff here. // Print useful stuff here.
#endif #endif
/** /**
* TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above. * TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above.
*/ */
} }
} }
void Robot::HandleDestination(void) void Robot::HandleDestination(void)
{ {
/** /**
* TODO: Stop and change state. Turn off LED. * TODO: Stop and change state. Turn off LED.
*/ */
} }

100
src/robot.cpp
View file

@ -1,50 +1,50 @@
#include "robot.h" #include "robot.h"
void Robot::InitializeRobot(void) void Robot::InitializeRobot(void)
{ {
chassis.InititalizeChassis(); chassis.InititalizeChassis();
/** /**
* TODO: Set pin 13 HIGH when navigating and LOW when destination is reached. * TODO: Set pin 13 HIGH when navigating and LOW when destination is reached.
* Need to set as OUTPUT here. * Need to set as OUTPUT here.
*/ */
} }
void Robot::EnterIdleState(void) void Robot::EnterIdleState(void)
{ {
chassis.Stop(); chassis.Stop();
Serial.println("-> IDLE"); Serial.println("-> IDLE");
robotState = ROBOT_IDLE; robotState = ROBOT_IDLE;
} }
/** /**
* The main loop for your robot. Process both synchronous events (motor control), * The main loop for your robot. Process both synchronous events (motor control),
* and asynchronous events (distance readings, etc.). * and asynchronous events (distance readings, etc.).
*/ */
void Robot::RobotLoop(void) void Robot::RobotLoop(void)
{ {
/** /**
* Run the chassis loop, which handles low-level control. * Run the chassis loop, which handles low-level control.
*/ */
Twist velocity; Twist velocity;
if(chassis.ChassisLoop(velocity)) if(chassis.ChassisLoop(velocity))
{ {
// We do FK regardless of state // We do FK regardless of state
UpdatePose(velocity); UpdatePose(velocity);
chassis.SetMotorEfforts(220,-220); chassis.SetMotorEfforts(220,-220);
/** /**
* Here, we break with tradition and only call these functions if we're in the * 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 * DRIVE_TO_POINT state. CheckReachedDestination() is expensive, so we don't want
* to do all the maths when we don't need to. * to do all the maths when we don't need to.
* *
* While we're at it, we'll toss DriveToPoint() in, as well. * While we're at it, we'll toss DriveToPoint() in, as well.
*/ */
if(robotState == ROBOT_DRIVE_TO_POINT) if(robotState == ROBOT_DRIVE_TO_POINT)
{ {
DriveToPoint(); DriveToPoint();
if(CheckReachedDestination()) HandleDestination(); if(CheckReachedDestination()) HandleDestination();
} }
} }
} }

92
src/robot.h
View file

@ -1,46 +1,46 @@
#pragma once #pragma once
#include "chassis.h" #include "chassis.h"
class Robot class Robot
{ {
protected: protected:
/** /**
* robotState is used to track the current task of the robot. You will add new states as * robotState is used to track the current task of the robot. You will add new states as
* the term progresses. * the term progresses.
*/ */
enum ROBOT_STATE enum ROBOT_STATE
{ {
ROBOT_IDLE, ROBOT_IDLE,
ROBOT_DRIVE_TO_POINT, ROBOT_DRIVE_TO_POINT,
}; };
ROBOT_STATE robotState = ROBOT_IDLE; ROBOT_STATE robotState = ROBOT_IDLE;
/* Define the chassis*/ /* Define the chassis*/
Chassis chassis; Chassis chassis;
// For managing key presses // For managing key presses
String keyString; String keyString;
/** /**
* For tracking current pose and the destination. * For tracking current pose and the destination.
*/ */
Pose currPose; Pose currPose;
Pose destPose; Pose destPose;
public: public:
Robot(void) {keyString.reserve(10);} Robot(void) {keyString.reserve(10);}
void InitializeRobot(void); void InitializeRobot(void);
void RobotLoop(void); void RobotLoop(void);
protected: protected:
/* State changes */ /* State changes */
void EnterIdleState(void); void EnterIdleState(void);
// /* Navigation methods.*/ // /* Navigation methods.*/
void UpdatePose(const Twist& u); void UpdatePose(const Twist& u);
void SetDestination(const Pose& destination); void SetDestination(const Pose& destination);
void DriveToPoint(void); void DriveToPoint(void);
bool CheckReachedDestination(void); bool CheckReachedDestination(void);
void HandleDestination(void); void HandleDestination(void);
}; };

22
test/README
View file

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