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
.pio
.vscode
.pio

78
include/README
View file

@ -1,39 +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
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

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.
*/
const float ROBOT_RADIUS = 7;
const float LEFT_TICKS_PER_CM = 50;
const float RIGHT_TICKS_PER_CM = 50;
/**
* 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;

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

@ -1,156 +1,156 @@
#include "chassis.h"
#include "Romi32U4MotorTemplate.h"
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");
/**
* 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
/* 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);
}
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;
}
#include "chassis.h"
#include "Romi32U4MotorTemplate.h"
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");
/**
* 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
/* 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);
}
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;
}

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

@ -1,54 +1,54 @@
#pragma once
#include <Arduino.h>
#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:
Twist CalcOdomFromWheelMotion(void);
void SetMotorEfforts(int16_t, int16_t);
void Stop(void) { SetMotorEfforts(0, 0);}
protected:
/**
* Initialization and Setup routines
*/
void InitializeMotorControlTimer(void);
void InitializeMotors(void);
void UpdateMotors(void);
};
#pragma once
#include <Arduino.h>
#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:
Twist CalcOdomFromWheelMotion(void);
void SetMotorEfforts(int16_t, int16_t);
void Stop(void) { SetMotorEfforts(0, 0);}
protected:
/**
* Initialization and Setup routines
*/
void InitializeMotorControlTimer(void);
void InitializeMotors(void);
void UpdateMotors(void);
};

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

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

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

@ -1,32 +1,32 @@
#pragma once
#include <Arduino.h>
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_) {}
};
#pragma once
#include <Arduino.h>
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_) {}
};

92
lib/README
View file

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

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

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

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

@ -1,204 +1,204 @@
#pragma once
#include <Arduino.h>
#include <FastGPIO.h>
// define the motor pins here
#define PWM_L 10
#define PWM_R 9
#define DIR_L 16
#define DIR_R 15
#define LEFT_XOR 8
#define LEFT_B IO_E2
#define RIGHT_XOR 7
#define RIGHT_B 23
#define OCR_L 0x8A
#define OCR_R 0x88
class Romi32U4MotorBase
{
protected:
// For prettier printing
const String name;
/**
* 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;
// 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)
{
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;
}
/**
* GetEncoderTotal() returns the total encoder counts"
*
*/
int16_t GetEncoderTotal(void)
{
cli();
int16_t currCount = encCount;
sei();
return currCount;
}
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) {}
};
template <uint8_t encXOR, uint8_t encB, uint8_t PWM, uint8_t DIR, uint8_t OCR>
class Romi32U4EncodedMotor : public Romi32U4MotorBase
{
protected:
void InitializeMotor(void)
{
FastGPIO::Pin<PWM>::setOutputLow();
FastGPIO::Pin<DIR>::setOutputLow();
InitializeEncoder();
}
/**
* SetEffort is used internally to set the motor effort without changing the control mode.
*/
void SetEffort(int16_t effort)
{
bool reverse = 0;
if (effort < 0)
{
effort = -effort; // Make speed a positive quantity.
reverse = 1; // Reverse the direction.
}
if (effort > maxEffort)
{
effort = maxEffort;
}
FastGPIO::Pin<DIR>::setOutput(reverse);
/**
* 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<encXOR>::setInputPulledUp();
FastGPIO::Pin<encB>::setInputPulledUp();
// Initialize the variables so that the speed will start as 0
lastB = FastGPIO::Pin<encB>::isInputHigh();
lastA = FastGPIO::Pin<encXOR>::isInputHigh() ^ lastB;
Serial.println("/InitEnc()");
}
public:
Romi32U4EncodedMotor(const String& name) : Romi32U4MotorBase(name) {}
void ProcessEncoderTick(void)
{
bool newB = FastGPIO::Pin<encB>::isInputHigh();
bool newA = FastGPIO::Pin<encXOR>::isInputHigh() ^ newB;
encCount += (lastA ^ newB) - (newA ^ lastB);
lastA = newA;
lastB = newB;
}
friend class Chassis; // Allow Chassis to call protected methods directly
};
extern Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor;
extern Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor;
#pragma once
#include <Arduino.h>
#include <FastGPIO.h>
// define the motor pins here
#define PWM_L 10
#define PWM_R 9
#define DIR_L 16
#define DIR_R 15
#define LEFT_XOR 8
#define LEFT_B IO_E2
#define RIGHT_XOR 7
#define RIGHT_B 23
#define OCR_L 0x8A
#define OCR_R 0x88
class Romi32U4MotorBase
{
protected:
// For prettier printing
const String name;
/**
* 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;
// 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)
{
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;
}
/**
* GetEncoderTotal() returns the total encoder counts"
*
*/
int16_t GetEncoderTotal(void)
{
cli();
int16_t currCount = encCount;
sei();
return currCount;
}
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) {}
};
template <uint8_t encXOR, uint8_t encB, uint8_t PWM, uint8_t DIR, uint8_t OCR>
class Romi32U4EncodedMotor : public Romi32U4MotorBase
{
protected:
void InitializeMotor(void)
{
FastGPIO::Pin<PWM>::setOutputLow();
FastGPIO::Pin<DIR>::setOutputLow();
InitializeEncoder();
}
/**
* SetEffort is used internally to set the motor effort without changing the control mode.
*/
void SetEffort(int16_t effort)
{
bool reverse = 0;
if (effort < 0)
{
effort = -effort; // Make speed a positive quantity.
reverse = 1; // Reverse the direction.
}
if (effort > maxEffort)
{
effort = maxEffort;
}
FastGPIO::Pin<DIR>::setOutput(reverse);
/**
* 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<encXOR>::setInputPulledUp();
FastGPIO::Pin<encB>::setInputPulledUp();
// Initialize the variables so that the speed will start as 0
lastB = FastGPIO::Pin<encB>::isInputHigh();
lastA = FastGPIO::Pin<encXOR>::isInputHigh() ^ lastB;
Serial.println("/InitEnc()");
}
public:
Romi32U4EncodedMotor(const String& name) : Romi32U4MotorBase(name) {}
void ProcessEncoderTick(void)
{
bool newB = FastGPIO::Pin<encB>::isInputHigh();
bool newA = FastGPIO::Pin<encXOR>::isInputHigh() ^ newB;
encCount += (lastA ^ newB) - (newA ^ lastB);
lastA = newA;
lastB = newB;
}
friend class Chassis; // Allow Chassis to call protected methods directly
};
extern Romi32U4EncodedMotor<LEFT_XOR, LEFT_B, PWM_L, DIR_L, OCR_L> leftMotor;
extern Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor;

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

@ -1,185 +1,185 @@
#include <servo32u4.h>
uint16_t Servo32U4Base::setMinMaxMicroseconds(uint16_t min, uint16_t max)
{
// swap if in the wrong place
if(min > max) {uint16_t temp = min; min = max; max = temp;}
usMin = min;
usMax = max;
return usMax - usMin; //return the range, in case the user wants to do a sanity check
}
void Servo32U4Pin5::attach(void)
{
pinMode(5, OUTPUT); // set pin as OUTPUT
cli();
// clear then set the OCR3A bits (pin 5)
TCCR3A = 0x82; //WGM
TCCR3B = 0x1A; //WGM + CS = 8
ICR3 = 39999; //20ms
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
}
#include <servo32u4.h>
uint16_t Servo32U4Base::setMinMaxMicroseconds(uint16_t min, uint16_t max)
{
// swap if in the wrong place
if(min > max) {uint16_t temp = min; min = max; max = temp;}
usMin = min;
usMax = max;
return usMax - usMin; //return the range, in case the user wants to do a sanity check
}
void Servo32U4Pin5::attach(void)
{
pinMode(5, OUTPUT); // set pin as OUTPUT
cli();
// clear then set the OCR3A bits (pin 5)
TCCR3A = 0x82; //WGM
TCCR3B = 0x1A; //WGM + CS = 8
ICR3 = 39999; //20ms
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
}

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

@ -1,182 +1,182 @@
#pragma once
#include <Arduino.h>
/** \file Manages servos on up to three pins:
* pin 5,
* pin 13, and
* either pin 6 OR 12 (don't use 6 and 12 simultaneously!).
*
* 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);
};
#pragma once
#include <Arduino.h>
/** \file Manages servos on up to three pins:
* pin 5,
* pin 13, and
* either pin 6 OR 12 (don't use 6 and 12 simultaneously!).
*
* 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);
};

56
platformio.ini
View file

@ -1,28 +1,28 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:a-star32U4]
platform = atmelavr
board = a-star32U4
framework = arduino
monitor_speed = 115200
lib_deps =
Wire
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__
; 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/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__

50
src/main.cpp
View file

@ -1,25 +1,25 @@
#include <Arduino.h>
#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();
}
#include <Arduino.h>
#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();
}

142
src/robot-nav.cpp
View file

@ -1,72 +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.SetMotorEfforts() to command the motion, based on your calculations above.
*/
}
}
void Robot::HandleDestination(void)
{
/**
* TODO: Stop and change state. Turn off LED.
*/
/**
* 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.SetMotorEfforts() to command the motion, based on your calculations above.
*/
}
}
void Robot::HandleDestination(void)
{
/**
* TODO: Stop and change state. Turn off LED.
*/
}

