Skip to content

Commit

Permalink
arm and swerve
Browse files Browse the repository at this point in the history
  • Loading branch information
@goanna247
goanna247 committed Mar 1, 2024
1 parent af62b5d commit 06befea
Show file tree
Hide file tree
Showing 20 changed files with 273 additions and 295 deletions.
14 changes: 7 additions & 7 deletions src/main/cpp/AlphaArm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,27 +32,27 @@ void AlphaArm::OnUpdate(units::second_t dt) {
{
_pidArm.SetSetpoint(-_goal);
//_setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
}
break;
case AlphaArmState::kAmpAngle:
std::cout << "Amp Angle" << std::endl;

_pidArmStates.SetSetpoint(-2.17);
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)) )};

break;

case AlphaArmState::kIntakeAngle:
std::cout << "Intake Angle" << std::endl;
_pidIntakeState.SetSetpoint(-0.48); //-0.48
_setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
_setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
break;

case AlphaArmState::kSpeakerAngle:
std::cout << "Speaker Angle" << std::endl;
_pidArmStates.SetSetpoint(-0.82);
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};

break;

Expand All @@ -64,18 +64,18 @@ void AlphaArm::OnUpdate(units::second_t dt) {
case AlphaArmState::kStowed:
std::cout << "Stowed" << std::endl;
_pidArmStates.SetSetpoint(-0.52);
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
_setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
default:
std::cout << "Error: alphaArm in INVALID STATE" << std::endl;
break;

}
_config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
_config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
std::cout << "Encoder Value: " << _config->alphaArmEncoder.GetEncoderPosition().value() << std::endl;
std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)) << std::endl;
_table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
_table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
_table->GetEntry("Input").SetDouble(_config->alphaArmEncoder.GetEncoderPosition().value());
_table->GetEntry("Input").SetDouble((_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)));

_table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError());
_table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint());
Expand Down
29 changes: 20 additions & 9 deletions src/main/cpp/AlphaArmBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,18 +29,29 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
}
}

if(_codriver->GetLeftTriggerAxis() > 0.1){
_alphaArm->SetState(AlphaArmState::kSpeakerAngle);
} else if (_codriver->GetLeftBumper()){
_alphaArm->SetState(AlphaArmState::kAmpAngle);
} else if(_codriver->GetYButton()){
_alphaArm->SetState(AlphaArmState::kStowed);
} else if(_codriver->GetRightBumper()){
_alphaArm->SetState(AlphaArmState::kIntakeAngle);
if (_rawControl) {
_alphaArm->SetState(AlphaArmState::kRaw);
if (wom::deadzone(_codriver->GetRightY())) {
_alphaArm->SetArmRaw(_codriver->GetRightY() * 8_V);
} else {
_alphaArm->SetArmRaw(0_V);
}
} else {
_alphaArm->SetState(AlphaArmState::kIdle);
if(_codriver->GetLeftTriggerAxis() > 0.1){
_alphaArm->SetState(AlphaArmState::kSpeakerAngle);
} else if (_codriver->GetLeftBumper()){
_alphaArm->SetState(AlphaArmState::kAmpAngle);
} else if(_codriver->GetYButton()){
_alphaArm->SetState(AlphaArmState::kStowed);
} else if(_codriver->GetRightBumper()){
_alphaArm->SetState(AlphaArmState::kIntakeAngle);
} else {
_alphaArm->SetState(AlphaArmState::kIdle);
}
}

}




