From d4830945b3816709a076f7bc08e70e733597854c Mon Sep 17 00:00:00 2001 From: SusanC3 Date: Mon, 28 Mar 2022 20:45:27 -0700 Subject: [PATCH 1/5] add raised hood zero functionality --- gitignore | 162 +++++++++++++++++++++++++++++++++++ src/main/cpp/Robot.cpp | 37 +++++++- src/main/cpp/Shooter.cpp | 27 ++++-- src/main/include/Constants.h | 3 +- src/main/include/Robot.h | 1 + src/main/include/Shooter.h | 8 ++ 6 files changed, 229 insertions(+), 9 deletions(-) create mode 100644 gitignore diff --git a/gitignore b/gitignore new file mode 100644 index 0000000..9535c83 --- /dev/null +++ b/gitignore @@ -0,0 +1,162 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# Simulation GUI and other tools window save file +*-window.json diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index b34d36f..d2ec386 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -22,7 +22,7 @@ Robot::RobotInit() { } catch(const std::exception& e){ std::cout << e.what() <GetYaw(), true); + //m_swerve.Drive(-x1, -y1, -x2, navx->GetYaw(), true); //Climbing if(m_climbing){ @@ -219,6 +219,7 @@ Robot::TeleopPeriodic() { void Robot::TestInit() { + //m_shooter.getHood()->SetSelectedSensorPosition(ShooterConstants::hoodInitAngle); } @@ -230,6 +231,38 @@ Robot::TestPeriodic() { // m_shooter.Manual(xbox.GetRawAxis(4)); // m_shooter.setState(Shooter::State::MANUAL); // } + + //HOOD TESTING UTILS BELOW + + // double P = frc::SmartDashboard::GetNumber("Hood P", 0.0); + // double I = frc::SmartDashboard::GetNumber("Hood I", 0.0); + // double D = frc::SmartDashboard::GetNumber("Hood D", 0.0); + // frc::SmartDashboard::PutNumber("Hood P", P); + // frc::SmartDashboard::PutNumber("Hood I", I); + // frc::SmartDashboard::PutNumber("Hood D", D); + + // m_shooter.getHood()->Config_kP(0, P); + // m_shooter.getHood()->Config_kI(0, I); + // m_shooter.getHood()->Config_kD(0, D); + + // //Note: This uses percent output instead of m_angle to check for past limit + // if(m_shooter.getHood()->GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ + // m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); + // } + // else if(m_shooter.getHood()->GetSelectedSensorPosition() > ShooterConstants::hoodMax && + // m_shooter.getHood()->GetMotorOutputPercent() > 0){ + // m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); + // return; + // } + // else if(m_shooter.getHood()->GetSelectedSensorPosition() < ShooterConstants::hoodMin && + // m_shooter.getHood()->GetMotorOutputPercent() < 0){ + // m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); + // return; + // } + // if (xbox.GetRawButton(1)) m_shooter.hoodGoTo511(); + // else if (xbox.GetRawButton(2)) m_shooter.hoodGoTo5700(); + // else if (xbox.GetRawButton(3)) m_shooter.hoodGoTo3000(); + // frc::SmartDashboard::PutNumber("hood pose", m_shooter.getHood()->GetSelectedSensorPosition()); } diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp index d74ffc8..2d2d4ee 100644 --- a/src/main/cpp/Shooter.cpp +++ b/src/main/cpp/Shooter.cpp @@ -145,6 +145,7 @@ Shooter::Periodic(){ void Shooter::Aim(){ //m_limelight->setLEDMode("ON"); + std::cout << "in aim\n"; if(m_colorMatcher.MatchClosestColor(m_colorSensor.GetColor(), confidence) == ballColor){ m_speed = 8000; @@ -229,11 +230,23 @@ Shooter::Aim(){ frc::SmartDashboard::PutNumber("hood angle", m_hood.GetSelectedSensorPosition()); // Set Hood Position - // if(m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ - // m_hood.Set(ControlMode::PercentOutput, 0.0); - // } else { - m_hood.Set(ControlMode::Position, m_angle); - // } + if(m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ + m_hood.Set(ControlMode::PercentOutput, 0.0); + return; + } + else if(m_hood.GetSelectedSensorPosition() > ShooterConstants::hoodMax && + m_angle >= ShooterConstants::hoodMax){ + m_hood.Set(ControlMode::PercentOutput, 0.0); + return; + } + else if(m_hood.GetSelectedSensorPosition() < ShooterConstants::hoodMin && + m_angle <= ShooterConstants::hoodMin){ + m_hood.Set(ControlMode::PercentOutput, 0.0); + return; + } + else { + m_hood.Set(ControlMode::Position, m_angle); + } //frc::SmartDashboard::PutNumber("yOff", m_limelight->getYOff()); //frc::SmartDashboard::PutNumber("xOff", m_limelight->getXOff()); } @@ -324,7 +337,8 @@ void Shooter::Zero(){ m_turret.SetSelectedSensorPosition(-31500); m_turretZero = true; - m_hood.SetSelectedSensorPosition(0); + //NOTE: REPLACE INIT ANGLE WITH ZERO IF WE'RE DOING CURRENT BASED ZEROING + m_hood.SetSelectedSensorPosition(ShooterConstants::hoodInitAngle); // m_hoodZero = true; // if(m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ // m_hood.Set(ControlMode::PercentOutput, 0.0); @@ -341,6 +355,7 @@ Shooter::Zero(){ } +//NOTE: SHOULD NOT BE CALLED IN CASE WHERE WE'RE DOING RAISED ZERO HOOD void Shooter::zeroHood(){ if(m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index f59a15f..31b4bb1 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -118,7 +118,8 @@ ShooterConstants{ const double turretMin = -63000.0; //confirmed const double hoodMax = 5700; //confirmed - const double hoodMin = 0; //confirmed + const double hoodInitAngle = 511; + const double hoodMin = hoodInitAngle; const double angleOff = 300; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 6e64572..87695f5 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -45,6 +45,7 @@ class Robot : public frc::TimedRobot { void TestPeriodic() override; + private: AutoMode m_auto; diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h index 58bf4db..6baf730 100644 --- a/src/main/include/Shooter.h +++ b/src/main/include/Shooter.h @@ -51,6 +51,14 @@ class Shooter{ void EdgeofTarmac(); void zeroHood(); + //For testing + void hoodGoTo511() {m_hood.Set(ControlMode::Position, 511);} + void hoodGoTo5700() {m_hood.Set(ControlMode::Position, 5700);} + void hoodGoTo3000() {m_hood.Set(ControlMode::Position, 3000);} + WPI_TalonFX * getHood() {return &m_hood; } + WPI_TalonFX * getTurret() {return &m_turret; } + WPI_TalonFX * getFlywheel() {return &m_flywheelMaster; } + private: //TalonFX in ticks - 0 - 20,000 //in rpm - 0 -6000 From 0c35f8cfe91462511cfaf4a3f7d6fb7908298211 Mon Sep 17 00:00:00 2001 From: SusanC3 Date: Mon, 28 Mar 2022 20:48:44 -0700 Subject: [PATCH 2/5] fixed mistake --- src/main/cpp/Robot.cpp | 2 +- src/main/cpp/Shooter.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index d2ec386..0d78387 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -140,7 +140,7 @@ Robot::TeleopPeriodic() { // frc::SmartDashboard::PutNumber("X",m_swerve.GetXPosition()); // frc::SmartDashboard::PutNumber("Y",m_swerve.GetYPosition()); - //m_swerve.Drive(-x1, -y1, -x2, navx->GetYaw(), true); + m_swerve.Drive(-x1, -y1, -x2, navx->GetYaw(), true); //Climbing if(m_climbing){ diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp index 2d2d4ee..1c143f5 100644 --- a/src/main/cpp/Shooter.cpp +++ b/src/main/cpp/Shooter.cpp @@ -145,7 +145,6 @@ Shooter::Periodic(){ void Shooter::Aim(){ //m_limelight->setLEDMode("ON"); - std::cout << "in aim\n"; if(m_colorMatcher.MatchClosestColor(m_colorSensor.GetColor(), confidence) == ballColor){ m_speed = 8000; From 9b1b845084eed21d0dbb4e3e2ed1ff59891b4cb7 Mon Sep 17 00:00:00 2001 From: SusanC3 Date: Wed, 30 Mar 2022 19:51:39 -0700 Subject: [PATCH 3/5] added breaks, fixed interpolation --- src/main/cpp/Shooter.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp index 1c143f5..b4ccdca 100644 --- a/src/main/cpp/Shooter.cpp +++ b/src/main/cpp/Shooter.cpp @@ -127,8 +127,10 @@ Shooter::Periodic(){ EdgeofTarmac(); // Shoot(); m_channel.setState(Channel::State::RUN); + break; case State::Hood: zeroHood(); + break; default: break; } @@ -186,7 +188,7 @@ Shooter::Aim(){ speed2 = data2.second; // interpolate - - double interval = point1 - point2; + double interval = point2 - point1; double xdiff = point - point1; m_angle = (((angle2 - angle1)/interval * xdiff) + angle1) * angle_scale_factor; m_speed = (((speed2 - speed1)/interval * xdiff) + speed1) * speed_scale_factor; From feb46ab8b7a2aabf6b3ff6600654c63fdc1309a3 Mon Sep 17 00:00:00 2001 From: SusanC3 Date: Thu, 31 Mar 2022 10:17:47 -0700 Subject: [PATCH 4/5] changed aim to use output not angle, added sdb outs --- src/main/cpp/Robot.cpp | 37 ++++++------ src/main/cpp/Shooter.cpp | 120 +++++++++++++++++++++++-------------- src/main/include/Shooter.h | 2 +- 3 files changed, 94 insertions(+), 65 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 0d78387..52caeb7 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -220,6 +220,7 @@ Robot::TeleopPeriodic() { void Robot::TestInit() { //m_shooter.getHood()->SetSelectedSensorPosition(ShooterConstants::hoodInitAngle); + m_shooter.Zero(); } @@ -245,24 +246,24 @@ Robot::TestPeriodic() { // m_shooter.getHood()->Config_kI(0, I); // m_shooter.getHood()->Config_kD(0, D); - // //Note: This uses percent output instead of m_angle to check for past limit - // if(m_shooter.getHood()->GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ - // m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); - // } - // else if(m_shooter.getHood()->GetSelectedSensorPosition() > ShooterConstants::hoodMax && - // m_shooter.getHood()->GetMotorOutputPercent() > 0){ - // m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); - // return; - // } - // else if(m_shooter.getHood()->GetSelectedSensorPosition() < ShooterConstants::hoodMin && - // m_shooter.getHood()->GetMotorOutputPercent() < 0){ - // m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); - // return; - // } - // if (xbox.GetRawButton(1)) m_shooter.hoodGoTo511(); - // else if (xbox.GetRawButton(2)) m_shooter.hoodGoTo5700(); - // else if (xbox.GetRawButton(3)) m_shooter.hoodGoTo3000(); - // frc::SmartDashboard::PutNumber("hood pose", m_shooter.getHood()->GetSelectedSensorPosition()); + //Note: This uses percent output instead of m_angle to check for past limit + if(m_shooter.getHood()->GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ + m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); + } + else if(m_shooter.getHood()->GetSelectedSensorPosition() > ShooterConstants::hoodMax && + m_shooter.getHood()->GetMotorOutputPercent() > 0){ + m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); + return; + } + else if(m_shooter.getHood()->GetSelectedSensorPosition() < ShooterConstants::hoodMin && + m_shooter.getHood()->GetMotorOutputPercent() < 0){ + m_shooter.getHood()->Set(ControlMode::PercentOutput, 0.0); + return; + } + if (xbox.GetRawButton(1)) m_shooter.hoodGoTo511(); + else if (xbox.GetRawButton(2)) m_shooter.hoodGoTo5700(); + else if (xbox.GetRawButton(3)) m_shooter.hoodGoTo3000(); + frc::SmartDashboard::PutNumber("hood pose", m_shooter.getHood()->GetSelectedSensorPosition()); } diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp index b4ccdca..518c7f3 100644 --- a/src/main/cpp/Shooter.cpp +++ b/src/main/cpp/Shooter.cpp @@ -40,47 +40,47 @@ Shooter::Shooter(){ // m_turretPos.EnableContinuousInput(-1000, -63000); - // dataMap[-24.0] = {5800, 18500}; - // dataMap[-23.5] = {5750, 18500}; - // dataMap[-22.5] = {5700, 18500}; - // dataMap[-21.5] = {5600, 18500}; - // dataMap[-20.0] = {5500, 18200}; - // dataMap[-19.2] = {5500, 18000}; - // dataMap[-18.0] = {5400, 17400}; - // dataMap[-16.7] = {5400, 17000}; - // dataMap[-15.4] = {4900, 15700}; - // dataMap[-14.5] = {4600, 15500}; - // dataMap[-12.6] = {4500, 14500}; - // dataMap[-11.2] = {4400, 13800}; - // dataMap[-9.7] = {4400, 13600}; - // dataMap[-8.0] = {4200, 13500}; - // dataMap[-6.2] = {4000, 13500}; - // dataMap[-5.0] = {3600, 14000}; - // dataMap[-2.5] = {3000, 13900}; - // dataMap[-0.5] = {2400, 13400}; - // dataMap[5.5] = {1900, 12900}; - // dataMap[9.1] = {1700, 12900}; - // dataMap[12.4] = {1500, 12800}; - // dataMap[16.5] = {1300, 12700}; - // dataMap[18.7] = {1200, 12500}; - - dataMap[-19.4] = {5700, 17200}; - dataMap[-18.9] = {5700, 16400}; - dataMap[-17.5] = {5600, 15900}; - dataMap[-14.8] = {5600, 14900}; - dataMap[-12.5] = {5600, 14000}; - dataMap[-9.5] = {5500, 13500}; - dataMap[-7.4] = {5400, 13200}; - dataMap[-5.4] = {5200, 12600}; - dataMap[-1.8] = {4800, 12300}; - dataMap[0.5] = {4700, 12200}; - dataMap[1.7] = {4500, 12000}; - dataMap[3.65] = {4100, 12000}; - dataMap[6.0] = {3800, 11800}; - dataMap[9.9] = {2800, 11700}; - dataMap[14.2] = {2800, 11500}; - dataMap[16.0] = {2600, 11000}; - dataMap[19.88] = {1800, 10500}; + dataMap[-24.0] = {5800, 18500}; + dataMap[-23.5] = {5750, 18500}; + dataMap[-22.5] = {5700, 18500}; + dataMap[-21.5] = {5600, 18500}; + dataMap[-20.0] = {5500, 18200}; + dataMap[-19.2] = {5500, 18000}; + dataMap[-18.0] = {5400, 17400}; + dataMap[-16.7] = {5400, 17000}; + dataMap[-15.4] = {4900, 15700}; + dataMap[-14.5] = {4600, 15500}; + dataMap[-12.6] = {4500, 14500}; + dataMap[-11.2] = {4400, 13800}; + dataMap[-9.7] = {4400, 13600}; + dataMap[-8.0] = {4200, 13500}; + dataMap[-6.2] = {4000, 13500}; + dataMap[-5.0] = {3600, 14000}; + dataMap[-2.5] = {3000, 13900}; + dataMap[-0.5] = {2400, 13400}; + dataMap[5.5] = {1900, 12900}; + dataMap[9.1] = {1700, 12900}; + dataMap[12.4] = {1500, 12800}; + dataMap[16.5] = {1300, 12700}; + dataMap[18.7] = {1200, 12500}; + + // dataMap[-19.4] = {5700, 17200}; + // dataMap[-18.9] = {5700, 16400}; + // dataMap[-17.5] = {5600, 15900}; + // dataMap[-14.8] = {5600, 14900}; + // dataMap[-12.5] = {5600, 14000}; + // dataMap[-9.5] = {5500, 13500}; + // dataMap[-7.4] = {5400, 13200}; + // dataMap[-5.4] = {5200, 12600}; + // dataMap[-1.8] = {4800, 12300}; + // dataMap[0.5] = {4700, 12200}; + // dataMap[1.7] = {4500, 12000}; + // dataMap[3.65] = {4100, 12000}; + // dataMap[6.0] = {3800, 11800}; + // dataMap[9.9] = {2800, 11700}; + // dataMap[14.2] = {2800, 11500}; + // dataMap[16.0] = {2600, 11000}; + // dataMap[19.88] = {1800, 10500}; m_hoodZero = false; m_turretZero = false; @@ -129,7 +129,8 @@ Shooter::Periodic(){ m_channel.setState(Channel::State::RUN); break; case State::Hood: - zeroHood(); + //commented this out to prevent going down to most down position + //zeroHood(); break; default: break; @@ -231,22 +232,26 @@ Shooter::Aim(){ frc::SmartDashboard::PutNumber("hood angle", m_hood.GetSelectedSensorPosition()); // Set Hood Position + //could alternatively do m_hood.Set(ControlMode::Position, std::min(ShooterConstants::hoodMax, std::max(ShooterConstants::hoodMin, m_angle))); if(m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ m_hood.Set(ControlMode::PercentOutput, 0.0); - return; + frc::SmartDashboard::PutString("Booted", "hoot got booted - zeroing current"); } else if(m_hood.GetSelectedSensorPosition() > ShooterConstants::hoodMax && - m_angle >= ShooterConstants::hoodMax){ - m_hood.Set(ControlMode::PercentOutput, 0.0); + m_hood.GetMotorOutputPercent() > 0){ + m_hood.Set(ControlMode::PercentOutput, 0.0); + frc::SmartDashboard::PutString("Booted", "hoot got booted - past hood max"); return; } else if(m_hood.GetSelectedSensorPosition() < ShooterConstants::hoodMin && - m_angle <= ShooterConstants::hoodMin){ + m_hood.GetMotorOutputPercent() < 0){ m_hood.Set(ControlMode::PercentOutput, 0.0); + frc::SmartDashboard::PutString("Booted", "hoot got booted - past hood min"); return; } else { m_hood.Set(ControlMode::Position, m_angle); + frc::SmartDashboard::PutString("Booted", "hood not booted"); } //frc::SmartDashboard::PutNumber("yOff", m_limelight->getYOff()); //frc::SmartDashboard::PutNumber("xOff", m_limelight->getXOff()); @@ -281,6 +286,29 @@ Shooter::EdgeofTarmac(){ m_hood.Set(ControlMode::PercentOutput, 0.0); } + // // Set Hood Position + // //could alternatively do m_hood.Set(ControlMode::Position, std::min(ShooterConstants::hoodMax, std::max(ShooterConstants::hoodMin, m_angle))); + // if(m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ + // m_hood.Set(ControlMode::PercentOutput, 0.0); + // frc::SmartDashboard::PutString("Booted", "hoot got booted - zeroing current"); + // } + // else if(m_hood.GetSelectedSensorPosition() > ShooterConstants::hoodMax && + // m_hood.GetMotorOutputPercent() > 0){ + // m_hood.Set(ControlMode::PercentOutput, 0.0); + // frc::SmartDashboard::PutString("Booted", "hoot got booted - past hood max"); + // return; + // } + // else if(m_hood.GetSelectedSensorPosition() < ShooterConstants::hoodMin && + // m_hood.GetMotorOutputPercent() < 0){ + // m_hood.Set(ControlMode::PercentOutput, 0.0); + // frc::SmartDashboard::PutString("Booted", "hoot got booted - past hood min"); + // return; + // } + // else { + // m_hood.Set(ControlMode::Position, m_tarmac_angle); + // frc::SmartDashboard::PutString("Booted", "hood not booted"); + // } + bool flywheelReady = abs(m_flywheelMaster.GetSelectedSensorVelocity() - m_tarmac_speed) < 400; bool hoodReady = abs(m_hood.GetSelectedSensorPosition() - m_tarmac_angle) < 100; // make this interval smaller // bool turretReady = abs(m_limelight->getXOff()+4.3) < 2.5; diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h index 6baf730..bce325a 100644 --- a/src/main/include/Shooter.h +++ b/src/main/include/Shooter.h @@ -52,7 +52,7 @@ class Shooter{ void zeroHood(); //For testing - void hoodGoTo511() {m_hood.Set(ControlMode::Position, 511);} + void hoodGoTo511() {m_hood.Set(ControlMode::Position, 511); } void hoodGoTo5700() {m_hood.Set(ControlMode::Position, 5700);} void hoodGoTo3000() {m_hood.Set(ControlMode::Position, 3000);} WPI_TalonFX * getHood() {return &m_hood; } From d9aaba7730392a0227d25c8280325196df2596ae Mon Sep 17 00:00:00 2001 From: SusanC3 Date: Thu, 31 Mar 2022 10:29:27 -0700 Subject: [PATCH 5/5] comment out interpolation, added return in hood logic --- src/main/cpp/Shooter.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp index 518c7f3..899b505 100644 --- a/src/main/cpp/Shooter.cpp +++ b/src/main/cpp/Shooter.cpp @@ -189,14 +189,14 @@ Shooter::Aim(){ speed2 = data2.second; // interpolate - - double interval = point2 - point1; - double xdiff = point - point1; - m_angle = (((angle2 - angle1)/interval * xdiff) + angle1) * angle_scale_factor; - m_speed = (((speed2 - speed1)/interval * xdiff) + speed1) * speed_scale_factor; + // double interval = point2 - point1; + // double xdiff = point - point1; + // m_angle = (((angle2 - angle1)/interval * xdiff) + angle1) * angle_scale_factor; + // m_speed = (((speed2 - speed1)/interval * xdiff) + speed1) * speed_scale_factor; // taking the average - - // m_angle = (angle1 + angle2)/2 * angle_scale_factor; - // m_speed = (speed1 + speed2)/2 * speed_scale_factor; + m_angle = (angle1 + angle2)/2 * angle_scale_factor; + m_speed = (speed1 + speed2)/2 * speed_scale_factor; }else{ m_angle = 0; @@ -236,6 +236,7 @@ Shooter::Aim(){ if(m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ m_hood.Set(ControlMode::PercentOutput, 0.0); frc::SmartDashboard::PutString("Booted", "hoot got booted - zeroing current"); + return; } else if(m_hood.GetSelectedSensorPosition() > ShooterConstants::hoodMax && m_hood.GetMotorOutputPercent() > 0){