100
src/robot.cpp
View file

@ -1,50 +1,50 @@
#include "robot.h"
void Robot::InitializeRobot(void)
{
chassis.InititalizeChassis();
/**
* 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 (distance readings, etc.).
*/
void Robot::RobotLoop(void)
{
/**
* Run the chassis loop, which handles low-level control.
*/
Twist velocity;
if(chassis.ChassisLoop(velocity))
{
// We do FK regardless of state
UpdatePose(velocity);
chassis.SetMotorEfforts(220,-220);
/**
* 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();
}
}
}
#include "robot.h"
void Robot::InitializeRobot(void)
{
chassis.InititalizeChassis();
/**
* 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 (distance readings, etc.).
*/
void Robot::RobotLoop(void)
{
/**
* Run the chassis loop, which handles low-level control.
*/
Twist velocity;
if(chassis.ChassisLoop(velocity))
{
// We do FK regardless of state
UpdatePose(velocity);
chassis.SetMotorEfforts(220,-220);
/**
* 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();
}
}
}

92
src/robot.h
View file

@ -1,46 +1,46 @@
#pragma once
#include "chassis.h"
class Robot
{
protected:
/**
* 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:
/* State changes */
void EnterIdleState(void);
// /* Navigation methods.*/
void UpdatePose(const Twist& u);
void SetDestination(const Pose& destination);
void DriveToPoint(void);
bool CheckReachedDestination(void);
void HandleDestination(void);
};
#pragma once
#include "chassis.h"
class Robot
{
protected:
/**
* 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:
/* State changes */
void EnterIdleState(void);
// /* Navigation methods.*/
void UpdatePose(const Twist& u);
void SetDestination(const Pose& destination);
void DriveToPoint(void);
bool CheckReachedDestination(void);
void HandleDestination(void);
};

22
test/README
View file

@ -1,11 +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
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