callum - changed elevator logic, added correct motor and updated swer…

…ve speed
FRC830 · Dec 11, 2024 · 33e765b · 33e765b
1 parent bbc95ca
commit 33e765b
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 12 deletions.
diff --git a/src/main/cpp/HAL/Elevator.cpp b/src/main/cpp/HAL/Elevator.cpp
@@ -11,13 +11,7 @@ Elevator::Elevator()
     std::cout << "configured elevator motors" << std::endl;
 }
 
-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;
-}
+
 
 double Elevator::GetHeight()
 {
@@ -34,13 +28,28 @@ double Elevator::GetHeight()
 // }
 void Elevator::ProfiledMoveToHeight(int direction) 
 {
+    m_Timer.Start();
+    while (m_Timer.Get()<=3_s) {
+        if (m_Timer.Get()<1_s){
+        m_elevatorMotor.Set(m_Timer.Get()/1_s);
+        } else if (m_Timer.Get()>2_s) {
+            --speedMult;
+        }
+        m_elevatorMotor.Set(m_Timer.Get()/1_s);
+    }
+    m_elevatorMotor.Set(0);
+    m_Timer.Stop();
+    m_Timer.Reset();
+    speedMult=0;
+    /*
     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;
@@ -62,6 +71,16 @@ void Elevator::ProfiledMoveToHeight(int direction)
     }
     */
 }
+void Elevator::MoveElevator(int direction) 
+{
+    if(direction==1) {
+        m_elevatorMotor.Set(0.05);
+    } else if (direction==-1) {
+        m_elevatorMotor.Set(-0.05);
+    } else {
+        m_elevatorMotor.Set(0);
+    }
+}
 
 void ShiftHeight(bool direction) 
 {

diff --git a/src/main/include/HAL/Elevator.h b/src/main/include/HAL/Elevator.h
@@ -1,7 +1,7 @@
 #pragma once
 
 #include <frc/Timer.h>
-#include <rev/CANSparkMax.h>
+#include <ctre/phoenix6/TalonFX.hpp>
 #include <frc/XboxController.h>
 #include "RobotControlData.h"    
 
@@ -21,10 +21,10 @@ class Elevator
 
 private:
     // Raw set height used by profiled move to set the height
-    void SetHeight(double desired_height);
-    rev::CANSparkMax m_elevatorMotor{5, rev::CANSparkMax::MotorType::kBrushless};
+    void MoveElevator(int direction);
+    ctre::phoenix6::hardware::TalonFX m_elevatorMotor{5};
     frc::Timer m_Timer;
-
+    int speedMult=0;
     bool m_ElevatorFlag;
     int m_ElevatorState;
     bool m_PrevElevatorFlag;

diff --git a/src/main/include/SwerveConfig.h b/src/main/include/SwerveConfig.h
@@ -150,7 +150,7 @@ const bool GYRO_INVERTED = true;
 const frc::Rotation2d GYRO_ZERO_HEADING{units::degree_t{0.0f}};
 
 // --------------------------Swerve---------------------------
-const double MAX_DRIVE_SPEED_FPS = 18.0;
+const double MAX_DRIVE_SPEED_FPS = 5.0;
 const double MAX_ANGULAR_VELOCITY_DEGPS = 540.0;
 const double CONTROLLER_DEADZONE = 0.1;
 const int TURN_MOTOR_CURRENT_LIMIT = 20;