diff --git a/.gitignore b/.gitignore index 80e7464..c524c67 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,2 @@ -.vscode -.pio +.vscode +.pio diff --git a/include/README b/include/README index 194dcd4..45496b1 100644 --- a/include/README +++ b/include/README @@ -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 diff --git a/lib/Chassis/src/chassis-params.h b/lib/Chassis/src/chassis-params.h index 09a43bb..fd0f4bf 100644 --- a/lib/Chassis/src/chassis-params.h +++ b/lib/Chassis/src/chassis-params.h @@ -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; diff --git a/lib/Chassis/src/chassis.cpp b/lib/Chassis/src/chassis.cpp index d8322cf..5d5ab99 100644 --- a/lib/Chassis/src/chassis.cpp +++ b/lib/Chassis/src/chassis.cpp @@ -1,156 +1,156 @@ -#include "chassis.h" -#include "Romi32U4MotorTemplate.h" - -Romi32U4EncodedMotor leftMotor("L"); -Romi32U4EncodedMotor rightMotor("R"); - -/** - * Because it's declared static, we initialize Chassis::loopFlag here. - */ -uint8_t Chassis::loopFlag = 0; - -/** - * For taking snapshots and raising the flag. - */ -void Chassis::Timer4OverflowISRHandler(void) -{ - loopFlag++; - - leftMotor.speed = leftMotor.CalcEncoderDelta(); - rightMotor.speed = rightMotor.CalcEncoderDelta(); -} - -/** - * ISR for timing. On Timer4 overflow, we take a 'snapshot' of the encoder counts - * and raise a flag to let the program it is time to execute the PID calculations. - */ -ISR(TIMER4_OVF_vect) -{ - Chassis::Timer4OverflowISRHandler(); -} - -/** - * Sets up a hardware timer on Timer4 to manage motor control on a precise schedule. - * - * We set the timer to set an interrupt flag on overflow, which is handled - * by ISR(TIMER4_OVF_vect) below. - */ -void Chassis::InitializeMotorControlTimer(void) -{ - Serial.println("InitTimer"); - // Disable interupts while we mess with the Timer4 registers - cli(); - - // Set up Timer4 - TCCR4A = 0x00; // Disable output to pins - TCCR4B = 0x0A; // Sets the prescaler -- see pp. 167-8 in datasheet - TCCR4C = 0x00; // Disables output to pins (but see below for buzzer) - TCCR4D = 0x00; // Normal mode: count up and roll-over - - /** - * Calculate TOP based on prescaler and loop duration. Note that loop is in integer ms -- - * there may be some rounding. Multiples of 4 ms will be exact. - */ - uint16_t top = ((CONTROL_LOOP_PERIOD_MS * 16000ul) >> 9) - 1; // divides by 512 - - /** - * Here we do a little trick to allow full 10-bit register access. - * We have 2 _bits_ in TC4H that we can use to add capacity to TOP. - * - * Note that the maximum period is limited by TOP = 0x3FF. If you want - * a longer period, you'll need to adjust the pre-scaler. - * - * There is no minumum period, but precision breaks down with low values, - * unless you adjust the pre-scaler, but the encoder resolution is limited, - * so you only want to go so fast. - */ - uint8_t highbits = top / 256; - uint8_t lowbits = top - highbits; - TC4H = highbits; OCR4C = lowbits; - - // Enable overflow interrupt - TIMSK4 = 0x04; - - /** - * Uncommenting the following lines will pipe the timer signal to pin 6, - * which controls the buzzer. The pin will toggle at the loop rate, which - * allows you to check that the timing is correct. It will also make a lot - * of noise, so do so sparingly. - */ - // TCCR4C = 0x04 - // pinMode(6, OUTPUT); - - // Re-enable interrupts - sei(); - - Serial.println("/InitTimer"); -} - -void Chassis::InititalizeChassis(void) -{ - InitializeMotorControlTimer(); - InitializeMotors(); -} - -/** - * The main Chassis loop. - */ -bool Chassis::ChassisLoop(Twist& velocity) -{ - bool retVal = false; - - if(loopFlag) - { - if(loopFlag > 1) Serial.println("Missed an update in Robot::RobotLoop()!"); - -#ifdef __LOOP_DEBUG__ - Serial.print(millis()); - Serial.print('\n'); -#endif - - /* 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 leftMotor("L"); +Romi32U4EncodedMotor rightMotor("R"); + +/** + * Because it's declared static, we initialize Chassis::loopFlag here. + */ +uint8_t Chassis::loopFlag = 0; + +/** + * For taking snapshots and raising the flag. + */ +void Chassis::Timer4OverflowISRHandler(void) +{ + loopFlag++; + + leftMotor.speed = leftMotor.CalcEncoderDelta(); + rightMotor.speed = rightMotor.CalcEncoderDelta(); +} + +/** + * ISR for timing. On Timer4 overflow, we take a 'snapshot' of the encoder counts + * and raise a flag to let the program it is time to execute the PID calculations. + */ +ISR(TIMER4_OVF_vect) +{ + Chassis::Timer4OverflowISRHandler(); +} + +/** + * Sets up a hardware timer on Timer4 to manage motor control on a precise schedule. + * + * We set the timer to set an interrupt flag on overflow, which is handled + * by ISR(TIMER4_OVF_vect) below. + */ +void Chassis::InitializeMotorControlTimer(void) +{ + Serial.println("InitTimer"); + // Disable interupts while we mess with the Timer4 registers + cli(); + + // Set up Timer4 + TCCR4A = 0x00; // Disable output to pins + TCCR4B = 0x0A; // Sets the prescaler -- see pp. 167-8 in datasheet + TCCR4C = 0x00; // Disables output to pins (but see below for buzzer) + TCCR4D = 0x00; // Normal mode: count up and roll-over + + /** + * Calculate TOP based on prescaler and loop duration. Note that loop is in integer ms -- + * there may be some rounding. Multiples of 4 ms will be exact. + */ + uint16_t top = ((CONTROL_LOOP_PERIOD_MS * 16000ul) >> 9) - 1; // divides by 512 + + /** + * Here we do a little trick to allow full 10-bit register access. + * We have 2 _bits_ in TC4H that we can use to add capacity to TOP. + * + * Note that the maximum period is limited by TOP = 0x3FF. If you want + * a longer period, you'll need to adjust the pre-scaler. + * + * There is no minumum period, but precision breaks down with low values, + * unless you adjust the pre-scaler, but the encoder resolution is limited, + * so you only want to go so fast. + */ + uint8_t highbits = top / 256; + uint8_t lowbits = top - highbits; + TC4H = highbits; OCR4C = lowbits; + + // Enable overflow interrupt + TIMSK4 = 0x04; + + /** + * Uncommenting the following lines will pipe the timer signal to pin 6, + * which controls the buzzer. The pin will toggle at the loop rate, which + * allows you to check that the timing is correct. It will also make a lot + * of noise, so do so sparingly. + */ + // TCCR4C = 0x04 + // pinMode(6, OUTPUT); + + // Re-enable interrupts + sei(); + + Serial.println("/InitTimer"); +} + +void Chassis::InititalizeChassis(void) +{ + InitializeMotorControlTimer(); + InitializeMotors(); +} + +/** + * The main Chassis loop. + */ +bool Chassis::ChassisLoop(Twist& velocity) +{ + bool retVal = false; + + if(loopFlag) + { + if(loopFlag > 1) Serial.println("Missed an update in Robot::RobotLoop()!"); + +#ifdef __LOOP_DEBUG__ + Serial.print(millis()); + Serial.print('\n'); +#endif + + /* 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; +} diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h index 8a0e8b5..0a10287 100644 --- a/lib/Chassis/src/chassis.h +++ b/lib/Chassis/src/chassis.h @@ -1,54 +1,54 @@ -#pragma once - -#include - -#include "chassis-params.h" -#include "utils.h" - -class Chassis -{ -protected: - - /** - * You can change the control loop period, but you should use multiples of 4 ms to - * avoid rounding errors. - */ - const uint16_t CONTROL_LOOP_PERIOD_MS = 20; - - /** - * loopFlag is used to tell the program when to update. It is set when Timer4 - * overflows (see InitializeMotorControlTimer). Some of the calculations are too - * lengthy for an ISR, so we set a flag and use that to key the calculations. - * - * Note that we use in integer so we can see if we've missed a loop. If loopFlag is - * more than 1, then we missed a cycle. - */ - static uint8_t loopFlag; - -public: - Chassis(void) {} - void InititalizeChassis(void); - - /* Where the bulk of the work for the motors gets done. */ - bool ChassisLoop(Twist&); - - /* Needed for managing motors. */ - static void Timer4OverflowISRHandler(void); - -public: - 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 + +#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); +}; diff --git a/lib/Chassis/src/utils.cpp b/lib/Chassis/src/utils.cpp index 12f34f0..58efd54 100644 --- a/lib/Chassis/src/utils.cpp +++ b/lib/Chassis/src/utils.cpp @@ -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'); +} diff --git a/lib/Chassis/src/utils.h b/lib/Chassis/src/utils.h index 391aaee..f3cae9c 100644 --- a/lib/Chassis/src/utils.h +++ b/lib/Chassis/src/utils.h @@ -1,32 +1,32 @@ -#pragma once -#include - -void TeleplotPrint(const char* var, float value); - -/** - * Pose includes information about the 2D pose of a robot: x, y, and heading. - */ -struct Pose -{ - float x = 0; - float y = 0; - float theta = 0; - - Pose(void) {} - Pose(float x_, float y_, float th_) : x(x_), y(y_), theta(th_) {} -}; - -/** - * Twist is very similar to Pose, but we make a separate struct to avoid confusion. - * - * Whereas Pose is position/heading, Twist contains velocity/ang. vel. - */ -struct Twist -{ - float u = 0; - float v = 0; // This will always be 0 in the robot frame. - float omega = 0; - - Twist(void) {} - Twist(float u_, float v_, float om_) : u(u_), v(v_), omega(om_) {} -}; +#pragma once +#include + +void TeleplotPrint(const char* var, float value); + +/** + * Pose includes information about the 2D pose of a robot: x, y, and heading. + */ +struct Pose +{ + float x = 0; + float y = 0; + float theta = 0; + + Pose(void) {} + Pose(float x_, float y_, float th_) : x(x_), y(y_), theta(th_) {} +}; + +/** + * Twist is very similar to Pose, but we make a separate struct to avoid confusion. + * + * Whereas Pose is position/heading, Twist contains velocity/ang. vel. + */ +struct Twist +{ + float u = 0; + float v = 0; // This will always be 0 in the robot frame. + float omega = 0; + + Twist(void) {} + Twist(float u_, float v_, float om_) : u(u_), v(v_), omega(om_) {} +}; diff --git a/lib/README b/lib/README index 2593a33..a10cade 100644 --- a/lib/README +++ b/lib/README @@ -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 -#include - -int main (void) -{ - ... -} - -``` - -PlatformIO Library Dependency Finder will find automatically dependent -libraries scanning project source files. - -More information about PlatformIO Library Dependency Finder -- https://docs.platformio.org/page/librarymanager/ldf.html + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp index 23bf8de..effe028 100644 --- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp +++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.cpp @@ -1,18 +1,18 @@ -#include "Romi32U4MotorTemplate.h" -#include - -void leftISR(void) -{ - leftMotor.ProcessEncoderTick(); -} - -void rightISR(void) -{ - rightMotor.ProcessEncoderTick(); -} - -void Romi32U4MotorBase::AttachInterrupts(void) -{ - attachPCInt(digitalPinToPCInterrupt(LEFT_XOR), leftISR); - attachInterrupt(digitalPinToInterrupt(RIGHT_XOR), rightISR, CHANGE); -} +#include "Romi32U4MotorTemplate.h" +#include + +void leftISR(void) +{ + leftMotor.ProcessEncoderTick(); +} + +void rightISR(void) +{ + rightMotor.ProcessEncoderTick(); +} + +void Romi32U4MotorBase::AttachInterrupts(void) +{ + attachPCInt(digitalPinToPCInterrupt(LEFT_XOR), leftISR); + attachInterrupt(digitalPinToInterrupt(RIGHT_XOR), rightISR, CHANGE); +} diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h index 22d9a1c..614afc0 100644 --- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h +++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h @@ -1,204 +1,204 @@ -#pragma once - -#include -#include - -// define the motor pins here -#define PWM_L 10 -#define PWM_R 9 -#define DIR_L 16 -#define DIR_R 15 - -#define LEFT_XOR 8 -#define LEFT_B IO_E2 -#define RIGHT_XOR 7 -#define RIGHT_B 23 - -#define OCR_L 0x8A -#define OCR_R 0x88 - -class Romi32U4MotorBase -{ -protected: - // 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 - class Romi32U4EncodedMotor : public Romi32U4MotorBase -{ -protected: - void InitializeMotor(void) - { - FastGPIO::Pin::setOutputLow(); - FastGPIO::Pin::setOutputLow(); - - InitializeEncoder(); - } - - /** - * SetEffort is used internally to set the motor effort without changing the control mode. - */ - void SetEffort(int16_t effort) - { - bool reverse = 0; - - if (effort < 0) - { - effort = -effort; // Make speed a positive quantity. - reverse = 1; // Reverse the direction. - } - if (effort > maxEffort) - { - effort = maxEffort; - } - - FastGPIO::Pin::setOutput(reverse); - - /** - * This line is a little esoteric, but it sets the duty cycle for the appropriate PWM pin. - * Note that TOP for the PWM is 420, so the duty cycle will be (effort / 420) * 100 (in %). - */ - _SFR_MEM16(OCR) = effort; - } - - void InitializeEncoder(void) - { - Serial.println("InitEnc()"); - - // Set the pins as pulled-up inputs. - FastGPIO::Pin::setInputPulledUp(); - FastGPIO::Pin::setInputPulledUp(); - - // Initialize the variables so that the speed will start as 0 - lastB = FastGPIO::Pin::isInputHigh(); - lastA = FastGPIO::Pin::isInputHigh() ^ lastB; - - Serial.println("/InitEnc()"); - } - -public: - Romi32U4EncodedMotor(const String& name) : Romi32U4MotorBase(name) {} - void ProcessEncoderTick(void) - { - bool newB = FastGPIO::Pin::isInputHigh(); - bool newA = FastGPIO::Pin::isInputHigh() ^ newB; - - encCount += (lastA ^ newB) - (newA ^ lastB); - - lastA = newA; - lastB = newB; - } - - friend class Chassis; // Allow Chassis to call protected methods directly -}; - -extern Romi32U4EncodedMotor leftMotor; -extern Romi32U4EncodedMotor rightMotor; +#pragma once + +#include +#include + +// define the motor pins here +#define PWM_L 10 +#define PWM_R 9 +#define DIR_L 16 +#define DIR_R 15 + +#define LEFT_XOR 8 +#define LEFT_B IO_E2 +#define RIGHT_XOR 7 +#define RIGHT_B 23 + +#define OCR_L 0x8A +#define OCR_R 0x88 + +class Romi32U4MotorBase +{ +protected: + // 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 + class Romi32U4EncodedMotor : public Romi32U4MotorBase +{ +protected: + void InitializeMotor(void) + { + FastGPIO::Pin::setOutputLow(); + FastGPIO::Pin::setOutputLow(); + + InitializeEncoder(); + } + + /** + * SetEffort is used internally to set the motor effort without changing the control mode. + */ + void SetEffort(int16_t effort) + { + bool reverse = 0; + + if (effort < 0) + { + effort = -effort; // Make speed a positive quantity. + reverse = 1; // Reverse the direction. + } + if (effort > maxEffort) + { + effort = maxEffort; + } + + FastGPIO::Pin::setOutput(reverse); + + /** + * This line is a little esoteric, but it sets the duty cycle for the appropriate PWM pin. + * Note that TOP for the PWM is 420, so the duty cycle will be (effort / 420) * 100 (in %). + */ + _SFR_MEM16(OCR) = effort; + } + + void InitializeEncoder(void) + { + Serial.println("InitEnc()"); + + // Set the pins as pulled-up inputs. + FastGPIO::Pin::setInputPulledUp(); + FastGPIO::Pin::setInputPulledUp(); + + // Initialize the variables so that the speed will start as 0 + lastB = FastGPIO::Pin::isInputHigh(); + lastA = FastGPIO::Pin::isInputHigh() ^ lastB; + + Serial.println("/InitEnc()"); + } + +public: + Romi32U4EncodedMotor(const String& name) : Romi32U4MotorBase(name) {} + void ProcessEncoderTick(void) + { + bool newB = FastGPIO::Pin::isInputHigh(); + bool newA = FastGPIO::Pin::isInputHigh() ^ newB; + + encCount += (lastA ^ newB) - (newA ^ lastB); + + lastA = newA; + lastB = newB; + } + + friend class Chassis; // Allow Chassis to call protected methods directly +}; + +extern Romi32U4EncodedMotor leftMotor; +extern Romi32U4EncodedMotor rightMotor; diff --git a/lib/Servo32u4/src/servo32u4.cpp b/lib/Servo32u4/src/servo32u4.cpp index 3360e18..fcb6a7d 100644 --- a/lib/Servo32u4/src/servo32u4.cpp +++ b/lib/Servo32u4/src/servo32u4.cpp @@ -1,185 +1,185 @@ -#include - -uint16_t Servo32U4Base::setMinMaxMicroseconds(uint16_t min, uint16_t max) -{ - // swap if in the wrong place - if(min > max) {uint16_t temp = min; min = max; max = temp;} - - usMin = min; - usMax = max; - - return usMax - usMin; //return the range, in case the user wants to do a sanity check -} - -void Servo32U4Pin5::attach(void) -{ - pinMode(5, OUTPUT); // set pin as OUTPUT - - cli(); - - // clear then set the OCR3A bits (pin 5) - TCCR3A = 0x82; //WGM - TCCR3B = 0x1A; //WGM + CS = 8 - ICR3 = 39999; //20ms - - sei(); - - isAttached = true; -} - -void Servo32U4Pin5::detach(void) -{ - cli(); - - // clear the OCR3A bits - TCCR3A &= 0x7f; //cancel OCR3A - sei(); - - isAttached = false; -} - -void Servo32U4Pin5::writeMicroseconds(uint16_t microseconds) -{ - if (!isAttached) - { - attach(); - } - - microseconds = constrain(microseconds, usMin, usMax); - - //prescaler is 8, so 1 timer count = 0.5 us - OCR3A = (microseconds << 1) - 1; // multiplies by 2 -} - -void Servo32U4Pin6::attach(void) -{ - pinMode(6, OUTPUT); // set pin as OUTPUT - - cli(); - - // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() - TCCR4C |= 0x05; - - sei(); - - isAttached = true; -} - -void Servo32U4Pin6::detach(void) -{ - cli(); - - // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() - TCCR4C &= ~0x05; - - sei(); - - isAttached = false; -} - - -// Resolution is 64 us; not great, but shouldn't be too constraining -void Servo32U4Pin6::writeMicroseconds(uint16_t microseconds) -{ - if (!isAttached) - { - attach(); - } - - microseconds = constrain(microseconds, usMin, usMax); - - //prescaler is 512, so 1 timer count = 32 us - //but be sure to set TC4H first! - TC4H = 0; - OCR4D = (microseconds >> 5) - 1; // divides by 32 -} - -void Servo32U4Pin13::attach(void) -{ - pinMode(13, OUTPUT); // set pin as OUTPUT - - cli(); - - // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() - TCCR4A |= 0x82; - - sei(); - - isAttached = true; -} - -void Servo32U4Pin13::detach(void) -{ - cli(); - - // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() - // If you're not using it for anything else, you can safely just set TCCR4A = 0, - // but we'll cancel the bits we set above - TCCR4A &= ~0x82; - - sei(); - - isAttached = false; -} - -// Resolution is 64 us; not great, but shouldn't be too constraining -void Servo32U4Pin13::writeMicroseconds(uint16_t microseconds) -{ - if (!isAttached) - { - attach(); - } - - microseconds = constrain(microseconds, usMin, usMax); - - //prescaler is 512, so 1 timer count = 32 us - //but be sure to set TC4H first! - TC4H = 0; - OCR4A = (microseconds >> 5) - 1; // divides by 32 -} - -void Servo32U4Pin12::attach(void) -{ - pinMode(12, OUTPUT); // set pin as OUTPUT - - cli(); - - // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() - TCCR4C |= 0x05; - - //Invert the output on pin 12 - TCCR4B |= 0x80; - - sei(); - - isAttached = true; -} - -void Servo32U4Pin12::detach(void) -{ - cli(); - - // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() - TCCR4C = 0x00; - TCCR4B &= ~0x80; - - sei(); - - isAttached = false; -} - -// Resolution is 64 us; not great, but shouldn't be too constraining -void Servo32U4Pin12::writeMicroseconds(uint16_t microseconds) -{ - if (!isAttached) - { - attach(); - } - - microseconds = constrain(microseconds, usMin, usMax); - - //prescaler is 512, so 1 timer count = 32 us - //but be sure to set TC4H first! - TC4H = 0; - OCR4D = (microseconds >> 5) - 1; // divides by 32 -} +#include + +uint16_t Servo32U4Base::setMinMaxMicroseconds(uint16_t min, uint16_t max) +{ + // swap if in the wrong place + if(min > max) {uint16_t temp = min; min = max; max = temp;} + + usMin = min; + usMax = max; + + return usMax - usMin; //return the range, in case the user wants to do a sanity check +} + +void Servo32U4Pin5::attach(void) +{ + pinMode(5, OUTPUT); // set pin as OUTPUT + + cli(); + + // clear then set the OCR3A bits (pin 5) + TCCR3A = 0x82; //WGM + TCCR3B = 0x1A; //WGM + CS = 8 + ICR3 = 39999; //20ms + + sei(); + + isAttached = true; +} + +void Servo32U4Pin5::detach(void) +{ + cli(); + + // clear the OCR3A bits + TCCR3A &= 0x7f; //cancel OCR3A + sei(); + + isAttached = false; +} + +void Servo32U4Pin5::writeMicroseconds(uint16_t microseconds) +{ + if (!isAttached) + { + attach(); + } + + microseconds = constrain(microseconds, usMin, usMax); + + //prescaler is 8, so 1 timer count = 0.5 us + OCR3A = (microseconds << 1) - 1; // multiplies by 2 +} + +void Servo32U4Pin6::attach(void) +{ + pinMode(6, OUTPUT); // set pin as OUTPUT + + cli(); + + // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() + TCCR4C |= 0x05; + + sei(); + + isAttached = true; +} + +void Servo32U4Pin6::detach(void) +{ + cli(); + + // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() + TCCR4C &= ~0x05; + + sei(); + + isAttached = false; +} + + +// Resolution is 64 us; not great, but shouldn't be too constraining +void Servo32U4Pin6::writeMicroseconds(uint16_t microseconds) +{ + if (!isAttached) + { + attach(); + } + + microseconds = constrain(microseconds, usMin, usMax); + + //prescaler is 512, so 1 timer count = 32 us + //but be sure to set TC4H first! + TC4H = 0; + OCR4D = (microseconds >> 5) - 1; // divides by 32 +} + +void Servo32U4Pin13::attach(void) +{ + pinMode(13, OUTPUT); // set pin as OUTPUT + + cli(); + + // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() + TCCR4A |= 0x82; + + sei(); + + isAttached = true; +} + +void Servo32U4Pin13::detach(void) +{ + cli(); + + // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() + // If you're not using it for anything else, you can safely just set TCCR4A = 0, + // but we'll cancel the bits we set above + TCCR4A &= ~0x82; + + sei(); + + isAttached = false; +} + +// Resolution is 64 us; not great, but shouldn't be too constraining +void Servo32U4Pin13::writeMicroseconds(uint16_t microseconds) +{ + if (!isAttached) + { + attach(); + } + + microseconds = constrain(microseconds, usMin, usMax); + + //prescaler is 512, so 1 timer count = 32 us + //but be sure to set TC4H first! + TC4H = 0; + OCR4A = (microseconds >> 5) - 1; // divides by 32 +} + +void Servo32U4Pin12::attach(void) +{ + pinMode(12, OUTPUT); // set pin as OUTPUT + + cli(); + + // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() + TCCR4C |= 0x05; + + //Invert the output on pin 12 + TCCR4B |= 0x80; + + sei(); + + isAttached = true; +} + +void Servo32U4Pin12::detach(void) +{ + cli(); + + // Be careful here, since Timer4 is used to manage speed controller. See Chassis::init() + TCCR4C = 0x00; + TCCR4B &= ~0x80; + + sei(); + + isAttached = false; +} + +// Resolution is 64 us; not great, but shouldn't be too constraining +void Servo32U4Pin12::writeMicroseconds(uint16_t microseconds) +{ + if (!isAttached) + { + attach(); + } + + microseconds = constrain(microseconds, usMin, usMax); + + //prescaler is 512, so 1 timer count = 32 us + //but be sure to set TC4H first! + TC4H = 0; + OCR4D = (microseconds >> 5) - 1; // divides by 32 +} diff --git a/lib/Servo32u4/src/servo32u4.h b/lib/Servo32u4/src/servo32u4.h index 3ef5f1b..f39ca8e 100644 --- a/lib/Servo32u4/src/servo32u4.h +++ b/lib/Servo32u4/src/servo32u4.h @@ -1,182 +1,182 @@ -#pragma once - -#include - -/** \file Manages servos on up to three pins: - * pin 5, - * pin 13, and - * either pin 6 OR 12 (don't use 6 and 12 simultaneously!). - * - * THIS IS DESIGNED TO WORK WITH THE ROMI SET UP WITH THE CHASSIS CLASS - * - * The Chassis class sets up Timer4, which is needed for pins 6, 12, and 13! - * - * Pin 5 uses Timer3 and has the fewest restrictions as well as the highest precision (0.5 us). - * - * The other three have potential conflicts and much lower resolution (64 us). - * - * Pin 13 uses Timer4, but shares functionality with the bootloader, which flashes the LED connected - * to that pin. So, your servo will go a bit nuts when you upload -- be careful! - * - * Pins 6 and 12 use Timer4, but share the same output compare -- except that they are inverted. - * Practically, that means you can use one or the other. Note that pin 6 is shared with the buzzer, - * so you'll need to cut the buzzer trace if you want to use that pin and maintain your sanity. - * - * If using the Chassis class, Timer4 is used for the control loop, so be sure you don't create any - * conflicts. - * - * writeMicroseconds is protected so that you don't call it directly (which will make it move - * really fast). Instead, set the target and call update() regularly. - * - * */ - -/** \class Servo32U4Base - * \brief Base class class for servos. - * - * Each derived class controls a specific pin (obvious from the name). - * - * Defaults to a range of 1000 - 2000 us, but can be customized. - */ -class Servo32U4Base -{ -protected: - uint16_t usMin = 1000; - uint16_t usMax = 2000; - - uint8_t feedbackPin = -1; - bool isAttached = false; - - uint16_t targetPos = 1500; - uint16_t currentPos = 1500; - -public: - // Virtual functions defined for each specific class - virtual void attach(void) = 0; - virtual void detach(void) = 0; - void setTargetPos(uint16_t target) {targetPos = target;} - - /** - * update() moves the servo towards the target position. You will want to - * change it to return a bool to detect the event of reaching the target. - */ - void update(void) - { - if(targetPos == currentPos) {} // no need to update - - else if(abs(targetPos - currentPos) <= 40) currentPos = targetPos; - else if(targetPos > currentPos) currentPos += 40; - else if(targetPos < currentPos) currentPos -= 40; - - writeMicroseconds(currentPos); - } - - uint16_t setMinMaxMicroseconds(uint16_t min, uint16_t max); - - virtual void writeMicroseconds(uint16_t microseconds) = 0; -}; - -/** \class Servo32U4Pin5 - * \brief A servo class to control a servo on pin 5. - * - * Servo32U4 uses output compare on Timer3 to control the pulse to the servo. - * The 16-bit Timer3 is set up with a pre-scaler of 8, TOP of 39999 + 1 => 20 ms interval. - * - * OCR3A controls the pulse on pin 5 -- this servo must be on pin 5! - * - * Defaults to a range of 1000 - 2000 us, but can be customized. - */ -class Servo32U4Pin5 : public Servo32U4Base -{ -public: - void attach(void); - void detach(void); - -protected: - void writeMicroseconds(uint16_t microseconds); -}; - -/** \class Servo32U4Pin6 - * \brief A servo class to control a servo on pin 6. - * - * Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo. - * _In Chassis::init(), - * The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval. - * - * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, - * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() - * - * OCR4D controls the pulse on pin 6 -- this servo must be on pin 6! - * - * Note that pin 6 controls the buzzer, so you'll go crazy if you don't cut the buzzer trace. - * See: https://www.pololu.com/docs/0J69/3.2 for how to cut the trace. - * - * Defaults to a range of 1000 - 2000 us, but can be customized. - * - * Note that because we're using an 8-bit timer, resolution is only 64 us. - */ -class Servo32U4Pin6 : public Servo32U4Base -{ -public: - void attach(void); - void detach(void); - -protected: - void writeMicroseconds(uint16_t microseconds); -}; - -/** \class Servo32U4Pin13 - * \brief A servo class to control a servo on pin 13. - * - * Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo. - * _In Chassis::init(), - * The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval. - * - * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, - * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() - * - * OCR4A controls the pulse on pin 13 -- this servo must be on pin 13! - * - * Note that there is a useful LED on pin 13 -- you'll lose that functionality. - * - * Pin 13 is also used during the upload process, so your servo will go crazy when uploading! - * - * Defaults to a range of 1000 - 2000 us, but can be customized. - * - * Note that because we're using an 8-bit timer, resolution is only 64 us. - */ -class Servo32U4Pin13 : public Servo32U4Base -{ -public: - void attach(void); - void detach(void); - -protected: - void writeMicroseconds(uint16_t microseconds); -}; - -/** \class Servo32U4Pin12 - * \brief A servo class to control a servo on pin 12. - * - * Servo32U4Pin12 uses output compare on Timer4 to control the pulse to the servo. - * _In Chassis::init(), - * - * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, - * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() - * - * ^OCR4D controls the pulse on pin 12 -- this servo _must_ be on pin 12! - * - * DO NOT USE IN CONJUNCTION WITH Servo32U4Pin6 - * - * Defaults to a range of 1000 - 2000 us, but can be customized. - * - * Note that because we're using an 8-bit timer, resolution is only 64 us. - */ -class Servo32U4Pin12 : public Servo32U4Base -{ -public: - void attach(void); - void detach(void); - -protected: - void writeMicroseconds(uint16_t microseconds); -}; +#pragma once + +#include + +/** \file Manages servos on up to three pins: + * pin 5, + * pin 13, and + * either pin 6 OR 12 (don't use 6 and 12 simultaneously!). + * + * THIS IS DESIGNED TO WORK WITH THE ROMI SET UP WITH THE CHASSIS CLASS + * + * The Chassis class sets up Timer4, which is needed for pins 6, 12, and 13! + * + * Pin 5 uses Timer3 and has the fewest restrictions as well as the highest precision (0.5 us). + * + * The other three have potential conflicts and much lower resolution (64 us). + * + * Pin 13 uses Timer4, but shares functionality with the bootloader, which flashes the LED connected + * to that pin. So, your servo will go a bit nuts when you upload -- be careful! + * + * Pins 6 and 12 use Timer4, but share the same output compare -- except that they are inverted. + * Practically, that means you can use one or the other. Note that pin 6 is shared with the buzzer, + * so you'll need to cut the buzzer trace if you want to use that pin and maintain your sanity. + * + * If using the Chassis class, Timer4 is used for the control loop, so be sure you don't create any + * conflicts. + * + * writeMicroseconds is protected so that you don't call it directly (which will make it move + * really fast). Instead, set the target and call update() regularly. + * + * */ + +/** \class Servo32U4Base + * \brief Base class class for servos. + * + * Each derived class controls a specific pin (obvious from the name). + * + * Defaults to a range of 1000 - 2000 us, but can be customized. + */ +class Servo32U4Base +{ +protected: + uint16_t usMin = 1000; + uint16_t usMax = 2000; + + uint8_t feedbackPin = -1; + bool isAttached = false; + + uint16_t targetPos = 1500; + uint16_t currentPos = 1500; + +public: + // Virtual functions defined for each specific class + virtual void attach(void) = 0; + virtual void detach(void) = 0; + void setTargetPos(uint16_t target) {targetPos = target;} + + /** + * update() moves the servo towards the target position. You will want to + * change it to return a bool to detect the event of reaching the target. + */ + void update(void) + { + if(targetPos == currentPos) {} // no need to update + + else if(abs(targetPos - currentPos) <= 40) currentPos = targetPos; + else if(targetPos > currentPos) currentPos += 40; + else if(targetPos < currentPos) currentPos -= 40; + + writeMicroseconds(currentPos); + } + + uint16_t setMinMaxMicroseconds(uint16_t min, uint16_t max); + + virtual void writeMicroseconds(uint16_t microseconds) = 0; +}; + +/** \class Servo32U4Pin5 + * \brief A servo class to control a servo on pin 5. + * + * Servo32U4 uses output compare on Timer3 to control the pulse to the servo. + * The 16-bit Timer3 is set up with a pre-scaler of 8, TOP of 39999 + 1 => 20 ms interval. + * + * OCR3A controls the pulse on pin 5 -- this servo must be on pin 5! + * + * Defaults to a range of 1000 - 2000 us, but can be customized. + */ +class Servo32U4Pin5 : public Servo32U4Base +{ +public: + void attach(void); + void detach(void); + +protected: + void writeMicroseconds(uint16_t microseconds); +}; + +/** \class Servo32U4Pin6 + * \brief A servo class to control a servo on pin 6. + * + * Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo. + * _In Chassis::init(), + * The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval. + * + * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, + * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() + * + * OCR4D controls the pulse on pin 6 -- this servo must be on pin 6! + * + * Note that pin 6 controls the buzzer, so you'll go crazy if you don't cut the buzzer trace. + * See: https://www.pololu.com/docs/0J69/3.2 for how to cut the trace. + * + * Defaults to a range of 1000 - 2000 us, but can be customized. + * + * Note that because we're using an 8-bit timer, resolution is only 64 us. + */ +class Servo32U4Pin6 : public Servo32U4Base +{ +public: + void attach(void); + void detach(void); + +protected: + void writeMicroseconds(uint16_t microseconds); +}; + +/** \class Servo32U4Pin13 + * \brief A servo class to control a servo on pin 13. + * + * Servo32U4Pin6 uses output compare on Timer4 to control the pulse to the servo. + * _In Chassis::init(), + * The 8-bit Timer4 is set up with a pre-scaler of 1024, TOP of 249 + 1 => 16 ms interval. + * + * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, + * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() + * + * OCR4A controls the pulse on pin 13 -- this servo must be on pin 13! + * + * Note that there is a useful LED on pin 13 -- you'll lose that functionality. + * + * Pin 13 is also used during the upload process, so your servo will go crazy when uploading! + * + * Defaults to a range of 1000 - 2000 us, but can be customized. + * + * Note that because we're using an 8-bit timer, resolution is only 64 us. + */ +class Servo32U4Pin13 : public Servo32U4Base +{ +public: + void attach(void); + void detach(void); + +protected: + void writeMicroseconds(uint16_t microseconds); +}; + +/** \class Servo32U4Pin12 + * \brief A servo class to control a servo on pin 12. + * + * Servo32U4Pin12 uses output compare on Timer4 to control the pulse to the servo. + * _In Chassis::init(), + * + * YOU MUST CALL Chasssis::init() IN setup() FOR THIS TO WORK, + * AND YOU MUST CALL Chassis::init() BEFORE YOU CALL attach() + * + * ^OCR4D controls the pulse on pin 12 -- this servo _must_ be on pin 12! + * + * DO NOT USE IN CONJUNCTION WITH Servo32U4Pin6 + * + * Defaults to a range of 1000 - 2000 us, but can be customized. + * + * Note that because we're using an 8-bit timer, resolution is only 64 us. + */ +class Servo32U4Pin12 : public Servo32U4Base +{ +public: + void attach(void); + void detach(void); + +protected: + void writeMicroseconds(uint16_t microseconds); +}; diff --git a/platformio.ini b/platformio.ini index 65db724..c12b564 100644 --- a/platformio.ini +++ b/platformio.ini @@ -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__ diff --git a/src/main.cpp b/src/main.cpp index 84d72f8..ba09f10 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,25 +1,25 @@ -#include -#include "robot.h" - -Robot robot; - -void setup() -{ - Serial.begin(115200); - -/** - * If you define __SETUP_DEBUG__ in your .ini file, this line will make the program wait - * until the Serial is set up so you can debug. WARNING: If you do that, you _must_ open - * the Serial Monitor for anything to happen! - */ -#ifdef __SETUP_DEBUG__ - while(!Serial){delay(5);} -#endif - - robot.InitializeRobot(); -} - -void loop() -{ - robot.RobotLoop(); -} +#include +#include "robot.h" + +Robot robot; + +void setup() +{ + Serial.begin(115200); + +/** + * If you define __SETUP_DEBUG__ in your .ini file, this line will make the program wait + * until the Serial is set up so you can debug. WARNING: If you do that, you _must_ open + * the Serial Monitor for anything to happen! + */ +#ifdef __SETUP_DEBUG__ + while(!Serial){delay(5);} +#endif + + robot.InitializeRobot(); +} + +void loop() +{ + robot.RobotLoop(); +} diff --git a/src/robot-nav.cpp b/src/robot-nav.cpp index 8319790..4fd96a6 100644 --- a/src/robot-nav.cpp +++ b/src/robot-nav.cpp @@ -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. + */ } \ No newline at end of file diff --git a/src/robot.cpp b/src/robot.cpp index 6d1bc35..4a39fc8 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -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(); + } + } +} diff --git a/src/robot.h b/src/robot.h index df8a8d0..40081bd 100644 --- a/src/robot.h +++ b/src/robot.h @@ -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); +}; diff --git a/test/README b/test/README index 9b1e87b..b0416ad 100644 --- a/test/README +++ b/test/README @@ -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