Claw to Main (#5)

* 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

* Started claw program base

* Added changes to ClawHAL

* Fix Compilation

- Mentor Nick

* Put ClawHAL into RobotControlData.h

* Kavin worked on the claw and combining the files

* [Nick] Fix Compile Errors

* Lucas Alex and Aiza made the claw work

* made the claw work correctly

---------

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: HugoHead <deanmartin@830>

src/main/cpp/ControllerInterface.cpp

@@ -3,6 +3,18 @@
 void ControllerInterface::UpdateRobotControlData(RobotControlData &controlData)
 {
     UpdateSwerveInput(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();
+    controlData.elevatorInput.down = m_copilot.GetLeftBumper();
 };
 
 void ControllerInterface::UpdateSwerveInput(RobotControlData &controlData)

src/main/cpp/HAL/Claw.cpp

@@ -1,31 +1,104 @@
 #include "HAL/Claw.h"
 #include <iostream>
 
+// Claw movements
 
 Claw::Claw()
 {
     // TODO - configure mechanism
+    motor1.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake);
+
     std::cout << "configured claw mechanism" << std::endl;
 }
 
+
+void ManualMovePivot(double speed)
+{ 
+    // Set specific usar input
+}
+double GetAngle()
+{ 
+    // Uses the claws usar input to take action
+    return 0.0;
+}
+
+
+void ProfiledMoveToAngle(double angle)
+{ 
+    // Uses the claws usar input take action
+}
+
+
+double GetSpeed()
+{
+
+    return 0.0;  
+    //m_intakeSpeed;
+}
+
+
+void SetPosition()
+{ 
+// Return to starting position
+}
+
+
+void PublishDebugInfo()
+{ 
+}
+
+
+void ResetProfiledMoveState()
+{ 
+    //m_profileState = 0;
+}
+
 void Claw::Clasp()
 {
-    if (_signal != 1)
-    {
-        std::cout << "claw is clasping" << std::endl;
-        _signal = 1;
-    }
+// if (_signal != 1)
+// {
 
+//     _signal = 1;
+// }
+    std::cout << "claw is clasping" << std::endl;
+    motor1.Set(0.1);
     // TODO - implement mechanism to clasp
+
+}
+void Claw::Stop()
+{
+    motor1.Set(0);
+}
+/*int Claw::LauncherHAL()
+{ 
+    // Put together the compontent
+}
+*/
+
+
+void SetClawSpeed(double speed)
+{ 
+    // Handle any usar input to take this action compontent
+}
+
+double GetClawSpeed()
+{
+    return 0;
+}
+
+void RunIntake(double speed)
+{ 
+    // Set motors to connect to certain speed
 }
 
 void Claw::Unclasp()
 {
-    if (_signal != 2)
-    {
-        std::cout << "claw is unclasping" << std::endl;
-        _signal = 2;
-    }
-
+// if (_signal != 2)
+// {
+//     std::cout << "claw is unclasping" << std::endl;
+//     _signal = 2;
+// }
+    std::cout << "claw is unclasping" << std::endl;
+    motor1.Set(-0.1);
     // TODO - implement mechanism to clasp
 }

src/main/cpp/HAL/Elevator.cpp

@@ -1,29 +1,47 @@
 #include "HAL/Elevator.h"
 #include <iostream>
+#include <frc/Timer.h>
 
 
 Elevator::Elevator()
 {
+    m_profileState = 0;
+    m_Timer = frc::Timer();
     // TODO - configure motors
     std::cout << "configured elevator motors" << std::endl;
 }
 
-void Elevator::ProfiledMoveToHeight(double desired_height)
+void Elevator::ProfiledMoveToHeight(int shelfNumber)
 {
     // TODO - implement profiled move semantics
-    std::cout << "elevator performing profiled move to " << desired_height << std::endl;
-    SetHeight(desired_height);
+    std::cout << "elevator performing profiled move to " << shelfNumber << std::endl;
+    // move elevator to (shelfNumber*15) inches
+    shelfCurrent = shelfNumber;
 }
 
 double Elevator::GetHeight()
 {
-    // TODO - read motors for height
+    // get data from motors
     std::cout << "elevator height measured as 0.0 inches" << std::endl;
     return 0.0f;
 }
-
-void Elevator::SetHeight(double desired_height)
+//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) 
 {
-    // TODO - set motors to go to height
-    std::cout << "elevator setting height to " << desired_height << std::endl;
-}
+    std::string height_increase;
+    // 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 moving" << height_increase << std::endl;
+
+}

