ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

Major simplification

Browse source
This commit is contained in:
Griffin Tabor 2025-09-02 21:42:40 -04:00
parent 68b52a9b14
commit b5d7ab47c3
9 changed files with 17 additions and 231 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -108,9 +108,6 @@ bool Chassis::ChassisLoop(Twist& velocity)
Serial.print('\n');
#endif
// motor updates
UpdateMotors();
/* Update the wheel velocity so it gets back to Robot. */
velocity = CalcOdomFromWheelMotion();
@ -139,30 +136,6 @@ void Chassis::SetMotorEfforts(int16_t left, int16_t right)
rightMotor.SetMotorEffortDirect(right);
}
void Chassis::UpdateMotors(void)
{
leftMotor.ControlMotorSpeed();
rightMotor.ControlMotorSpeed();
}
/**
* SetWheelSpeeds converts the linear wheel speeds (axes relative to ground) to motor speeds.
*/
void Chassis::SetWheelSpeeds(float leftSpeedCMperSec, float rightSpeedCMperSec)
{
/**
* TODO: Check the code below. You did this in Lab 1, so we give you the calcs.
*/
leftMotor.SetTargetSpeed(leftSpeedCMperSec * LEFT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.);
rightMotor.SetTargetSpeed(rightSpeedCMperSec * RIGHT_TICKS_PER_CM * CONTROL_LOOP_PERIOD_MS / 1000.);
}
void Chassis::SetTwist(const Twist& twist)
{
/**
* TODO: Complete SetTwist() to call SetWheelSpeeds() from target u and omega
*/
}
Twist Chassis::CalcOdomFromWheelMotion(void)
{

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

@ -42,12 +42,10 @@ public:
void SetTwist(const Twist& twist);
Twist CalcOdomFromWheelMotion(void);
/**
* A utility function for converting robot speed to wheel speed. Left
* public so that you can test more easily.
*/
void SetWheelSpeeds(float, float);
void Stop(void) { SetWheelSpeeds(0, 0); }
void SetMotorEfforts(int16_t, int16_t);
void Stop(void) { SetMotorEfforts(0, 0);}
protected:
/**
@ -57,5 +55,4 @@ protected:
void InitializeMotors(void);
void UpdateMotors(void);
void SetMotorEfforts(int16_t, int16_t);
};

5
lib/Romi32U4Motors/src/Romi32U4MotorParameters.h
View file

@ -1,5 +0,0 @@
#define KP_MOTOR 5.0
#define KI_MOTOR 0.5
#define KD_MOTOR 0.0 // don't use
#define INTEGRAL_CAP_MOTOR 0.0 // if you want to add an integral cap

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

@ -2,7 +2,6 @@
#include <Arduino.h>
#include <FastGPIO.h>
#include "Romi32U4MotorParameters.h"
// define the motor pins here
#define PWM_L 10
@ -24,30 +23,12 @@ protected:
// For prettier printing
const String name;
// Used to control the motors in different ways
enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
// Used to manage PID coefficients; note that the defaults stink for speed control.
float Kp = KP_MOTOR;
float Ki = KI_MOTOR;
float Kd = KD_MOTOR;
// Used to keep track of the target speed, in counts / interval.
float targetSpeed = 0;
/**
* This is the speed of the motor, in "encoder counts / encoder interval".
* The encoder interval is set in Robot::InitializeMotorControlTimer.
*/
volatile int16_t speed = 0;
// For tracking the error integral (sum)
float sumError = 0;
// For tracking derivative (difference)
float prevError = 0;
// Maximum effort
int16_t maxEffort = 420;
@ -69,7 +50,6 @@ protected:
*/
void SetMotorEffortDirect(int16_t effort)
{
ctrlMode = CTRL_DIRECT;
SetEffort(effort);
}
@ -93,64 +73,20 @@ protected:
}
/**
* Sets the target speed in "encoder ticks/16 ms interval"
* */
void SetTargetSpeed(float target)
{
targetSpeed = target;
if(ctrlMode != CTRL_SPEED)
{
// Reset the error integral if we are switching from another mode
// Otherwise, the robot may jump due to residual integral
sumError = 0;
// Also set prevCount to encCount so to avoid speed jumps when switching mode
CalcEncoderDelta();
}
ctrlMode = CTRL_SPEED;
}
/**
* ControlMotorSpeed implements the PID controller. It should _not_ be called by user code.
* Instead, ControlMotorSpeed is called from Chassis::UpdateMotors, which is on a timer schedule.
* GetEncoderTotal() returns the total encoder counts"
*
*/
void ControlMotorSpeed(void)
int16_t GetEncoderTotal(void)
{
if(ctrlMode == CTRL_SPEED)
{
/**
* TODO: Implement PI controller. Only P at the moment.
*/
cli();
int16_t currCount = encCount;
sei();
// Calculate the error in speed
float error = targetSpeed - speed;
sumError+= error;
int16_t effort = Kp * error + Ki * sumError;
// Set the effort for the motor
SetEffort(effort);
#ifdef __MOTOR_DEBUG__
Serial.print('>');
Serial.print(name);
Serial.print("_target:");
Serial.println(targetSpeed);
Serial.print('>');
Serial.print(name);
Serial.print("_speed:");
Serial.println(speed);
Serial.print('>');
Serial.print(name);
Serial.print("_error:");
Serial.println(error);
#endif
}
return currCount;
}
static void AttachInterrupts(void);
/**
@ -193,7 +129,6 @@ protected:
public:
Romi32U4MotorBase(const String& nm) : name(nm) {}
void SetPIDCoeffs(float p, float i, float d) {Kp = p; Ki = i; Kd = d; sumError = 0;}
};
template <uint8_t encXOR, uint8_t encB, uint8_t PWM, uint8_t DIR, uint8_t OCR>

1
platformio.ini
View file

@ -17,7 +17,6 @@ monitor_speed = 115200
lib_deps =
Wire
https://github.com/gcl8a/IRDecoder
https://github.com/gcl8a/Button
https://github.com/gcl8a/event_timer
https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities

2
src/robot-nav.cpp
View file

@ -59,7 +59,7 @@ void Robot::DriveToPoint(void)
#endif
/**
* TODO: Call chassis.SetTwist() to command the motion, based on your calculations above.
* TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above.
*/
}
}

