Elevator HAL (#2)

* created elevator code structure

* added more funtions to ElevatorHAL

* Fix Compilation

- Mentor Nick

* added MoveHeightBy function.

* Elevator Code Expanded (but to be improved)

* [Nick] Fix compile errors

* callum
removed extra HAL folders, developed elevator functions

* updated internal elevator logic, and controller interfacing for elevator
callum

* Callum, Kavin, Sid - Elevator code attempt #1

* Elevator on test board

* Callum & Kavin - Elevator partially working on test board

* callum - changed profiled move to height to have 3 states

* elevator works on testboard

---------

Co-authored-by: Jack Jin <79951112+Rvistix@users.noreply.github.com>
Co-authored-by: RandomPerson5125 <murkavin@gmail.com>
Co-authored-by: Student <student@ratpack>
Co-authored-by: TheCrudeOilBro <lucasyuan1981@gmail.com>
Co-authored-by: NottheIRS <92690588+NottheIRS@users.noreply.github.com>

src/main/cpp/ControllerInterface.cpp

@@ -6,11 +6,13 @@ void ControllerInterface::UpdateRobotControlData(RobotControlData &controlData)
     UpdateElevatorInput(controlData);
     UpdateClawInput(controlData);
 };
+
 void ControllerInterface::UpdateClawInput(RobotControlData &controlData){
 
     controlData.clawInput.closed = m_pilot.GetLeftTriggerAxis() > 0.05;
     controlData.clawInput.open = m_pilot.GetRightTriggerAxis() > 0.05;
 }
+
 void ControllerInterface::UpdateElevatorInput(RobotControlData &controlData)
 {
     controlData.elevatorInput.up = m_copilot.GetRightBumper();

src/main/cpp/HAL/Elevator.cpp

@@ -1,7 +1,7 @@
 #include "HAL/Elevator.h"
 #include <iostream>
 #include <frc/Timer.h>
-
+#include <math.h>
 
 Elevator::Elevator()
 {
@@ -11,12 +11,12 @@ Elevator::Elevator()
     std::cout << "configured elevator motors" << std::endl;
 }
 
-void Elevator::ProfiledMoveToHeight(int shelfNumber)
+void ProfiledMoveToHeight(int shelfNumber)
 {
     // TODO - implement profiled move semantics
     std::cout << "elevator performing profiled move to " << shelfNumber << std::endl;
     // move elevator to (shelfNumber*15) inches
-    shelfCurrent = shelfNumber;
+    //shelfCurrent = shelfNumber;
 }
 
 double Elevator::GetHeight()
@@ -25,23 +25,54 @@ double Elevator::GetHeight()
     std::cout << "elevator height measured as 0.0 inches" << std::endl;
     return 0.0f;
 }
+
 //setHeight switched to ProfiledMoveToHeight
 // void Elevator::SetHeight(double desired_height)
 // {
 //     // TODO - set motors to go to height
 //     std::cout << "elevator setting height to " << desired_height << std::endl;
 // }
-void Elevator::ShiftHeight(bool direction) 
+void Elevator::ProfiledMoveToHeight(int direction) 
+{
+    if(direction==1) {
+        m_elevatorMotor.Set(0.05);
+    } else if (direction==-1) {
+        m_elevatorMotor.Set(-0.05);
+    } else {
+        m_elevatorMotor.Set(0);
+    }
+
+    /*
+    std::string height_increase;
+    int shelfNumber = shelfCurrent + 1;
+    // TODO - make motors go up or down 
+    std::cout << "elevator performing profiled move to " << shelfNumber << std::endl;
+    if (shelfCurrent < 3)
+    {
+        if (direction)
+    {
+        m_elevatorMotor.Set(0.5);
+        ++shelfCurrent;
+    } else if (direction){
+        m_elevatorMotor.Set(-0.5);
+        --shelfCurrent;
+    }
+    std::cout << "performed profiled move to " << shelfNumber << std::endl;
+    std::cout << "elevator moving" << height_increase << std::endl;
+    }
+    */
+}
+
+void ShiftHeight(bool direction) 
 {
     std::string height_increase;
     // TODO - make motors go up or down 
     if (direction==true){
         height_increase = "up";
-        Elevator::ProfiledMoveToHeight(shelfCurrent+1);
+        //Elevator::ProfiledMoveToHeight(shelfCurrent+1);
     }else{
         height_increase = "down";
-        Elevator::ProfiledMoveToHeight(shelfCurrent-1);
+        //Elevator::ProfiledMoveToHeight(shelfCurrent-1);
     }
     std::cout << "elevator moving" << height_increase << std::endl;
-
 }

src/main/cpp/InputManager/ElevatorManager.cpp

