Skip to content

Commit

Permalink
2 note auto
Browse files Browse the repository at this point in the history
  • Loading branch information
@goanna247
goanna247 committed Mar 8, 2024
1 parent c90d349 commit 32ae114
Show file tree
Hide file tree
Showing 11 changed files with 225 additions and 59 deletions.
10 changes: 5 additions & 5 deletions src/main/cpp/AlphaArm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,12 +44,12 @@ void AlphaArm::OnUpdate(units::second_t dt) {
break;

case AlphaArmState::kIntakeAngle:
std::cout << "Intake Angle" << std::endl;
_pidIntakeState.SetSetpoint(-0.50); //-0.48
// std::cout << "Intake Angle" << std::endl;
_pidIntakeState.SetSetpoint(-0.52); //-0.48
_setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
break;
case AlphaArmState::kIntakedAngle:
std::cout << "Intake Angle" << std::endl;
// std::cout << "Intake Angle" << std::endl;
_pidIntakeState.SetSetpoint(-0.55); //-0.48
_setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
break;
Expand Down Expand Up @@ -88,7 +88,7 @@ void AlphaArm::OnUpdate(units::second_t dt) {
}
_config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
_config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)) << 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() * (2 * 3.1415)));
Expand All @@ -101,7 +101,7 @@ void AlphaArm::OnUpdate(units::second_t dt) {



std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;
// std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;

}

Expand Down
73 changes: 61 additions & 12 deletions src/main/cpp/Auto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,18 +45,67 @@ std::shared_ptr<behaviour::Behaviour> autos::OneNoteTaxi(wom::SwerveAutoBuilder*

std::shared_ptr<behaviour::Behaviour> autos::ManualTaxi(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter *_shooter, Intake *_intake, AlphaArm *_arm) {
return
make<AutoShooter>(_shooter, _intake, 300_rad_per_s)->Until(make<WaitTime>(1_s))
<< make<PassNote>(_intake)->Until(make<WaitTime>(1_s))
<< make<TurnToAngleBeh>(_swerveDrive, 0_rad)->Until(make<WaitTime>(0.5_s))
<< make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{-0.01_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(0.2_s))
<< make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{-2_m, 0_m, 0_deg}, 6_V, true)->Until(make<WaitTime>(5_s))
<< make<TurnToAngleBeh>(_swerveDrive, 0_rad)->Until(make<WaitTime>(1_s))
<< make<IntakeNote>(_intake)->Until(make<WaitTime>(1_s))
<< make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0.01_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(0.2_s))
<< make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{1_m, 0_m, 0_deg}, 6_V, true)->Until(make<WaitTime>(5_s))
<< make<TurnToAngleBeh>(_swerveDrive, 0_rad)->Until(make<WaitTime>(1_s))
<< make<AutoShooter>(_shooter, _intake, 300_rad_per_s)->Until(make<WaitTime>(1_s))
<< make<PassNote>(_intake)->Until(make<WaitTime>(1_s));
//WORKING PATH
make<AutoShooter>(_shooter, _intake, 800_rad_per_s)->Until(make<WaitTime>(4.1_s))
<< make<PassNote>(_intake)->Until(make<WaitTime>(1_s))
<< make<ResetDrivebasePose>(_swerveDrive, true)
<< make<WaitTime>(0.1_s)
<< make<ResetDrivebasePose>(_swerveDrive, true)
<< make<IntakeNote>(_intake)->Until(make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{(_swerveDrive->GetPose().X() - 1.7_m), 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(1.5_s)))
<< make<WaitTime>(0.3_s)
<< make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(1.5_s))
<< make<AutoShooter>(_shooter, _intake, 800_rad_per_s)->Until(make<WaitTime>(4_s))
<< make<PassNote>(_intake)->Until(make<WaitTime>(3_s));
// << make<WaitTime>(1_s);

//TESTING
// make<ResetDrivebasePose>(_swerveDrive, true)
// make<TurnToAngleBeh>(_swerveDrive, 180_deg)->Until(make<WaitTime>(5_s));

// make<ResetDrivebasePose>(_swerveDrive, true)
// << make<WaitTime>(1_s)
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{-0.01_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(1_s))
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{-1_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(4_s))
// << make<WaitTime>(1_s)
// // << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0.01_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(1_s))
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{1_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(4_s));
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0.01_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(1_s))
// << make<WaitTime>(1_s)
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{1_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(4_s))
// << make<WaitTime>(1_s);

// << make<TurnToAngleBeh>(_swerveDrive, 0_rad)->Until(make<WaitTime>(0.2_s))
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0.01_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(0.2_s))
// << make<WaitTime>(1_s)
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{2_m, 0_m, 0_deg}, 6_V, true)->Until(make<WaitTime>(3_s))
// << make<WaitTime>(1_s)
// << make<TurnToAngleBeh>(_swerveDrive, 0_rad)->Until(make<WaitTime>(0.2_s));