83
src/robot-remote.cpp
View file

@ -1,83 +0,0 @@
/**
* robot-remote.cpp implements features of class Robot that are related to processing
* remote control commands. It also manages modes. You might want to trim away some of
* the code that isn't needed later in the term.
*/
#include "robot.h"
#include <ir_codes.h>
#include <IRdecoder.h>
/**
* IRDecoder decoder is declared extern in IRdecoder.h (for ISR purposes),
* so we _must_ name it decoder.
*/
#define IR_PIN 17
IRDecoder decoder(IR_PIN);
void Robot::HandleKeyCode(int16_t keyCode)
{
Serial.println(keyCode);
// Regardless of current state, if ENTER is pressed, go to idle state
if(keyCode == ENTER_SAVE) { EnterIdleState(); keyString = "";}
// If STOP/MODE is pressed, it toggles control mode (auto <-> teleop)
else if(keyCode == STOP_MODE)
{
if(robotCtrlMode == CTRL_AUTO) {EnterTeleopMode(); }
else if(robotCtrlMode == CTRL_TELEOP) {EnterAutoMode(); }
EnterIdleState(); // Idle to avoid surprises
keyString = "";
}
if(robotCtrlMode == CTRL_AUTO)
{
/**
* TODO: Add destinations.
*/
switch(keyCode)
{
case NUM_2:
SetDestination(Pose(60, 0, 0));
break;
case NUM_4:
break;
case NUM_6:
break;
case NUM_8:
break;
}
keyString = "";
}
else if(robotCtrlMode == CTRL_TELEOP)
{
switch(keyCode)
{
case UP_ARROW:
chassis.SetTwist(Twist(20, 0, 0));
break;
case RIGHT_ARROW:
break;
case DOWN_ARROW:
break;
case LEFT_ARROW:
break;
}
keyString = "";
}
}
void Robot::EnterTeleopMode(void)
{
Serial.println("-> TELEOP");
robotCtrlMode = CTRL_TELEOP;
}
void Robot::EnterAutoMode(void)
{
Serial.println("-> AUTO");
robotCtrlMode = CTRL_AUTO;
}

16
src/robot.cpp
View file

@ -1,16 +1,9 @@
#include "robot.h"
#include <IRdecoder.h>
void Robot::InitializeRobot(void)
{
chassis.InititalizeChassis();
/**
* Initialize the IR decoder. Declared extern in IRdecoder.h; see robot-remote.cpp
* for instantiation and setting the pin.
*/
decoder.init();
/**
* TODO: Set pin 13 HIGH when navigating and LOW when destination is reached.
* Need to set as OUTPUT here.
@ -27,16 +20,10 @@ void Robot::EnterIdleState(void)
/**
* The main loop for your robot. Process both synchronous events (motor control),
* and asynchronous events (IR presses, distance readings, etc.).
* and asynchronous events (distance readings, etc.).
*/
void Robot::RobotLoop(void)
{
/**
* Handle any IR remote keypresses.
*/
int16_t keyCode = decoder.getKeyCode();
if(keyCode != -1) HandleKeyCode(keyCode);
/**
* Run the chassis loop, which handles low-level control.
*/
@ -45,6 +32,7 @@ void Robot::RobotLoop(void)
{
// 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

18
src/robot.h
View file

@ -5,17 +5,6 @@
class Robot
{
protected:
/**
* We define some modes for you. SETUP is used for adjusting gains and so forth. Most
* of the activities will run in AUTO. You shouldn't need to mess with these.
*/
enum ROBOT_CTRL_MODE
{
CTRL_TELEOP,
CTRL_AUTO,
};
ROBOT_CTRL_MODE robotCtrlMode = CTRL_AUTO;
/**
* robotState is used to track the current task of the robot. You will add new states as
* the term progresses.
@ -45,16 +34,9 @@ public:
void RobotLoop(void);
protected:
/* For managing IR remote key presses*/
void HandleKeyCode(int16_t keyCode);
/* State changes */
void EnterIdleState(void);
/* Mode changes */
void EnterTeleopMode(void);
void EnterAutoMode(void);
// /* Navigation methods.*/
void UpdatePose(const Twist& u);
void SetDestination(const Pose& destination);