@@ -1,16 +1,21 @@
-#include "HAL/Elevator.h"
 #include "InputManager/ElevatorManager.h"
 #include <iostream>
 #include <frc/Timer.h>
 
+ElevatorManager::ElevatorManager()
+{
 
-void HandleInput(RobotControlData& control_data) {
+}
+
+void ElevatorManager::HandleInput(RobotControlData& control_data) {
     if (control_data.elevatorInput.up) {
-        m_elevator.ShiftHeight(true);
-    }else if(control_data.elevatorInput.down) {
-        m_elevator.ShiftHeight(false);
+        m_elevator.ProfiledMoveToHeight(1);
+    } else if(control_data.elevatorInput.down) {
+        m_elevator.ProfiledMoveToHeight(-1);
+    }  else {
+        m_elevator.ProfiledMoveToHeight(0);
     }
 };
-void Reset(){
+void ElevatorManager::Reset(){
     m_elevator.ProfiledMoveToHeight(0);
 };

src/main/cpp/Robot.cpp

@@ -5,6 +5,7 @@
 #include "Robot.h"
 #include <fmt/core.h>
 #include <frc/smartdashboard/SmartDashboard.h>
+#include "HAL\Elevator.h"
 
 void Robot::RobotInit() {
 
@@ -68,8 +69,8 @@ void Robot::TeleopInit() {
 
 void Robot::TeleopPeriodic() {
   controllerInterface.UpdateRobotControlData(_robot_control_data);
+  elevatorManager.HandleInput(_robot_control_data);
   clawManager.HandleInput(_robot_control_data);
-
 }
 
 void Robot::DisabledInit() {}

src/main/include/ControllerInterface.h

@@ -8,12 +8,13 @@ class ControllerInterface
         ControllerInterface() = default;
         ~ControllerInterface() = default;
         void UpdateRobotControlData(RobotControlData &controlData);
+        frc::XboxController m_pilot{0};
+        frc::XboxController m_copilot{1};
+
     private:
         void UpdateSwerveInput(RobotControlData &controlData);
         void UpdateClawInput(RobotControlData &controlData);
-        void UpdateElevatorInput(RobotControlData &controlData);
-
-        frc::XboxController m_pilot{0};
-        frc::XboxController m_copilot{1};
+        void UpdateElevatorInput(RobotControlData &controlData);        
+
         double m_slowmodefactor = 0.25;
 };

src/main/include/HAL/Elevator.h

@@ -3,8 +3,7 @@
 #include <frc/Timer.h>
 #include <rev/CANSparkMax.h>
 #include <frc/XboxController.h>
-
-
+#include "RobotControlData.h"    
 
 class Elevator
 {
@@ -13,17 +12,24 @@ class Elevator
     ~Elevator() = default;
 
     // Functions that make using the elevator simple
-    void ProfiledMoveToHeight(int shelf);
-    void ShiftHeight(bool direction);
+    void ProfiledMoveToHeight(int direction);
+    void HandleInput(RobotControlData& input);
 
     double GetHeight();
     int m_profileState = 0;
+    int shelfCurrent = 0;
 
 private:
     // Raw set height used by profiled move to set the height
     void SetHeight(double desired_height);
-    rev::CANSparkMax m_elevatorMotor{9, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax m_elevatorMotor{5, rev::CANSparkMax::MotorType::kBrushless};
     frc::Timer m_Timer;
-    int shelfCurrent;
-
+
+    bool m_ElevatorFlag;
+    int m_ElevatorState;
+    bool m_PrevElevatorFlag;
+    bool m_SwitchModeFlag;
+    bool m_prevSwitchMode;
+    double r=0.0;
+    double shelfHeight=15.0;
 };

src/main/include/InputManager/ElevatorManager.h

@@ -3,7 +3,6 @@
 #include "HAL/Elevator.h"
 #include "RobotControlData.h"
 
-Elevator m_elevator = Elevator();
 
 class ElevatorManager
 {
@@ -15,5 +14,5 @@ class ElevatorManager
     void Reset();
 
 private:
-    Elevator _elevator;
+    Elevator m_elevator;
 };

src/main/include/Robot.h

@@ -12,6 +12,7 @@
 #include <memory>
 #include "ControllerInterface.h"
 #include "InputManager/ClawManager.h"
+#include "InputManager/ElevatorManager.h"
 #include "RobotControlData.h"
 class Robot : public frc::TimedRobot {
  public:
@@ -29,9 +30,9 @@ class Robot : public frc::TimedRobot {
   void SimulationPeriodic() override;
   ControllerInterface controllerInterface;
   ClawManager clawManager;
+  ElevatorManager elevatorManager;
 
  private:
-
   frc::SendableChooser<std::string> m_chooser;
   const std::string kAutoNameDefault = "Default";
   const std::string kAutoNameCustom = "My Auto";