// make<AutoShooter>(_shooter, _intake, 800_rad_per_s)->Until(make<WaitTime>(4.1_s))
// << make<PassNote>(_intake)->Until(make<WaitTime>(1_s))
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<TurnToAngleBeh>(_swerveDrive, 0_rad)->Until(make<WaitTime>(0.1_s))
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{-0.01_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(0.2_s))
// << make<IntakeNote>(_intake)->Until(make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{-2_m, 0_m, 0_deg}, 6_V, true))
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<TurnToAngleBeh>(_swerveDrive, 0_rad)->Until(make<WaitTime>(0.2_s))
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0.01_m, 0_m, 0_deg}, 3_V, true)->Until(make<WaitTime>(0.1_s))
// << make<DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{1.2_m, 0_m, 0_deg}, 6_V, true)->Until(make<WaitTime>(3_s))
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<TurnToAngleBeh>(_swerveDrive, 0_rad)->Until(make<WaitTime>(1_s))
// << make<AutoShooter>(_shooter, _intake, 800_rad_per_s)->Until(make<WaitTime>(1_s))
// << make<PassNote>(_intake)->Until(make<WaitTime>(1_s))
// << make<ResetDrivebasePose>(_swerveDrive, true)
// << make<TurnToAngleBeh>(_swerveDrive, 60_deg)->Until(make<WaitTime>(1_s));
}

// std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
Expand Down
3 changes: 3 additions & 0 deletions src/main/cpp/Intake.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,9 @@ void Intake::OnUpdate(units::second_t dt) {
_table->GetEntry("Shot Count").SetDouble(_noteShot);
// _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());

// if (_setVoltage < 4_V && _setVoltage > 0_V) {
// _setVoltage = 0_V;
// }
_config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
}

Expand Down
4 changes: 3 additions & 1 deletion src/main/cpp/IntakeBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,11 +73,13 @@ IntakeNote::IntakeNote(Intake* intake) : _intake(intake) {
}

void IntakeNote::OnTick(units::second_t dt) {
_intake->SetState(IntakeState::kIntake);

if (_intake->GetState() == IntakeState::kHold) {
std::cerr << "EXITING" << std::endl;
_intake->SetState(IntakeState::kIdle);
SetDone();
} else {
_intake->SetState(IntakeState::kIntake);
}
}

Expand Down
31 changes: 19 additions & 12 deletions src/main/cpp/ShooterBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,22 +59,29 @@ void ShooterManualControl::OnTick(units::second_t dt) {
AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal) : _shooter(shooter), _intake(intake), _goal(goal) {
Controls(shooter);

_timer.Start();
// _timer.Start();
}

void AutoShooter::OnTick(units::second_t dt) {
_goal = 300_rad_per_s;
_shooter->SetPidGoal(_goal);
_shooter->SetState(ShooterState::kSpinUp);
_intake->SetState(IntakeState::kIdle);
if (!hasShot) {
_timer.Start();
hasShot = true;
}


// if (_timer.Get() > 3_s) {
// // _intake->SetState(IntakeState::kPass);
if (_timer.Get() > 2_s) {
_shooter->SetPidGoal(_goal);
_shooter->SetState(ShooterState::kSpinUp);

// if (_timer.Get() > 5_s) {
// _intake->SetState(IntakeState::kIdle);
// _shooter->SetState(ShooterState::kIdle);
if (_timer.Get() > 4_s) {
// _intake->SetState(IntakeState::kIdle);
_shooter->SetState(ShooterState::kIdle);

// SetDone();
// }
// }
_timer.Stop();
_timer.Reset();
hasShot = false;
SetDone();
}
}
}
4 changes: 2 additions & 2 deletions src/main/include/RobotMap.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,8 @@ struct RobotMap {
struct IntakeSystem {
rev::CANSparkMax intakeMotor{35, rev::CANSparkMax::MotorType::kBrushless};
wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
frc::DigitalInput intakeSensor{5};
frc::DigitalInput passSensor{7};
frc::DigitalInput intakeSensor{7};
frc::DigitalInput passSensor{5};
// frc::DigitalInput magSensor{0};
// frc::DigitalInput shooterSensor{0};

Expand Down
1 change: 1 addition & 0 deletions src/main/include/ShooterBehaviour.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,5 +43,6 @@ class AutoShooter : public behaviour::Behaviour {
Intake* _intake;
units::radians_per_second_t _goal;

bool hasShot = false;
frc::Timer _timer;
};
54 changes: 45 additions & 9 deletions wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -302,23 +302,30 @@ void SwerveModule::OnUpdate(units::second_t dt) {
// driveVoltage =
// units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);

driveVoltage = units::math::min(driveVoltage, 7_V);
driveVoltage = units::math::min(driveVoltage, 10_V);
turnVoltage = units::math::min(turnVoltage, 6_V);

// driveVoltage = units::math::min(
// units::math::max(driveVoltage, -_driveModuleVoltageLimit),
// _driveModuleVoltageLimit); // was originally 10_V
units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
// units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
// turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
// turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);

_table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value());
_table->GetEntry("TurnSetpoint").SetDouble(_anglePIDController.GetSetpoint());
_table->GetEntry("Demand").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
_table->GetEntry("Error").SetDouble(_anglePIDController.GetPositionError());

_config.driveMotor.motorController->SetVoltage(driveVoltage);
_config.turnMotor.motorController->SetVoltage(turnVoltage);
if (_slowSpeed) {
_config.driveMotor.motorController->SetVoltage(driveVoltage/2);
_config.turnMotor.motorController->SetVoltage(turnVoltage);
} else {
_config.driveMotor.motorController->SetVoltage(driveVoltage);
_config.turnMotor.motorController->SetVoltage(turnVoltage);
}



_table->GetEntry("speed").SetDouble(GetSpeed().value());
_table->GetEntry("angle").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
Expand Down Expand Up @@ -380,6 +387,14 @@ void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t spee
// }
}

void SwerveModule::reduceSpeed() {
_slowSpeed = true;
}

void SwerveModule::normalSpeed() {
_slowSpeed = false;
}

void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
units::meters_per_second_t xVelocityComponent =
1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
Expand Down Expand Up @@ -512,7 +527,8 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
// _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
// _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
_target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().X().value())};
_target_fr_speeds.vy = units::meters_per_second_t{_yPIDController.Calculate(GetPose().Y().value())};
_target_fr_speeds.vy = 0_mps;
// _target_fr_speeds.vy = units::meters_per_second_t{_yPIDController.Calculate(GetPose().Y().value())};
_target_fr_speeds.omega = 0_rad_per_s;
// units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value() - (3.14159))};

