Skip to content

Commit

Permalink
Started work on odom refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@SizzinSeal
SizzinSeal committed Sep 12, 2023
1 parent c04683a commit ef00cb0
Show file tree
Hide file tree
Showing 3 changed files with 81 additions and 206 deletions.
1 change: 1 addition & 0 deletions include/lemlib/chassis/chassis.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,7 @@ float defaultDriveCurve(float input, float scale);
*
*/
class Chassis {
friend class Odometry;
public:
/**
* @brief Construct a new Chassis
Expand Down
97 changes: 44 additions & 53 deletions include/lemlib/chassis/odom.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,57 +16,48 @@
#include "lemlib/pose.hpp"

namespace lemlib {
/**
* @brief Set the sensors to be used for odometry
*
* @param sensors the sensors to be used
* @param drivetrain drivetrain to be used
*/
void setSensors(lemlib::OdomSensors_t sensors, lemlib::Drivetrain_t drivetrain);
/**
* @brief Get the pose of the robot
*
* @param radians true for theta in radians, false for degrees. False by default
* @return Pose
*/
Pose getPose(bool radians = false);
/**
* @brief Set the Pose of the robot
*
* @param pose the new pose
* @param radians true if theta is in radians, false if in degrees. False by default
*/
void setPose(Pose pose, bool radians = false);
/**
* @brief Get the speed of the robot
*
* @param radians true for theta in radians, false for degrees. False by default
* @return lemlib::Pose
*/
Pose getSpeed(bool radians = false);
/**
* @brief Get the local speed of the robot
*
* @param radians true for theta in radians, false for degrees. False by default
* @return lemlib::Pose
*/
Pose getLocalSpeed(bool radians = false);
/**
* @brief Estimate the pose of the robot after a certain amount of time
*
* @param time time in seconds
* @param radians False for degrees, true for radians. False by default
* @return lemlib::Pose
*/
Pose estimatePose(float time, bool radians = false);
/**
* @brief Update the pose of the robot
*
*/
void update();
/**
* @brief Initialize the odometry system
*
*/
void init();
class Odometry {
public:
/**
* @brief Construct a new Odometry object
*
* @param chassis pointer to the chassis object
*/
Odometry(Chassis* chassis);

/**
* @brief Get the Pose
*
* @note Units are in radians. Locked from 0 to 2pi. Right is 0, increasing counter-clockwise
*
* @return Pose
*/
Pose getPose();

/**
* @brief Set the Pose
*
* @note Units are in radians. Right is 0, increasing counter-clockwise
*
* @param pose
*/
void setPose();

/**
* @brief Update the pose of the robot
*
* @note this should be called in a loop, ideally every 10ms (the sensor polling rate)
*/
void update();
private:
float prevVertical = 0;
float prevVertical1 = 0;
float prevVertical2 = 0;
float prevHorizontal = 0;
float prevHorizontal1 = 0;
float prevHorizontal2 = 0;
float prevImu = 0;
Pose pose = Pose(0, 0, 0); // the pose of the robot
Chassis* chassis;
};
} // namespace lemlib
189 changes: 36 additions & 153 deletions src/lemlib/chassis/odom.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,128 +15,32 @@
// http://thepilons.ca/wp-content/uploads/2018/10/Tracking.pdf

#include <math.h>
#include "pros/rtos.hpp"
#include "lemlib/util.hpp"
#include "lemlib/chassis/odom.hpp"
#include "lemlib/chassis/chassis.hpp"
#include "lemlib/chassis/trackingWheel.hpp"

// tracking thread
pros::Task* trackingTask = nullptr;

// global variables
lemlib::OdomSensors_t odomSensors; // the sensors to be used for odometry
lemlib::Drivetrain_t drive; // the drivetrain to be used for odometry
lemlib::Pose odomPose(0, 0, 0); // the pose of the robot
lemlib::Pose odomSpeed(0, 0, 0); // the speed of the robot
lemlib::Pose odomLocalSpeed(0, 0, 0); // the local speed of the robot

float prevVertical = 0;
float prevVertical1 = 0;
float prevVertical2 = 0;
float prevHorizontal = 0;
float prevHorizontal1 = 0;
float prevHorizontal2 = 0;
float prevImu = 0;

/**
* @brief Set the sensors to be used for odometry
*
* @param sensors the sensors to be used
* @param drivetrain drivetrain to be used
*/
void lemlib::setSensors(lemlib::OdomSensors_t sensors, lemlib::Drivetrain_t drivetrain) {
odomSensors = sensors;
drive = drivetrain;
}

/**
* @brief Get the pose of the robot
*
* @param radians true for theta in radians, false for degrees. False by default
* @return Pose
*/
lemlib::Pose lemlib::getPose(bool radians) {
if (radians) return odomPose;
else return lemlib::Pose(odomPose.x, odomPose.y, radToDeg(odomPose.theta));
}

