diff --git a/src/main/cpp/HAL/Elevator.cpp b/src/main/cpp/HAL/Elevator.cpp index 75b3ef2..c5f4759 100644 --- 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,6 +28,20 @@ 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) { @@ -41,6 +49,7 @@ void Elevator::ProfiledMoveToHeight(int direction) } 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 index 6f9a657..2fc214a 100644 --- a/src/main/include/HAL/Elevator.h +++ b/src/main/include/HAL/Elevator.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #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 index ca23701..1e66ca4 100644 --- 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;