Skip to content

Commit

Permalink
robot running well, field oriented, turning pretty good needs work in…
Browse files Browse the repository at this point in the history 
… 1 direction i think it's fighting a PID loop, i put acceleration and speed limit all the way up be careful, also needs LOTS more current limiting
  • Loading branch information
@goanna247
goanna247 committed Jan 28, 2024
1 parent 091b8d9 commit d7bfbe0
Show file tree
Hide file tree
Showing 7 changed files with 86 additions and 40 deletions.
19 changes: 11 additions & 8 deletions src/main/cpp/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,10 +55,13 @@ void Robot::RobotInit() {
// m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
// m_driveSim = wom::TempSimSwerveDrive();

robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.640_rad);
robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.685_rad);
robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(2.871_rad);
robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(1.715_rad);
robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.5826_rad);
robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.6539_rad);
robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(2.8057_rad);
robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(-1.74018_rad);

robotmap.swerveBase.moduleConfigs[0].driveMotor.motorController->SetInverted(true);
robotmap.swerveBase.moduleConfigs[1].driveMotor.motorController->SetInverted(true);

// robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
// robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
Expand All @@ -83,10 +86,10 @@ void Robot::RobotPeriodic() {
loop.Poll();
sched->Tick();

robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());

alphaArm->OnUpdate(dt);
_swerveDrive->OnUpdate(dt);
Expand Down
59 changes: 45 additions & 14 deletions wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -267,7 +267,8 @@ SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
_velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
_table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {
// _anglePIDController.SetTolerance(360);
_anglePIDController.EnableContinuousInput(-3.1415, (2 * 3.1415));
// _anglePIDController.EnableContinuousInput(-3.1415, (2 * 3.1415));
_anglePIDController.EnableContinuousInput(-3.1415, (3.1415));
}

