Merge pull request #155 from met4000/v_drive

Add velocity stuffs, and some other fixes
CurtinFRC · Feb 19, 2019 · 943bbf2 · 943bbf2
2 parents de57135 + 4efb8ee
commit 943bbf2
Show file tree

Hide file tree

Showing 11 changed files with 51 additions and 16 deletions.
diff --git a/5333/src/main/cpp/Robot5333.cpp b/5333/src/main/cpp/Robot5333.cpp
@@ -33,7 +33,7 @@ void Robot::RobotInit() {
   robotmap.drivetrain.leftGearbox.encoder->ZeroEncoder();
   robotmap.drivetrain.rightGearbox.encoder->ZeroEncoder();
 
-  drivetrain = new Drivetrain(robotmap.drivetrain.config);
+  drivetrain = new Drivetrain(robotmap.drivetrain.config, robotmap.drivetrain.gainsVelocity);
   drivetrain->SetDefault(std::make_shared<DrivetrainManualStrategy>(*drivetrain, robotmap.contGroup));
   drivetrain->StartLoop(100);
   stratFOC = std::make_shared<DrivetrainFOCStrategy>(*drivetrain, robotmap.contGroup, robotmap.drivetrain.gainsFOC);

diff --git a/5333/src/main/include/BoxIntake.h b/5333/src/main/include/BoxIntake.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "strategy/Strategy.h"
+#include "strategy/StrategySystem.h"
 #include "devices/DeployableDevice.h"
 #include "CurtinControllers.h"
 #include "Toggle.h"
@@ -14,7 +15,7 @@ struct BoxIntakeConfig : public curtinfrc::devices::DeployableDeviceConfig {
   BoxIntakeConfig(curtinfrc::Gearbox &motorsIn, curtinfrc::actuators::BinaryActuator &actuatorIn, bool canEjectIn = true) : curtinfrc::devices::DeployableDeviceConfig(actuatorIn, canEjectIn), motors(motorsIn) {};
 };
 
-class BoxIntake : public curtinfrc::devices::DeployableDevice {
+class BoxIntake : public curtinfrc::devices::DeployableDevice, public curtinfrc::StrategySystem {
  public:
   BoxIntake(BoxIntakeConfig config) : DeployableDevice(config), _config(config) {};
 

diff --git a/5333/src/main/include/HatchIntake.h b/5333/src/main/include/HatchIntake.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "strategy/Strategy.h"
+#include "strategy/StrategySystem.h"
 #include "devices/DeployableDevice.h"
 #include "actuators/BinaryActuator.h"
 #include "CurtinControllers.h"
@@ -15,7 +16,7 @@ struct HatchIntakeConfig : public curtinfrc::devices::DeployableDeviceConfig {
   HatchIntakeConfig(curtinfrc::actuators::BinaryActuator &manipulatorIn, curtinfrc::actuators::BinaryActuator &actuatorIn, curtinfrc::actuators::BinaryActuatorState stowedStateIn = curtinfrc::actuators::kForward, bool canEjectIn = false) : curtinfrc::devices::DeployableDeviceConfig(actuatorIn, canEjectIn), stowedState(stowedStateIn), manipulator(manipulatorIn) {};
 };
 
-class HatchIntake : public curtinfrc::devices::DeployableDevice {
+class HatchIntake : public curtinfrc::devices::DeployableDevice, public curtinfrc::StrategySystem {
  public:
   HatchIntake(HatchIntakeConfig config) : DeployableDevice(config), _config(config) {};
 

diff --git a/5333/src/main/include/RobotMap.h b/5333/src/main/include/RobotMap.h
@@ -52,6 +52,7 @@ struct RobotMap {
     curtinfrc::PathfinderGains gainsPathfinder{ "Drivetrain Pathfinder", 24.0, 0, 1.5, 0.36, 0.08, 12.0 / 90.0 };    // PIDVAG
 
     curtinfrc::DrivetrainConfig config{ leftGearbox, rightGearbox, &gyro, 0.71, 0.71, 0.0762, 50 };
+    curtinfrc::control::PIDGains gainsVelocity{ "Drivetrain Velocity", 1 };
   };
 
   DriveTrain drivetrain;

diff --git a/common/src/main/cpp/Drivetrain.cpp b/common/src/main/cpp/Drivetrain.cpp
@@ -5,10 +5,15 @@ void curtinfrc::Drivetrain::Set(double leftPower, double rightPower) {
 }
 
 void curtinfrc::Drivetrain::SetVoltage(double left, double right) {
-  _setpoint = std::pair<double, double>{left, right};
+  _setpoint = std::pair<double, double>{ left, right };
   SetState(curtinfrc::DrivetrainState::kManual);
 }
 
+void curtinfrc::Drivetrain::SetVelocity(double left, double right) {
+  _setpoint = std::pair<double, double>{ left, right };
+  SetState(curtinfrc::DrivetrainState::kVelocity);
+}
+
 void curtinfrc::Drivetrain::SetExternalLoop(std::function<std::pair<double, double>(curtinfrc::Drivetrain &, double)> func) {
   _externalLoop = func;
   SetState(curtinfrc::DrivetrainState::kExternalLoop);
@@ -43,16 +48,40 @@ double curtinfrc::Drivetrain::GetRightDistance() {
 
 // Protected
 
+void curtinfrc::Drivetrain::OnStateChange(DrivetrainState newState, DrivetrainState oldState) {
+  switch (newState) {
+   case curtinfrc::DrivetrainState::kVelocity:
+    _pidLeft.SetSetpoint(_setpoint.first);
+    _pidRight.SetSetpoint(_setpoint.second);
+    break;
+
+   default:
+    break;
+  }
+}
+
 void curtinfrc::Drivetrain::OnStatePeriodic(curtinfrc::DrivetrainState state, double dt) {
-  std::pair<double, double> outputs{0, 0};
+  std::pair<double, double> outputs{ 0, 0 };
 
   switch (state) {
    case curtinfrc::DrivetrainState::kManual:
     outputs = _setpoint;
     break;
+
+   case curtinfrc::DrivetrainState::kVelocity:
+    _pidLeft.SetSetpoint(_setpoint.first, false);
+    _pidRight.SetSetpoint(_setpoint.second, false);
+
+    outputs = {
+      _pidLeft.Calculate(GetLeft().encoder->GetEncoderAngularVelocity() * _config.wheelRadius, dt),
+      _pidRight.Calculate(GetRight().encoder->GetEncoderAngularVelocity() * _config.wheelRadius, dt)
+    };
+    break;
+
    case curtinfrc::DrivetrainState::kExternalLoop:
     outputs = _externalLoop(*this, dt);
     break;
+
    default:
     break;
   }

diff --git a/common/src/main/cpp/control/PIDController.cpp b/common/src/main/cpp/control/PIDController.cpp
@@ -41,8 +41,8 @@ double PIDGains::GetkF() const {
 
 PIDController::PIDController(PIDGains gains, double setpoint) : _gains(gains), _setpoint(setpoint), _lastError(0), _filterPos(LinearFilter::MovingAverage(20)), _filterVel(LinearFilter::MovingAverage(20)) {}
 
-void PIDController::SetSetpoint(double setpoint) {
-  Reset();
+void PIDController::SetSetpoint(double setpoint, bool reset) {
+  if (reset) Reset();
   _setpoint = setpoint;
   if (_threshAvgSet == false) {
     _threshAvgPos = setpoint * 0.05;

diff --git a/common/src/main/cpp/strategy/MPStrategy.cpp b/common/src/main/cpp/strategy/MPStrategy.cpp
@@ -24,8 +24,8 @@ void DrivetrainCharacterizationStrategy::OnUpdate(double dt) {
   double leftPos = _drivetrain.GetLeftDistance();
   double rightPos = _drivetrain.GetRightDistance();
 
-  double leftRate = _drivetrain.GetConfig().leftDrive.encoder->GetEncoderAngularVelocity() * 3.14159265 * 2 * _drivetrain.GetConfig().wheelRadius;
-  double rightRate = _drivetrain.GetConfig().rightDrive.encoder->GetEncoderAngularVelocity() * 3.14159265 * 2 * _drivetrain.GetConfig().wheelRadius;
+  double leftRate = _drivetrain.GetConfig().leftDrive.encoder->GetEncoderAngularVelocity() * _drivetrain.GetConfig().wheelRadius;
+  double rightRate = _drivetrain.GetConfig().rightDrive.encoder->GetEncoderAngularVelocity() * _drivetrain.GetConfig().wheelRadius;
 
   double battery = frc::RobotController::GetInputVoltage();
   double motorVolts = battery * std::abs(_lastAutospeed);

diff --git a/common/src/main/include/Drivetrain.h b/common/src/main/include/Drivetrain.h
@@ -52,14 +52,15 @@ namespace curtinfrc {
     bool reversed = false;
   };
 
-  enum class DrivetrainState { kManual = 0, kIdle, kExternalLoop };
+  enum class DrivetrainState { kManual = 0, kVelocity, kIdle, kExternalLoop };
 
-  class Drivetrain : public devices::StateDevice<DrivetrainState> {
+  class Drivetrain : public devices::StateDevice<DrivetrainState>, public StrategySystem {
    public:
-    Drivetrain(DrivetrainConfig config) : _config(config) {};
+    Drivetrain(DrivetrainConfig config, control::PIDGains gains = { "Drivetrain Velocity" }) : _config(config), _pidLeft(gains), _pidRight(gains) {};
 
     void Set(double leftPower, double rightPower);
     void SetVoltage(double left, double right);
+    void SetVelocity(double left, double right);
     void SetExternalLoop(std::function<std::pair<double, double>(Drivetrain &, double)> func);
     void SetIdle();
 
@@ -72,12 +73,14 @@ namespace curtinfrc {
     double GetRightDistance();
 
    protected:
+    void OnStateChange(DrivetrainState newState, DrivetrainState oldState) override;
     void OnStatePeriodic(DrivetrainState state, double dt) override;
 
     Gearbox &GetLeft();
     Gearbox &GetRight();
 
    private:
+    control::PIDController _pidLeft, _pidRight;
     std::pair<double, double> _setpoint;
     std::function<std::pair<double,double>(Drivetrain &,double)> _externalLoop;
 

diff --git a/common/src/main/include/Elevator.h b/common/src/main/include/Elevator.h
@@ -4,6 +4,7 @@
 #include "Gearbox.h"
 #include "sensors/BinarySensor.h"
 #include "control/PIDController.h"
+#include "strategy/StrategySystem.h"
 
 #include "Usage.h"
 
@@ -31,7 +32,7 @@ namespace curtinfrc {
   };
 
   enum class ElevatorState { kStationary = 0, kMoving, kZeroing, kManual };
-  class Elevator : public devices::StateDevice<ElevatorState> {
+  class Elevator : public devices::StateDevice<ElevatorState>, public StrategySystem {
    public:
     Elevator(ElevatorConfig config, control::PIDGains gain) : _config(config), _gain(gain), _controller(gain) {};
 

diff --git a/common/src/main/include/control/PIDController.h b/common/src/main/include/control/PIDController.h
@@ -33,7 +33,7 @@ namespace control {
    public:
     PIDController(PIDGains gains, double setpoint = 0);
 
-    void SetSetpoint(double setpoint);
+    void SetSetpoint(double setpoint, bool reset = true);
     double GetSetpoint();
 
     void SetIZone(double threshIZone);

diff --git a/common/src/main/include/devices/StateDevice.h b/common/src/main/include/devices/StateDevice.h
@@ -1,13 +1,12 @@
 #pragma once
 
 #include "loops/LoopSystem.h"
-#include "strategy/StrategySystem.h"
 
 namespace curtinfrc {
 namespace devices {
 
   template <typename StateType>
-  class StateDevice : public StrategySystem, public loops::LoopSystem {
+  class StateDevice : public loops::LoopSystem {
    public:
     StateDevice() { _state = (StateType)0; };