33 changes: 9 additions & 24 deletions src/main/cpp/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ void Robot::RobotInit() {

// m_chooser.SetDefaultOption("Default Auto", "Default Auto");

// frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
sched = wom::BehaviourScheduler::GetInstance();
m_chooser.SetDefaultOption("Default Auto", "Default Auto");

Expand All @@ -55,24 +55,10 @@ void Robot::RobotInit() {
// robotmap.swerveBase.gyro->Reset();


// _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
// wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
// _swerveDrive->SetDefaultBehaviour(
// [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });


_swerveDrive =
new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
_swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
_swerveDrive->SetDefaultBehaviour([this]() {
return wom::make<wom::ManualDrivebase>(_swerveDrive,
&robotmap.controllers.driver);
});


// m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
// m_driveSim = wom::TempSimSwerveDrive();

_swerveDrive->SetDefaultBehaviour(
[this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });

alphaArm = new AlphaArm(&robotmap.alphaArmSystem.config);
wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
Expand Down Expand Up @@ -146,14 +132,13 @@ void Robot::RobotPeriodic() {
// _swerveDrive->OnUpdate(dt);


// 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());
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);

_swerveDrive->OnUpdate(dt);
}

void Robot::AutonomousInit() {
Expand Down
6 changes: 3 additions & 3 deletions src/main/include/Robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,15 +42,15 @@ class Robot : public frc::TimedRobot {
//Shooter* shooter;

// Intake* intake;
// frc::SendableChooser<std::string> m_chooser;
frc::SendableChooser<std::string> m_chooser;

// frc::Field2d m_field;

// frc::Timer simulation_timer;

// frc::SendableChooser<std::string> m_path_chooser;
frc::SendableChooser<std::string> m_path_chooser;

//wom::SwerveDrive* _swerveDrive;
wom::SwerveDrive* _swerveDrive;

// rev::CANSparkMax testMotorUp{1, rev::CANSparkMax::MotorType::kBrushless};
// rev::CANSparkMax testMotorDown{6, rev::CANSparkMax::MotorType::kBrushless};
Expand Down
111 changes: 1 addition & 110 deletions src/main/include/RobotMap.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,116 +38,6 @@ struct RobotMap {
frc::XboxController testController = frc::XboxController(2);
};
Controllers controllers;

// struct SwerveBase {
// ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19, "Drivebase"};
// ctre::phoenix6::hardware::CANcoder frontRightCancoder{17, "Driverbase"};
// ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
// ctre::phoenix6::hardware::CANcoder backRightCancoder{18, "Drivebase"};

// ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
// wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
// new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),
// new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),
// new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),
// new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};
// wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
// new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),
// new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),
// new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),
// new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};

// wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
// wom::SwerveModuleConfig{
// // front left module
// frc::Translation2d(10.761_in, 9.455_in),
// wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
// frc::DCMotor::Falcon500(1).WithReduction(6.75)},
// wom::Gearbox{turnMotors[0], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
// frc::DCMotor::Falcon500(1).WithReduction(12.8)},
// &frontLeftCancoder, 4_in / 2},
// wom::SwerveModuleConfig{
// // front right module
// frc::Translation2d(10.761_in, -9.455_in),
// wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
// frc::DCMotor::Falcon500(1).WithReduction(6.75)},
// wom::Gearbox{turnMotors[1], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
// frc::DCMotor::Falcon500(1).WithReduction(12.8)},
// &frontRightCancoder, 4_in / 2},
// wom::SwerveModuleConfig{
// // back left module
// frc::Translation2d(-10.761_in, 9.455_in),
// wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
// frc::DCMotor::Falcon500(1).WithReduction(6.75)},
// wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
// frc::DCMotor::Falcon500(1).WithReduction(12.8)},
// &backRightCancoder, 4_in / 2},
// wom::SwerveModuleConfig{
// // back right module
// frc::Translation2d(-10.761_in, -9.455_in),
// wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
// frc::DCMotor::Falcon500(1).WithReduction(6.75)},
// wom::Gearbox{turnMotors[3], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
// frc::DCMotor::Falcon500(1).WithReduction(12.8)},
// &backLeftCancoder, 4_in / 2},
// };

// // Setting the PID path and values to be used for SwerveDrive and
// // SwerveModules
// wom::SwerveModule::angle_pid_conf_t anglePID{
// "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s),
// 0_V / (100_deg / 1_s), 1_deg, 0.5_deg / 2_s};
// wom::SwerveModule::velocity_pid_conf_t velocityPID{
// "/drivetrain/pid/velocity/config",
// // 12_V / 4_mps // webers per metre
// };
// wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
// "/drivetrain/pid/pose/angle/config",
// 180_deg / 1_s / 45_deg,
// wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0.1},
// 0_deg / 1_deg,
// 10_deg,
// 10_deg / 1_s};
// wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
// "/drivetrain/pid/pose/position/config",
// 3_mps / 1_m,
// wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
// 0_m / 1_m,
// 20_cm,
// 10_cm / 1_s,
// 10_cm};

// // the config for the whole swerve drive
// wom::SwerveDriveConfig config{"/drivetrain",
// anglePID,
// velocityPID,
// moduleConfigs, // each module
// gyro,
// poseAnglePID,
// posePositionPID,
// 60_kg, // robot mass (estimate rn)
// {0.1, 0.1, 0.1},
// {0.9, 0.9, 0.9}};

// // current limiting and setting idle mode of modules to brake mode
// // SwerveBase() {
// // for (size_t i = 0; i < 4; i++) {
// // turnMotors[i]->ConfigSupplyCurrentLimit(
// // SupplyCurrentLimitConfiguration(true, 15, 15, 0));
// // driveMotors[i]->SetNeutralMode(NeutralMode::Brake);
// // turnMotors[i]->SetNeutralMode(NeutralMode::Brake);
// // driveMotors[i]->SetInverted(true);
// // }
// //}
// };
// SwerveBase swerveBase;


// struct SwerveTable {
// std::shared_ptr<nt::NetworkTable> swerveDriveTable =
// nt::NetworkTableInstance::GetDefault().GetTable("swerve");
// };
// SwerveTable swerveTable;

struct AlphaArmSystem {
rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless};
Expand All @@ -168,6 +58,7 @@ struct RobotMap {
AlphaArmSystem alphaArmSystem;



struct SwerveBase {
ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
Expand Down
23 changes: 15 additions & 8 deletions wombat/src/main/cpp/behaviour/Behaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,9 @@ using namespace behaviour;

// Behaviour
Behaviour::Behaviour(std::string name, units::time::second_t period)
: _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
: _bhvr_name(name),
_bhvr_period(period),
_bhvr_state(BehaviourState::INITIALISED) {}
Behaviour::~Behaviour() {
if (!IsFinished())
Interrupt();
Expand Down Expand Up @@ -80,7 +82,8 @@ bool Behaviour::Tick() {

if (dt > 2 * _bhvr_period) {
std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
<< " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
<< " Dt(deadline)=" << (2 * _bhvr_period).value()
<< ". Bhvr: " << GetName() << std::endl;
}

if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
Expand All @@ -97,7 +100,8 @@ bool Behaviour::IsRunning() const {
}

bool Behaviour::IsFinished() const {
return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
return _bhvr_state != BehaviourState::INITIALISED &&
_bhvr_state != BehaviourState::RUNNING;
}

void Behaviour::Stop(BehaviourState new_state) {
Expand Down Expand Up @@ -169,7 +173,8 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
}

std::string ConcurrentBehaviour::GetName() const {
std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
std::string msg =
(_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
for (auto b : _children)
msg += b->GetName() + ", ";
msg += "}";
Expand All @@ -185,8 +190,8 @@ void ConcurrentBehaviour::OnStart() {
using namespace std::chrono_literals;

b->Tick();
std::this_thread::sleep_for(
std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
std::this_thread::sleep_for(std::chrono::milliseconds(
static_cast<int64_t>(b->GetPeriod().value() * 1000)));
}

if (IsFinished() && !b->IsFinished())
Expand Down Expand Up @@ -268,8 +273,10 @@ void WaitFor::OnTick(units::time::second_t dt) {
}

// WaitTime
WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
WaitTime::WaitTime(units::time::second_t time)
: WaitTime([time]() { return time; }) {}
WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
: _time_fn(time_fn) {}

void WaitTime::OnStart() {
_time = _time_fn();
Expand Down
4 changes: 2 additions & 2 deletions wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
std::lock_guard<std::recursive_mutex> lk(_active_mtx);
behaviour->Tick();
}
std::this_thread::sleep_for(
std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
std::this_thread::sleep_for(std::chrono::milliseconds(
static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
}
});
}
Expand Down
3 changes: 2 additions & 1 deletion wombat/src/main/cpp/behaviour/HasBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,8 @@

using namespace behaviour;

void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
void HasBehaviour::SetDefaultBehaviour(
std::function<std::shared_ptr<Behaviour>(void)> fn) {
_default_behaviour_producer = fn;
}

Expand Down
Loading

0 comments on commit 06befea

Please sign in to comment.