void SwerveModule::OnStart() {
Expand Down Expand Up @@ -297,7 +298,15 @@ void SwerveModule::OnUpdate(units::second_t dt) {
double input = _config.turnMotor.encoder->GetEncoderPosition().value();
_table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
// _velocityPIDController.SetSetpoint(3);


driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
if (_turnOffset == TurnOffsetValues::forward) {
input = input + (3.1415/2);
} else if (_turnOffset == TurnOffsetValues::reverse) {
input = input - (3.1415/2);
driveVoltage = -driveVoltage;
}
double demand = _anglePIDController.Calculate(input);
// if ((_anglePIDController.GetSetpoint() - input) > 180) {
// if (demand > 0) {
Expand Down Expand Up @@ -346,13 +355,22 @@ void SwerveModule::OnUpdate(units::second_t dt) {
_config.turnMotor.motorController->SetVoltage(turnVoltage);

_table->GetEntry("speed").SetDouble(GetSpeed().value());
_table->GetEntry("angle").SetDouble(
_config.turnMotor.encoder->GetEncoderPosition()
.convert<units::degree>()
.value());
_table->GetEntry("angle").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
_config.WriteNT(_table->GetSubTable("config"));
}

void SwerveModule::SetTurnOffsetForward() {
_turnOffset = TurnOffsetValues::forward;
}

void SwerveModule::SetTurnOffsetReverse() {
_turnOffset = TurnOffsetValues::reverse;
}

void SwerveModule::TurnOffset() {
_turnOffset = TurnOffsetValues::none;
}

// double SwerveModule::GetCancoderPosition() {
// return (_config.turnMotor.encoder->GetEncoderPosition().value());
// }
Expand Down Expand Up @@ -388,17 +406,17 @@ void SwerveModule::SetPID(units::radian_t angle,
_state = SwerveModuleState::kPID;


double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value());
_table->GetEntry("diff").SetDouble(diff);
if (std::abs(diff) > (3.14159/2)) {
speed *= -1;
angle = 3.14159_rad - units::radian_t{diff};
_anglePIDController.SetSetpoint(angle.value());
_velocityPIDController.SetSetpoint(speed.value());
} else {
// double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value());
// _table->GetEntry("diff").SetDouble(diff);
// if (std::abs(diff) > (3.14159/2)) {
// speed *= -1;
// angle = 3.14159_rad - units::radian_t{diff};
// _anglePIDController.SetSetpoint(angle.value());
// _velocityPIDController.SetSetpoint(speed.value());
// } else {
_anglePIDController.SetSetpoint(angle.value());
_velocityPIDController.SetSetpoint(speed.value());
}
// }
}

void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
Expand Down Expand Up @@ -509,6 +527,19 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
// _target_fr_speeds.vy.value() << std::endl;
[[fallthrough]];
case SwerveDriveState::kVelocity: {
_table->GetEntry("Swerve module VX").SetDouble(_target_speed.vx.value());
_table->GetEntry("Swerve module VY").SetDouble(_target_speed.vy.value());
_table->GetEntry("Swerve module Omega").SetDouble(_target_speed.omega.value());
if (_target_speed.omega.value() > 0) {
_modules[0].SetTurnOffsetForward();
_modules[1].SetTurnOffsetForward();
} else if (_target_speed.omega.value() < 0) {
_modules[0].SetTurnOffsetReverse();
_modules[1].SetTurnOffsetReverse();
} else {
_modules[0].TurnOffset();
_modules[1].TurnOffset();
}
auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
for (size_t i = 0; i < _modules.size(); i++) {
_modules[i].SetPID(target_states[i].angle.Radians(),
Expand Down
17 changes: 11 additions & 6 deletions wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ ManualDrivebase::ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,

void ManualDrivebase::OnStart() {
_swerveDrivebase->OnStart();
_swerveDrivebase->SetAccelerationLimit(6_mps_sq);
_swerveDrivebase->SetAccelerationLimit(10_mps_sq);
}

void ManualDrivebase::OnTick(units::second_t deltaTime) {
Expand All @@ -37,9 +37,9 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
// isRotateMatch = !isRotateMatch;
// }

// if (_driverController->GetYButton()) {
// _swerveDrivebase->ResetPose(frc::Pose2d());
// }
if (_driverController->GetYButton()) {
_swerveDrivebase->ResetPose(frc::Pose2d());
}

// if (_driverController->GetLeftBumperPressed()) {
// maxMovementMagnitude = lowSensitivityDriveSpeed;
Expand Down Expand Up @@ -103,9 +103,14 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
// r_x * maxRotationMagnitude});
// } else {
_swerveDrivebase->SetFieldRelativeVelocity(
wom::drivetrain::FieldRelativeSpeeds{xVelocity * maxMovementMagnitude,
yVelocity * maxMovementMagnitude,
wom::drivetrain::FieldRelativeSpeeds{xVelocity * -maxMovementMagnitude,
yVelocity * -maxMovementMagnitude,
r_x * maxRotationMagnitude});

// _swerveDrivebase->SetVelocity(
// frc::ChassisSpeeds{xVelocity * maxMovementMagnitude,
// yVelocity * maxMovementMagnitude,
// r_x * maxRotationMagnitude});
// }
// }
// _swerveDrivebase->SetTuning(100_deg, 0_mps);
Expand Down
12 changes: 6 additions & 6 deletions wombat/src/main/cpp/utils/Encoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,18 +22,18 @@ double wom::utils::Encoder::GetEncoderTicksPerRotation() const {
}

void wom::utils::Encoder::ZeroEncoder() {
_offset = GetEncoderRawTicks() * 1_rad;
// _offtt = GetEncoderRawTicks() * 1_rad;
}

void wom::utils::Encoder::SetEncoderPosition(units::degree_t position) {
// units::radian_t offset_turns = position - GetEncoderPosition();
units::degree_t offset = position - (GetEncoderRawTicks() * 360 * 1_deg);
_offset = offset;
// units::degree_t offset = position - (GetEncoderRawTicks() * 360 * 1_deg);
// _offset = offset;
// _offset = -offset_turns;
}

void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) {
_offset = offset;
void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { //HERE!
_offset = offset;
// units::turn_t offset_turns = offset;
// _offset = offset_turns.value() * GetEncoderTicksPerRotation();
}
Expand Down Expand Up @@ -158,7 +158,7 @@ wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
}

double wom::utils::CanEncoder::GetEncoderRawTicks() const {
return _canEncoder->GetAbsolutePosition().GetValue().value() * 2 * 3.14;
return (_canEncoder->GetAbsolutePosition().GetValue().value() * 2 * 3.14) - _offset.value();
}

double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
Expand Down
15 changes: 11 additions & 4 deletions wombat/src/main/include/drivetrain/SwerveDrive.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -306,6 +306,7 @@ class PIDController


enum class SwerveModuleState { kZeroing, kIdle, kPID };
enum class TurnOffsetValues { reverse, forward, none };

struct SwerveModuleConfig {
frc::Translation2d position;
Expand Down Expand Up @@ -346,6 +347,10 @@ class SwerveModule {
void SetZero();
void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);

void SetTurnOffsetForward();
void SetTurnOffsetReverse();
void TurnOffset();

// double GetCancoderPosition(); // from liam's

void SetAccelerationLimit(units::meters_per_second_squared_t limit);
Expand Down Expand Up @@ -377,6 +382,8 @@ class SwerveModule {

double _offset;
units::meters_per_second_squared_t _currentAccelerationLimit = 6_mps / 1_s;

TurnOffsetValues _turnOffset = TurnOffsetValues::none;
};

struct SwerveDriveConfig {
Expand Down Expand Up @@ -511,10 +518,10 @@ class SwerveDrive : public behaviour::HasBehaviour {
units::radian_t _angle;
units::meters_per_second_t _speed;

double frontLeftEncoderOffset = -143.26171875;
double frontRightEncoderOffset = 167.87109375;
double backLeftEncoderOffset = -316.669921875;
double backRightEncoderOffset = -119.619140625;
// double frontLeftEncoderOffset = -143.26171875;
// double frontRightEncoderOffset = 167.87109375;
// double backLeftEncoderOffset = -316.669921875;
// double backRightEncoderOffset = -119.619140625;
};
} // namespace drivetrain
} // namespace wom
2 changes: 1 addition & 1 deletion wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ class ManualDrivebase : public behaviour::Behaviour {
// The translation speeds for when "slow speed", "normal speed", "fast speed"
// modes are active
const translationSpeed_ lowSensitivityDriveSpeed = 3.25_ft / 1_s;
const translationSpeed_ defaultDriveSpeed = 13_ft / 1_s;
const translationSpeed_ defaultDriveSpeed = 27_ft / 1_s;
const translationSpeed_ highSensitivityDriveSpeed = 18_ft / 1_s;
// The rotation speeds for when "slow speed", "normal speed", "fast speed"
// modes are active
Expand Down
2 changes: 1 addition & 1 deletion wombat/src/main/include/utils/Encoder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,9 +45,9 @@ class Encoder {
virtual double GetVelocity() const = 0;
double GetVelocityValue() const;

units::radian_t _offset = 0_rad; //bad
private:
double _encoderTicksPerRotation;
units::radian_t _offset = 0_rad;
int _type = 0;
units::meter_t _wheelRadius;
};
Expand Down

0 comments on commit d7bfbe0

Please sign in to comment.