manual movement

src/main/cpp/Auto.cpp

@@ -14,6 +14,9 @@
 #include "units/angle.h"
 #include "utils/Pathplanner.h"
 
+using namespace wom;
+using namespace behaviour;
+
 // Shoots starting note then moves out of starting position.
 wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
                                             Intake* _intake, AlphaArm* _alphaArm) {
@@ -40,6 +43,22 @@ std::shared_ptr<behaviour::Behaviour> autos::OneNoteTaxi(wom::SwerveAutoBuilder*
   return builder->GetAutoRoutine("OneNoteTaxi");
 }
 
+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));
+}
+
 // std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
 //                                                             Shooter* _shooter, Intake* _intake,
 //                                                             AlphaArm* _alphaArm) {

src/main/cpp/Robot.cpp

@@ -138,15 +138,17 @@ void Robot::AutonomousInit() {
 
   _swerveDrive->GetConfig().gyro->Reset();
 
-  m_autoSelected = m_chooser.GetSelected();
+  // m_autoSelected = m_chooser.GetSelected();
 
-  robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
+  // robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
 
-  if (m_autoSelected == "kTaxi") {
-   sched->Schedule(autos::Taxi(robotmap._builder));
-  }
+  // if (m_autoSelected == "kTaxi") {
+  //  sched->Schedule(autos::Taxi(robotmap._builder));
+  // }
   // sched->Schedule(robotmap._builder->GetAutoRoutine("OneNoteTaxi"));
 
+  sched->Schedule(autos::ManualTaxi(_swerveDrive, shooter, intake, alphaArm));
+
   _swerveDrive->OnStart();
 }
 

src/main/cpp/ShooterBehaviour.cpp

@@ -64,17 +64,17 @@ AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_se
 
 void AutoShooter::OnTick(units::second_t dt) {
   _goal = 300_rad_per_s;
-  _shooter->SetState(ShooterState::kSpinUp);
   _shooter->SetPidGoal(_goal);
+  _shooter->SetState(ShooterState::kSpinUp);
 
-  if (_timer.Get() > 3_s) {
-    _intake->SetState(IntakeState::kPass);
+  // if (_timer.Get() > 3_s) {
+  //   // _intake->SetState(IntakeState::kPass);
 
-    if (_timer.Get() > 5_s) {
-      _intake->SetState(IntakeState::kIdle);
-      _shooter->SetState(ShooterState::kIdle);
+  //   if (_timer.Get() > 5_s) {
+  //     _intake->SetState(IntakeState::kIdle);
+  //     _shooter->SetState(ShooterState::kIdle);
 
-      SetDone();
-    }
-  }
+  //     SetDone();
+  //   }
+  // }
 }

src/main/cpp/vision/Vision.cpp

@@ -340,6 +340,8 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
   std::cout << pose.Rotation().Degrees().value() << std::endl;
 
   swerveDrive->SetPose(pose);
+
+  return pose;
 }
 
 std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {

src/main/include/Auto.h

@@ -9,13 +9,15 @@
 #include "AlphaArmBehaviour.h"
 #include "IntakeBehaviour.h"
 #include "ShooterBehaviour.h"
+#include "drivetrain/behaviours/SwerveBehaviours.h"
 #include "Wombat.h"
 #include "utils/Pathplanner.h"
 
 namespace autos {
 
 // wom::Commands* _commands = nullptr;
 // wom::SwerveAutoBuilder* builder = nullptr;
+std::shared_ptr<behaviour::Behaviour> ManualTaxi(wom::drivetrain::SwerveDrive *_swerveDrive, Shooter *_shooter, Intake *_intake, AlphaArm *_arm);
 
 wom::utils::SwerveAutoBuilder* InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
                                             Intake* _intake, AlphaArm* _alphaArm);

wombat/src/main/cpp/drivetrain/SwerveDrive.cpp

@@ -303,7 +303,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
 
   driveVoltage = units::math::min(driveVoltage, 7_V);
-  turnVoltage = units::math::min(turnVoltage, 4_V);
+  turnVoltage = units::math::min(turnVoltage, 6_V);
 
   // driveVoltage = units::math::min(
   //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
@@ -429,8 +429,8 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
                                                    frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
       _anglePIDController{PIDController(0, 0, 0)},
-      _xPIDController(PIDController(4, 0.1, 0)),
-      _yPIDController(PIDController(4, 0.1, 0)),
+      _xPIDController(PIDController(6, 0.1, 0)),
+      _yPIDController(PIDController(6, 0.1, 0)),
       _turnPIDController(PIDController(1, 0, 0)),
       // _xPIDController(std::string path, config_t initialGains)
       // _xPIDController(config.path + "/pid/x", _config.posePositionPID),

wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp

@@ -275,3 +275,17 @@ void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t
     }
   }
 }
+
+
+wom::drivetrain::behaviours::TurnToAngleBeh::TurnToAngleBeh(SwerveDrive* swerveDrive, units::radian_t angle)
+    : _swerveDrive(swerveDrive) {
+  Controls(swerveDrive);
+}
+
+// used in autonomous for going to set drive poses
+void wom::drivetrain::behaviours::TurnToAngleBeh::OnTick(units::second_t deltaTime) {
+  _swerveDrive->TurnToAngle(0_deg);
+  // if (_swerveDrivebase->GetConfig().gyro->GetRotation2d().Radians().value() <= 0.1 && _swerveDrivebase->GetConfig().gyro->GetRotation2d().Radians().value() >= -0.1) {
+  //   SetDone();
+  // }
+}

wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h

@@ -235,6 +235,18 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour {
   std::shared_ptr<nt::NetworkTable> _swerveDriveTable =
       nt::NetworkTableInstance::GetDefault().GetTable("swerve");
 };
+
+class TurnToAngleBeh : public behaviour::Behaviour {
+ public:
+  TurnToAngleBeh(SwerveDrive* swerveDrive, units::radian_t angle);
+  void OnTick(units::second_t deltaTime) override;
+
+ private:
+  SwerveDrive* _swerveDrive;
+
+  std::shared_ptr<nt::NetworkTable> _swerveDriveBehTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+};
 }  // namespace behaviours
 }  // namespace drivetrain
 }  // namespace wom