Expand Down Expand Up @@ -555,8 +571,18 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
for (size_t i = 0; i < _modules.size(); i++) {
if (i == 0 || i == 2 || i == 1) {
_modules[i].SetPID(new_target_states[i].angle.Radians(), new_target_states[i].speed, dt);
if (_inPose) {
_modules[i].reduceSpeed();
} else {
_modules[i].normalSpeed();
}
} else {
_modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
if (_inPose) {
_modules[i].reduceSpeed();
} else {
_modules[i].normalSpeed();
}
}
}
} break;
Expand Down Expand Up @@ -622,6 +648,7 @@ void SwerveDrive::SetVoltageLimit(units::volt_t driveVoltageLimit) {
// return _modules[mod].GetCancoderPosition();
// }


void SwerveDrive::OnStart() {
OnResetMode();

Expand Down Expand Up @@ -652,10 +679,12 @@ void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelati

void SwerveDrive::SetIdle() {
_state = SwerveDriveState::kIdle;
_inPose = false;
}

void SwerveDrive::SetVelocity(frc::ChassisSpeeds speeds) {
_state = SwerveDriveState::kVelocity;
_inPose = false;
_target_speed = speeds;
}

Expand All @@ -681,11 +710,13 @@ void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t sp

void SwerveDrive::SetFieldRelativeVelocity(FieldRelativeSpeeds speeds) {
_state = SwerveDriveState::kFieldRelativeVelocity;
_inPose = false;
isRotateToMatchJoystick = false;
_target_fr_speeds = speeds;
}

void SwerveDrive::SetPose(frc::Pose2d pose) {
_inPose = true;
_state = SwerveDriveState::kPose;
_anglePIDController.SetSetpoint(pose.Rotation().Radians().value() - 3.14159);
_xPIDController.SetSetpoint(pose.X().value());
Expand Down Expand Up @@ -715,11 +746,16 @@ SwerveDriveState SwerveDrive::GetState() {
}

void SwerveDrive::ResetPose(frc::Pose2d pose) {
// _poseEstimator.ResetPosition(
// _config.gyro->GetRotation2d() - 1_rad,
// wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
// _modules[2].GetPosition(), _modules[3].GetPosition()},
// pose);
_poseEstimator.ResetPosition(
_config.gyro->GetRotation2d() - 1_rad,
wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
_modules[2].GetPosition(), _modules[3].GetPosition()},
pose);
wpi::array<frc::SwerveModulePosition, 4>{_modules[3].GetPosition(), _modules[0].GetPosition(),
_modules[1].GetPosition(), _modules[2].GetPosition()},
frc::Pose2d(0_m, 0_m, 0_deg));
}

frc::Pose2d SwerveDrive::GetPose() {
Expand Down
Loading

0 comments on commit 32ae114

Please sign in to comment.