/**
* @brief Set the Pose of the robot
*
* @param pose the new pose
* @param radians true if theta is in radians, false if in degrees. False by default
*/
void lemlib::setPose(lemlib::Pose pose, bool radians) {
if (radians) odomPose = pose;
else odomPose = lemlib::Pose(pose.x, pose.y, degToRad(pose.theta));
}

/**
* @brief Get the speed of the robot
*
* @param radians true for theta in radians, false for degrees. False by default
* @return lemlib::Pose
*/
lemlib::Pose lemlib::getSpeed(bool radians) {
if (radians) return odomSpeed;
else return lemlib::Pose(odomSpeed.x, odomSpeed.y, radToDeg(odomSpeed.theta));
}
using namespace lemlib;

/**
* @brief Get the local speed of the robot
* Odometry update function
*
* @param radians true for theta in radians, false for degrees. False by default
* @return lemlib::Pose
*/
lemlib::Pose lemlib::getLocalSpeed(bool radians) {
if (radians) return odomLocalSpeed;
else return lemlib::Pose(odomLocalSpeed.x, odomLocalSpeed.y, radToDeg(odomLocalSpeed.theta));
}

/**
* @brief Estimate the pose of the robot after a certain amount of time
*
* @param time time in seconds
* @param radians False for degrees, true for radians. False by default
* @return lemlib::Pose
*/
lemlib::Pose lemlib::estimatePose(float time, bool radians) {
// get current position and speed
Pose curPose = getPose(true);
Pose localSpeed = getLocalSpeed(true);
// calculate the change in local position
Pose deltaLocalPose = localSpeed * time;

// calculate the future pose
float avgHeading = curPose.theta + deltaLocalPose.theta / 2;
Pose futurePose = curPose;
futurePose.x += deltaLocalPose.y * sin(avgHeading);
futurePose.y += deltaLocalPose.y * cos(avgHeading);
futurePose.x += deltaLocalPose.x * -cos(avgHeading);
futurePose.y += deltaLocalPose.x * sin(avgHeading);
if (!radians) futurePose.theta = radToDeg(futurePose.theta);

return futurePose;
}