src/main/cpp/InputManager/ClawManager.cpp

@@ -0,0 +1,24 @@
+#include "HAL/Claw.h"
+#include "InputManager/ClawManager.h"
+
+
+void ClawManager::HandleInput(RobotControlData& control_data) {
+    if (control_data.clawInput.open){
+        m_claw.Unclasp();
+    } else if(control_data.clawInput.closed){
+        m_claw.Clasp();
+    }
+    else
+    {
+        m_claw.Stop();
+    }
+
+}
+void ClawManager::Reset(){
+    // No sensor, no reset
+}
+
+ClawManager::ClawManager()
+{
+
+}

src/main/cpp/InputManager/ElevatorManager.cpp

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

src/main/cpp/Robot.cpp

@@ -67,6 +67,8 @@ void Robot::TeleopInit() {
 }
 
 void Robot::TeleopPeriodic() {
+  controllerInterface.UpdateRobotControlData(_robot_control_data);
+  clawManager.HandleInput(_robot_control_data);
 
 }
 

src/main/include/ControllerInterface.h

@@ -10,6 +10,8 @@ class ControllerInterface
         void UpdateRobotControlData(RobotControlData &controlData);
     private:
         void UpdateSwerveInput(RobotControlData &controlData);
+        void UpdateClawInput(RobotControlData &controlData);
+        void UpdateElevatorInput(RobotControlData &controlData);
 
         frc::XboxController m_pilot{0};
         frc::XboxController m_copilot{1};

src/main/include/HAL/Claw.h

@@ -1,4 +1,5 @@
 #pragma once
+#include "rev/CANSparkMax.h"
 
 class Claw
 {
@@ -9,10 +10,12 @@ class Claw
 
     void Clasp();
     void Unclasp();
+    void Stop();
 
 private:
     // TODO - get rid... only used for printing
     int _signal = 0;
+    rev::CANSparkMax motor1{4, rev::CANSparkMax::MotorType::kBrushless};
 
 
     // TODO - define mechanism

src/main/include/HAL/Elevator.h

@@ -1,21 +1,29 @@
 #pragma once
 
+#include <frc/Timer.h>
+#include <rev/CANSparkMax.h>
+#include <frc/XboxController.h>
+
+
+
 class Elevator
 {
 public:
     Elevator();
     ~Elevator() = default;
 
     // Functions that make using the elevator simple
-    void ProfiledMoveToHeight(double desired_height);
+    void ProfiledMoveToHeight(int shelf);
+    void ShiftHeight(bool direction);
 
     double GetHeight();
+    int m_profileState = 0;
 
 private:
-
     // Raw set height used by profiled move to set the height
     void SetHeight(double desired_height);
-
-    // TODO - define motors
+    rev::CANSparkMax m_elevatorMotor{9, rev::CANSparkMax::MotorType::kBrushless};
+    frc::Timer m_Timer;
+    int shelfCurrent;
 
 };

src/main/include/InputManager/ClawManager.h

@@ -3,6 +3,7 @@
 #include "HAL/Claw.h"
 #include "RobotControlData.h"
 
+
 class ClawManager
 {
 public:
@@ -13,5 +14,5 @@ class ClawManager
     void Reset();
 
 private:
-    Claw _claw;
+    Claw m_claw;    
 };

src/main/include/InputManager/ElevatorManager.h

@@ -3,6 +3,8 @@
 #include "HAL/Elevator.h"
 #include "RobotControlData.h"
 
+Elevator m_elevator = Elevator();
+
 class ElevatorManager
 {
 public:

src/main/include/Robot.h

@@ -10,6 +10,8 @@
 #include <array>
 #include <frc/XboxController.h>
 #include <memory>
+#include "ControllerInterface.h"
+#include "InputManager/ClawManager.h"
 #include "RobotControlData.h"
 class Robot : public frc::TimedRobot {
  public:
@@ -25,6 +27,8 @@ class Robot : public frc::TimedRobot {
   void TestPeriodic() override;
   void SimulationInit() override;
   void SimulationPeriodic() override;
+  ControllerInterface controllerInterface;
+  ClawManager clawManager;
 
  private:
 

src/main/include/RobotControlData.h

@@ -9,17 +9,24 @@ struct SwerveInput{
 
 struct ClawInput
 {
+
     // TODO - define members
+    bool open;
+    bool closed;
+
 };
 
 struct ClawOutput
 {
+
+
     // TODO - define members
 };
 
 struct ElevatorInput
 {
-    // TODO - define members
+    bool up;
+    bool down;
 };
 
 struct ElevatorOutput