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..52caeb7 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() <SetSelectedSensorPosition(ShooterConstants::hoodInitAngle); + m_shooter.Zero(); } @@ -230,6 +232,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..899b505 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; @@ -127,8 +127,11 @@ Shooter::Periodic(){ EdgeofTarmac(); // Shoot(); 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; } @@ -186,14 +189,14 @@ Shooter::Aim(){ speed2 = data2.second; // interpolate - - double interval = point1 - point2; - 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; @@ -229,11 +232,28 @@ 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); - // } + //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"); + return; + } + 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_angle); + frc::SmartDashboard::PutString("Booted", "hood not booted"); + } //frc::SmartDashboard::PutNumber("yOff", m_limelight->getYOff()); //frc::SmartDashboard::PutNumber("xOff", m_limelight->getXOff()); } @@ -267,6 +287,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; @@ -324,7 +367,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 +385,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..bce325a 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