/**
* @brief Update the pose of the robot
*
*/
void lemlib::update() {
void Odometry::update() {
// TODO: add particle filter
// get the current sensor values
float vertical1Raw = 0;
float vertical2Raw = 0;
float horizontal1Raw = 0;
float horizontal2Raw = 0;
float imuRaw = 0;
if (odomSensors.vertical1 != nullptr) vertical1Raw = odomSensors.vertical1->getDistanceTraveled();
if (odomSensors.vertical2 != nullptr) vertical2Raw = odomSensors.vertical2->getDistanceTraveled();
if (odomSensors.horizontal1 != nullptr) horizontal1Raw = odomSensors.horizontal1->getDistanceTraveled();
if (odomSensors.horizontal2 != nullptr) horizontal2Raw = odomSensors.horizontal2->getDistanceTraveled();
if (odomSensors.imu != nullptr) imuRaw = degToRad(odomSensors.imu->get_rotation());
if (chassis->odomSensors.vertical1 != nullptr) vertical1Raw = chassis->odomSensors.vertical1->getDistanceTraveled();
if (chassis->odomSensors.vertical2 != nullptr) vertical2Raw = chassis->odomSensors.vertical2->getDistanceTraveled();
if (chassis->odomSensors.horizontal1 != nullptr)
horizontal1Raw = chassis->odomSensors.horizontal1->getDistanceTraveled();
if (chassis->odomSensors.horizontal2 != nullptr)
horizontal2Raw = chassis->odomSensors.horizontal2->getDistanceTraveled();
if (chassis->odomSensors.imu != nullptr) imuRaw = degToRad(chassis->odomSensors.imu->get_rotation());

// calculate the change in sensor values
float deltaVertical1 = vertical1Raw - prevVertical1;
Expand All @@ -154,37 +58,44 @@ void lemlib::update() {

// calculate the heading of the robot
// Priority:
// 1. Inertial Sensor
// 2. Dual vertical tracking wheels
// 3. Dual horizontal tracking wheels
// 4. Single vertical tracking wheel w/ drivetrain
// 5. Single horizontal tracking wheel w/ drivetrain
// 6. Drivetrain

// 1. Horizontal tracking wheels
// 2. Vertical tracking wheels
// 3. Inertial Sensor
// 4. Drivetrain
float heading = odomPose.theta;
float heading = pose.theta;
// calculate the heading using the horizontal tracking wheels
if (odomSensors.horizontal1 != nullptr && odomSensors.horizontal2 != nullptr)
if (chassis->odomSensors.horizontal1 != nullptr && chassis->odomSensors.horizontal2 != nullptr)
heading += (deltaHorizontal1 - deltaHorizontal2) /
(odomSensors.horizontal1->getOffset() - odomSensors.horizontal2->getOffset());
(chassis->odomSensors.horizontal1->getOffset() - chassis->odomSensors.horizontal2->getOffset());
// else, if both vertical tracking wheels aren't substituted by the drivetrain, use the vertical tracking wheels
else if (!odomSensors.vertical1->getType() && !odomSensors.vertical2->getType())
else if (!chassis->odomSensors.vertical1->getType() && !chassis->odomSensors.vertical2->getType())
heading += (deltaVertical1 - deltaVertical2) /
(odomSensors.vertical1->getOffset() - odomSensors.vertical2->getOffset());
(chassis->odomSensors.vertical1->getOffset() - chassis->odomSensors.vertical2->getOffset());
// else, if the inertial sensor exists, use it
else if (odomSensors.imu != nullptr) heading += deltaImu;
else if (chassis->odomSensors.imu != nullptr) heading += deltaImu;
// else, use the the substituted tracking wheels
else
heading += (deltaVertical1 - deltaVertical2) /
(odomSensors.vertical1->getOffset() - odomSensors.vertical2->getOffset());
float deltaHeading = heading - odomPose.theta;
float avgHeading = odomPose.theta + deltaHeading / 2;
(chassis->odomSensors.vertical1->getOffset() - chassis->odomSensors.vertical2->getOffset());
float deltaHeading = heading - pose.theta;
float avgHeading = pose.theta + deltaHeading / 2;

// choose tracking wheels to use
// Prioritize non-powered tracking wheels
lemlib::TrackingWheel* verticalWheel = nullptr;
lemlib::TrackingWheel* horizontalWheel = nullptr;
if (!odomSensors.vertical1->getType()) verticalWheel = odomSensors.vertical1;
else if (!odomSensors.vertical2->getType()) verticalWheel = odomSensors.vertical2;
else verticalWheel = odomSensors.vertical1;
if (odomSensors.horizontal1 != nullptr) horizontalWheel = odomSensors.horizontal1;
else if (odomSensors.horizontal2 != nullptr) horizontalWheel = odomSensors.horizontal2;
if (!chassis->odomSensors.vertical1->getType()) verticalWheel = chassis->odomSensors.vertical1;
else if (!chassis->odomSensors.vertical2->getType()) verticalWheel = chassis->odomSensors.vertical2;
else verticalWheel = chassis->odomSensors.vertical1;
if (chassis->odomSensors.horizontal1 != nullptr) horizontalWheel = chassis->odomSensors.horizontal1;
else if (chassis->odomSensors.horizontal2 != nullptr) horizontalWheel = chassis->odomSensors.horizontal2;
float rawVertical = 0;
float rawHorizontal = 0;
if (verticalWheel != nullptr) rawVertical = verticalWheel->getDistanceTraveled();
Expand Down Expand Up @@ -213,38 +124,10 @@ void lemlib::update() {
localY = 2 * sin(deltaHeading / 2) * (deltaY / deltaHeading + verticalOffset);
}

// save previous pose
lemlib::Pose prevPose = odomPose;

// calculate global x and y
odomPose.x += localY * sin(avgHeading);
odomPose.y += localY * cos(avgHeading);
odomPose.x += localX * -cos(avgHeading);
odomPose.y += localX * sin(avgHeading);
odomPose.theta = heading;

// calculate speed
odomSpeed.x = ema((odomPose.x - prevPose.x) / 0.01, odomSpeed.x, 0.95);
odomSpeed.y = ema((odomPose.y - prevPose.y) / 0.01, odomSpeed.y, 0.95);
odomSpeed.theta = ema((odomPose.theta - prevPose.theta) / 0.01, odomSpeed.theta, 0.95);

// calculate local speed
odomLocalSpeed.x = ema(localX / 0.01, odomLocalSpeed.x, 0.95);
odomLocalSpeed.y = ema(localY / 0.01, odomLocalSpeed.y, 0.95);
odomLocalSpeed.theta = ema(deltaHeading / 0.01, odomLocalSpeed.theta, 0.95);
}

/**
* @brief Initialize the odometry system
*
*/
void lemlib::init() {
if (trackingTask == nullptr) {
trackingTask = new pros::Task {[=] {
while (true) {
update();
pros::delay(10);
}
}};
}
pose.x += localY * sin(avgHeading);
pose.y += localY * cos(avgHeading);
pose.x += localX * -cos(avgHeading);
pose.y += localX * sin(avgHeading);
pose.theta = heading;
}

0 comments on commit ef00cb0

Please sign in to comment.