From 5c5899b0d5d9ca3fd24e7ea04e4ef623131873a0 Mon Sep 17 00:00:00 2001 From: Paul Hodges Date: Tue, 15 Aug 2023 18:42:13 +0800 Subject: [PATCH] Added Velocity Setpoints --- src/main/cpp/behaviour/ArmavatorBehaviour.cpp | 102 +++++++++++++++++- 1 file changed, 97 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/behaviour/ArmavatorBehaviour.cpp b/src/main/cpp/behaviour/ArmavatorBehaviour.cpp index 7fd12a4..3ef628e 100644 --- a/src/main/cpp/behaviour/ArmavatorBehaviour.cpp +++ b/src/main/cpp/behaviour/ArmavatorBehaviour.cpp @@ -103,7 +103,7 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) { // velocity control { ArmavatorVelocity av; - av.angleSpeed = -wom::spow2(wom::deadzone(_codriver.GetLeftY(), 0.1)) * (180_deg / 1_s); + av.angleSpeed = wom::spow2(wom::deadzone(_codriver.GetLeftY(), 0.1)) * (180_deg / 1_s); av.elevatorSpeed = -wom::spow2(wom::deadzone(_codriver.GetRightY(), 0.1)) * (2_m / 1_s); units::meter_t height = _armavator->GetCurrentPosition().height; @@ -204,7 +204,7 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) { _manualSetpoint.angle = _min_angle; } else { - _manualSetpoint.angle -= (wom::deadzone(_codriver.GetLeftY(), 0.2) * 1_deg * 1.5); + _manualSetpoint.angle += (wom::deadzone(_codriver.GetLeftY(), 0.2) * 1_deg * 1.5); } // @@ -462,16 +462,108 @@ void ArmavatorManualBehaviour::CheckSetpoints() { return; } -void ArmavatorManualBehaviour::SetPosition(units::degree_t angle, units::meter_t height, std::string name, double elevatorSpeed, double armSpeed) { +void ArmavatorManualBehaviour::SetPosition(units::degree_t setpoint_angle, units::meter_t setpoint_height, std::string name, double elevatorSpeed, double armSpeed) { if (_armManualModes == ArmavatorManualModeEnum::kPosition) { - _manualSetpoint.height = height; - _manualSetpoint.angle = angle; + _manualSetpoint.height = setpoint_height; + _manualSetpoint.angle = setpoint_angle; // _armavator->SetPosition(_manualSetpoint); // _manualSetpoint = {_armavator->GetCurrentPosition().height, _armavator->GetCurrentPosition().angle}; std::cout << "GO TO armavator POS " << name << std::endl; _armavator->SetSpeedValues(elevatorSpeed, armSpeed); } else if (_armManualModes == ArmavatorManualModeEnum::kVelocity) { + units::meter_t height = _armavator->GetCurrentPosition().height; + units::degree_t angle = _armavator->GetCurrentPosition().angle; + _armavator->SetSpeedValues(0.5, 0.3); + //bool above_height = true; + //sets hard limits in place, necessary to make the system not break itself (you will burn the neos out if they try to go past what they can) + if (_manualSetpoint.height > _max_height) { + _manualSetpoint.height = _max_height; + } else if (_manualSetpoint.height < _min_height) { + _manualSetpoint.height = _min_height; + } + else { + _manualSetpoint.height = setpoint_height; + } + + if (_manualSetpoint.angle > _max_angle) { + _manualSetpoint.angle = _max_angle; + } else if (_manualSetpoint.angle < _min_angle) { + _manualSetpoint.angle = _min_angle; + } + else { + _manualSetpoint.angle = setpoint_angle; + } + + // + // _manualSetpoint.height -= (wom::deadzone(_codriver.GetRightY(), 0.2) * 1_m * 0.05); //slows the system down, otherwise it's wayyy too fast + // _manualSetpoint.angle -= (wom::deadzone(_codriver.GetLeftY(), 0.2) * 1_deg * 1); + + //some funky math which makes the armavator stay within extension limits + units::meter_t max_height = (1.6_m - 0.51_m) - _armavator->arm->GetConfig().armLength * units::math::sin(_manualSetpoint.angle); + ArmavatorPosition sp{ + units::math::min(_manualSetpoint.height, max_height), + _manualSetpoint.angle + }; + + + //units::meter_t max_height = (1.7_m - 0.51_m) - _armavator->arm->GetConfig().armLength * units::math::sin(_manualSetpoint.angle); + units::degree_t max_angle_front = units::math::acos(_armavator->GetCurrentPosition().height/_armavator->arm->GetConfig().armLength) + 10_deg - 120_deg; + units::degree_t max_angle_back = 360_deg - units::math::acos(_armavator->GetCurrentPosition().height/_armavator->arm->GetConfig().armLength) - 5_deg - 50_deg; + + + //units::meter_t height = _armavator->GetCurrentPosition().height; + //units::degree_t angle = _armavator->GetCurrentPosition().angle; + + frc::SmartDashboard::PutNumber("Max Angle Front: ", max_angle_front.value()); + frc::SmartDashboard::PutNumber("Max Angle Back: ", max_angle_back.value()); + frc::SmartDashboard::PutNumber("Current Angle", angle.value()); + + frc::SmartDashboard::PutNumber("Height", height.value()); + frc::SmartDashboard::PutNumber("Max Height", max_height.value()); + + + + if (height > 0.675_m) { + _armavator->SetPosition(sp); + } + else if (angle < max_angle_front) { + //_armavator->SetArmPosition(max_angle_front * (std::numbers::pi / 180)); + if (sp.angle > max_angle_front) { + _armavator->SetPosition(sp); + } + else { + //sp.angle = max_angle_front / 60; + _manualSetpoint.angle = max_angle_front; + _armavator->SetPosition(sp); + } + } + else if (angle > max_angle_back) { + if (sp.angle < max_angle_back) { + _armavator->SetPosition(sp); + } + else { + _manualSetpoint.angle = max_angle_back; + _armavator->SetPosition(sp); + } + + } + + else { + //if (wom::deadzone(_codriver.GetLeftY(), 0.2)) { + //_manualSetpoint.angle -= (wom::deadzone(_codriver.GetLeftY(), 0.2) * 1_deg * 1); + //} + //if (wom::deadzone(_codriver.GetRightY(), 0.2)) { + //_manualSetpoint.height -= (wom::deadzone(_codriver.GetRightY(), 0.2) * 1_m * 0.05); //slows the system down, otherwise it's wayyy too fast + //} + _armavator->SetPosition(sp); + + frc::SmartDashboard::PutNumber("Current Angle", _manualSetpoint.angle.value()); + frc::SmartDashboard::PutNumber("Current Height", _manualSetpoint.height.value()); + + + } + } }