Callum, Kavin, Sid - Elevator code attempt #1

src/main/cpp/HAL/Elevator.cpp

@@ -1,6 +1,7 @@
 #include "HAL/Elevator.h"
 #include <iostream>
 #include <frc/Timer.h>
+#include <math.h>
 
 
 Elevator::Elevator()
@@ -11,37 +12,29 @@ Elevator::Elevator()
     std::cout << "configured elevator motors" << std::endl;
 }
 
-void Elevator::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;
-}
 
-double Elevator::GetHeight()
-{
-    // get data from motors
-    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(bool direction) 
 {
     std::string height_increase;
+    int shelfNumber = shelfCurrent + 1;
     // TODO - make motors go up or down 
-    if (direction==true){
-        height_increase = "up";
-        Elevator::ProfiledMoveToHeight(shelfCurrent+1);
-    }else{
-        height_increase = "down";
-        Elevator::ProfiledMoveToHeight(shelfCurrent-1);
+    std::cout << "elevator performing profiled move to " << shelfNumber << std::endl;
+    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;
 
 }

src/main/cpp/InputManager/ElevatorManager.cpp

@@ -6,9 +6,9 @@
 
 void HandleInput(RobotControlData& control_data) {
     if (control_data.elevatorInput.up) {
-        m_elevator.ShiftHeight(true);
+        m_elevator.ProfiledMoveToHeight(true);
     }else if(control_data.elevatorInput.down) {
-        m_elevator.ShiftHeight(false);
+        m_elevator.ProfiledMoveToHeight(false);
     }
 };
 void Reset(){

src/main/include/ControllerInterface.h

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

src/main/include/HAL/Elevator.h

@@ -3,6 +3,7 @@
 #include <frc/Timer.h>
 #include <rev/CANSparkMax.h>
 #include <frc/XboxController.h>
+#include "RobotControlData.h"
 
 
 
@@ -13,17 +14,24 @@ class Elevator
     ~Elevator() = default;
 
     // Functions that make using the elevator simple
-    void ProfiledMoveToHeight(int shelf);
-    void ShiftHeight(bool direction);
+    void ProfiledMoveToHeight(bool 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{4, 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;
 };