Skip to content

Commit

Permalink
Basic line following
Browse files Browse the repository at this point in the history
  • Loading branch information
@dkt01
dkt01 committed Aug 29, 2023
1 parent 5dc3d47 commit 7aefe9a
Show file tree
Hide file tree
Showing 6 changed files with 93 additions and 17 deletions.
10 changes: 7 additions & 3 deletions src/PlatformApp/PlatformApp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ int main(int /*argc*/, char** /*argv*/) {
if (homingCalDebounce(controllerState.value().Buttons.A)) {
if (!calTrigger) {
calTrigger = true;
swervePlatform.Home(0_deg);
// swervePlatform.Home(0_deg);
}
controller.SetVibration(ArgosLib::VibrationConstant(0.5));
} else {
Expand All @@ -125,7 +125,8 @@ int main(int /*argc*/, char** /*argv*/) {
if (!driveMode) {
if (driveMapLon.map(controllerState.value().Axes.LeftY) == 0 &&
driveMapLat.map(controllerState.value().Axes.LeftX) == 0 &&
driveMapRot.map(controllerState.value().Axes.RightX) == 0) {
driveMapRot.map(controllerState.value().Axes.RightX) == 0 && !controllerState.value().Buttons.DUp &&
!controllerState.value().Buttons.DDown) {
// Vibration pulse to indicate drive mode activated
controller.SetVibration(0.3, 0.3, 500ms);
driveMode = true;
Expand All @@ -135,10 +136,13 @@ int main(int /*argc*/, char** /*argv*/) {
active = false;
}
}
if (active) {
if (active && !controllerState.value().Buttons.LB) {
swervePlatform.SwerveDrive(driveMapLon.map(controllerState.value().Axes.LeftY),
driveMapLat.map(controllerState.value().Axes.LeftX),
driveMapRot.map(controllerState.value().Axes.RightX));
} else if (active) {
swervePlatform.LineFollow(
controllerState.value().Buttons.DUp, controllerState.value().Buttons.DDown, lineSensor.GetArrayStatus());
} else {
swervePlatform.Stop();
}
Expand Down
14 changes: 7 additions & 7 deletions src/SerialLineSensor/SerialLineSensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,9 +52,9 @@ SerialLineSensor::~SerialLineSensor() {
if (!rawValues) {
return std::nullopt;
}
return SensorArrayStatus{.leftLineDetected = rawValues.value().left < m_calibrationThreshold,
.centerLineDetected = rawValues.value().center < m_calibrationThreshold,
.rightLineDetected = rawValues.value().right < m_calibrationThreshold};
return SensorArrayStatus{.leftLineDetected = rawValues.value().left < m_calibrationActivateThreshold,
.centerLineDetected = rawValues.value().center < m_calibrationActivateThreshold,
.rightLineDetected = rawValues.value().right < m_calibrationActivateThreshold};
}

[[nodiscard]] std::optional<RawSensorArrayStatus> SerialLineSensor::GetRawArrayStatus() const {
Expand Down Expand Up @@ -157,12 +157,12 @@ void SerialLineSensor::ReceiverThread() {
auto rawStates = ParseMessage(std::string_view(buf, nBytes));
if (rawStates) {
std::scoped_lock lock(m_dataMutex);
m_currentLeft = rawStates.value().left;
/// @todo fix left/right in arduino code or something...
m_currentLeft = rawStates.value().right;
m_currentCenter = rawStates.value().center;
m_currentRight = rawStates.value().right;
m_currentRight = rawStates.value().left;
m_lastUpdateTime = std::chrono::steady_clock::now();
std::cout << rawStates.value().left << ' ' << rawStates.value().center << ' ' << rawStates.value().right
<< '\n';
std::cout << m_currentLeft.value() << ' ' << m_currentCenter.value() << ' ' << m_currentRight.value() << '\n';
}
} else if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() -
m_lastUpdateTime) > m_timeout) {
Expand Down
2 changes: 1 addition & 1 deletion src/SerialLineSensor/SerialLineSensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class SerialLineSensor {
private:
std::string m_serialDeviceName;
int m_serialPort;
uint16_t m_calibrationThreshold;
uint16_t m_calibrationThreshold{1015};
std::optional<uint16_t> m_currentLeft{std::nullopt};
std::optional<uint16_t> m_currentCenter{std::nullopt};
std::optional<uint16_t> m_currentRight{std::nullopt};
Expand Down
1 change: 1 addition & 0 deletions src/SwervePlatform/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ add_library(${PROJECT_NAME} SwervePlatform.cpp)

target_link_libraries(${PROJECT_NAME} argosLib)
target_link_libraries(${PROJECT_NAME} ctre)
target_link_libraries(${PROJECT_NAME} SerialLineSensor)

target_include_directories(${PROJECT_NAME}
PUBLIC
Expand Down
67 changes: 63 additions & 4 deletions src/SwervePlatform/SwervePlatform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,11 @@

#include "argosLib/general/swerveUtils.h"

void SwervePlatform::SwerveDrive(const double fwVelocity, const double latVelocity, const double rotateVelocity) {
void SwervePlatform::SwerveDrive(const double fwVelocity,
const double latVelocity,
const double rotateVelocity,
const bool lineFollow,
frc::Translation2d offset) {
// Halt motion
if (fwVelocity == 0 && latVelocity == 0 && rotateVelocity == 0) {
m_motorDriveFrontLeft.Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, 0.0);
Expand All @@ -21,7 +25,11 @@ void SwervePlatform::SwerveDrive(const double fwVelocity, const double latVeloci
return;
}

auto moduleStates = RawModuleStates(fwVelocity, latVelocity, rotateVelocity);
if (!lineFollow) {
m_followDirection = LineFollowDirection::unknown;
}

auto moduleStates = RawModuleStates(fwVelocity, latVelocity, rotateVelocity, offset);
// printf("FR raw module velocity: %0.2fm/s\n", moduleStates.at(ModuleIndex::frontRight).speed.to<double>());

std::for_each(
Expand Down Expand Up @@ -85,6 +93,56 @@ void SwervePlatform::SwerveDrive(const double fwVelocity, const double latVeloci
measureUp::sensorConversion::swerveRotate::fromAngle(moduleStates.at(ModuleIndex::rearLeft).angle.Degrees()));
}

void SwervePlatform::LineFollow(bool forward, bool reverse, std::optional<SensorArrayStatus> arrayStatus) {
if (!arrayStatus || (!forward && !reverse) ||
(!arrayStatus.value().leftLineDetected && !arrayStatus.value().centerLineDetected &&
!arrayStatus.value().rightLineDetected)) {
Stop();
return;
}

auto desiredFollowDirection = forward ? LineFollowDirection::forward : LineFollowDirection::reverse;

const double forwardSpeed = desiredFollowDirection == LineFollowDirection::forward ? 0.5 : -0.5;

if (arrayStatus.value().leftLineDetected && arrayStatus.value().centerLineDetected &&
arrayStatus.value().rightLineDetected) {
if (desiredFollowDirection == m_followDirection) {
// Reached end of line, don't cross
Stop();
return;
} else {
// Leaving end line. Don't change stored direction because then the platform will stop next loop
SwerveDrive(forwardSpeed, 0, 0, true);
return;
}
}
m_followDirection = desiredFollowDirection;

frc::Translation2d offset{-2_m, 0_m};
if (desiredFollowDirection == LineFollowDirection::reverse) {
offset *= -1.0;
}

double leftTurnSpeed = 0;

if (arrayStatus.value().leftLineDetected) {
leftTurnSpeed = -0.1;
} else if (arrayStatus.value().rightLineDetected) {
leftTurnSpeed = 0.1;
}

if (arrayStatus.value().centerLineDetected) {
leftTurnSpeed *= 0.5;
}

if (desiredFollowDirection == LineFollowDirection::reverse) {
leftTurnSpeed *= -1.0;
}

SwerveDrive(forwardSpeed, 0, leftTurnSpeed, true, offset);
}

void SwervePlatform::Stop() {
for (const auto motor : {&m_motorDriveFrontLeft,
&m_motorDriveFrontRight,
Expand Down Expand Up @@ -166,7 +224,8 @@ double SwervePlatform::ModuleDriveSpeed(const units::velocity::feet_per_second_t

wpi::array<frc::SwerveModuleState, 4> SwervePlatform::RawModuleStates(const double fwVelocity,
const double latVelocity,
const double rotateVelocity) {
const double rotateVelocity,
frc::Translation2d offset) {
const auto desiredFwVelocity = m_maxVelocity * fwVelocity;
const auto desiredLatVelocity = m_maxVelocity * latVelocity;
const auto desiredRotVelocity = m_maxAngularRate * rotateVelocity;
Expand All @@ -180,7 +239,7 @@ wpi::array<frc::SwerveModuleState, 4> SwervePlatform::RawModuleStates(const doub
// }
case ControlMode::robotCentric:
return m_pSwerveKinematicsModel->ToSwerveModuleStates(
frc::ChassisSpeeds{desiredFwVelocity, desiredLatVelocity, desiredRotVelocity});
frc::ChassisSpeeds{desiredFwVelocity, desiredLatVelocity, desiredRotVelocity}, offset);
}
// This shouldn't be reachable (and there will be a compiler warning if a switch case is unhandled), but stop if in unknown drive state
return m_pSwerveKinematicsModel->ToSwerveModuleStates(frc::ChassisSpeeds{0_mps, 0_mps, 0_rpm});
Expand Down
16 changes: 14 additions & 2 deletions src/SwervePlatform/SwervePlatform.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,15 +11,18 @@
#include "ctre/Phoenix.h"
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/geometry/Translation2d.h>
#include <units/length.h>
#include <units/velocity.h>
#include <argosLib/general/swerveHomeStorage.h>
#include "SerialLineSensor.h"

using units::feet_per_second_t;

class SwervePlatform {
public:
enum ModuleIndex { frontLeft, frontRight, rearRight, rearLeft };
enum class LineFollowDirection { forward, reverse, unknown };

enum class ControlMode {
fieldCentric,
Expand Down Expand Up @@ -57,7 +60,12 @@ class SwervePlatform {
const auto& rearRightTurnEncoderConfig,
const auto& rearLeftTurnEncoderConfig);

void SwerveDrive(const double fwVelocity, const double latVelocity, const double rotateVelocity);
void SwerveDrive(const double fwVelocity,
const double latVelocity,
const double rotateVelocity,
const bool lineFollow = false,
frc::Translation2d offset = frc::Translation2d{});
void LineFollow(bool forward, bool reverse, std::optional<SensorArrayStatus> arrayStatus);
void Stop();

void Home(const units::degree_t currentAngle);
Expand All @@ -71,7 +79,10 @@ class SwervePlatform {
double ModuleDriveSpeed(const units::velocity::feet_per_second_t,
const units::velocity::feet_per_second_t,
const ctre::phoenix::motorcontrol::Faults);
wpi::array<frc::SwerveModuleState, 4> RawModuleStates(const double, const double, const double);
wpi::array<frc::SwerveModuleState, 4> RawModuleStates(const double,
const double,
const double,
frc::Translation2d offset = frc::Translation2d{});

TalonFX m_motorDriveFrontLeft;
TalonFX m_motorDriveFrontRight;
Expand All @@ -95,6 +106,7 @@ class SwervePlatform {
std::unique_ptr<ArgosLib::SwerveHomeStorageInterface> m_pHomingStorage;

ControlMode m_activeControlMode;
LineFollowDirection m_followDirection{LineFollowDirection::unknown};
};

namespace measureUp {
Expand Down

0 comments on commit 7aefe9a

Please sign in to comment.