Pid testing (#132)

* added the state raw and changed voltage * test * added the state raw and changed voltage * added pid calculate loop, fixed indenting * added calculate function on a new line, changed the PID to not stable * Fixed the if condition so shooterSensor is not tripped * changed the functions so they start with a capital * changed the value for shooterMotor * added shuffleboard and networking tables * added shuffleboard and networking tables * added network tables * Alpha Arm vers 1 * Alpha Arm ver 1 w/ explicit * Alpha arm v1 formatting * Alpha arm w/ wrist v1 * formatted * fixed merge conflicts and merge errors * fixed merge conflicts and merge errors * fixed merge conflicts and merge errors * fixed errors * ran wpiformat * Alpha arm v1 with ports * fixed merge errors and conflicts * fixed github errors * Working alpha arm no encoders * Alpha Bot Shooter * Swerve fix (#106) * fixed swerve * wrapping function * Shooter - Implements Networking tables (#99) * added the state raw and changed voltage * test * added the state raw and changed voltage * added pid calculate loop, fixed indenting * added calculate function on a new line, changed the PID to not stable * Fixed the if condition so shooterSensor is not tripped * changed the functions so they start with a capital * changed the value for shooterMotor * added shuffleboard and networking tables * added shuffleboard and networking tables * added network tables * fixed merge conflicts and merge errors * fixed merge conflicts and merge errors * fixed merge conflicts and merge errors * fixed errors * ran wpiformat * fixed merge errors and conflicts * fixed github errors * Alpha Bot Shooter --------- Co-authored-by: Havish12 <havishchopra10@gmail.com> Co-authored-by: Isaac Turner <spacey_sooty@outlook.com> * Intake (#100) * First version Tank Drive * Intake draft * Intake draft fixed errors 1 * Intake draft fixed errors 2 * Intake draft fixed errors 3 * Intake draft fixed errors 4 * Intake draft fixed errors 5 * Network files 1 * Network files 2 * Network files 3 * Almost done i think * finished wpiformat errors * finished wpiformat errors 2 * finished wpiformat errors 3 * finished wpiformat errors 4 * ran wpiformat * finished wpiformat errors 5 * finished wpiformat errors 8 * finished wpiformat errors 9 * finished wpiformat errors 10 * Tested on robot * Fixed github build errors after modifying during robot tests * fixed more github build errors 2 * fixed more github build errors 3 * Tested on robot and works --------- Co-authored-by: Isaac Turner <spacey_sooty@outlook.com> * Arm (#103) * Alpha Arm vers 1 * Alpha Arm ver 1 w/ explicit * Alpha arm v1 formatting * Alpha arm w/ wrist v1 * formatted * Alpha arm v1 with ports * Working alpha arm no encoders --------- Co-authored-by: Isaac Turner <spacey_sooty@outlook.com> * fix some formatting * fix more stuff * shooter with pid loop * Fixed errors * ':pensive:' * shortest path in 1 direction * Fixing pid loop control * fixing swerve * robot running well, field oriented, turning pretty good needs work in 1 direction i think it's fighting a PID loop, i put acceleration and speed limit all the way up be careful, also needs LOTS more current limiting * fixing pid control * turning and driving fixed swerve modules being ordered wrong * pid loop tuning * AlphaArm PID v1 probably unit issues * fixed PID control * Arm WomPID * ran wpiformat * ran wpiformat again; * fixed build * wpiformat * Shooter pid (#117) * added the state raw and changed voltage * test * added the state raw and changed voltage * added pid calculate loop, fixed indenting * added calculate function on a new line, changed the PID to not stable * Fixed the if condition so shooterSensor is not tripped * changed the functions so they start with a capital * changed the value for shooterMotor * added shuffleboard and networking tables * added shuffleboard and networking tables * added network tables * fixed merge conflicts and merge errors * fixed merge conflicts and merge errors * fixed merge conflicts and merge errors * fixed errors * ran wpiformat * fixed merge errors and conflicts * fixed github errors * Alpha Bot Shooter * shooter with pid loop * Fixed errors * Fixing pid loop control * fixing pid control * pid loop tuning * fixed PID control * ran wpiformat * ran wpiformat again; * fixed build * wpiformat --------- Co-authored-by: Havish12 <havishchopra10@gmail.com> Co-authored-by: Isaac Turner <spacey_sooty@outlook.com> * Intake - Manual/Auto fixes (#114) * First version Tank Drive * Intake draft * Intake draft fixed errors 1 * Intake draft fixed errors 2 * Intake draft fixed errors 3 * Intake draft fixed errors 4 * Intake draft fixed errors 5 * Network files 1 * Network files 2 * Network files 3 * Almost done i think * finished wpiformat errors * finished wpiformat errors 2 * finished wpiformat errors 3 * finished wpiformat errors 4 * ran wpiformat * finished wpiformat errors 5 * finished wpiformat errors 8 * finished wpiformat errors 9 * finished wpiformat errors 10 * Tested on robot * Fixed github build errors after modifying during robot tests * fixed more github build errors 2 * fixed more github build errors 3 * Tested on robot and works * added beam break * [docs] Update README.md (#107) * [docs] Update README.md * Update README.md * Added auto to intake * [ci] add comment command action (#111) * [ci] add comment command action * fix * Tested on robot * fixed intake manual auto, has not been tested on robot * fixed github build errors * Added intake better manual and verfied on robot! * Bump actions/setup-python from 4 to 5 (#116) Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](actions/setup-python@v4...v5) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] <support@github.com> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> * Bump actions/github-script from 6 to 7 (#115) Bumps [actions/github-script](https://github.com/actions/github-script) from 6 to 7. - [Release notes](https://github.com/actions/github-script/releases) - [Commits](actions/github-script@v6...v7) --- updated-dependencies: - dependency-name: actions/github-script dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] <support@github.com> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> * Fixed readme (#118) * ran wpiformat * fix * fix line endings * builds --------- Signed-off-by: dependabot[bot] <support@github.com> Co-authored-by: Kingsley Wong <kingsley.wong@student.education.wa.edu.au> Co-authored-by: Isaac Turner <spacey_sooty@outlook.com> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com> * arm with pid hold * going to try frc PID loop * Driver testing finished * beta bot swerve working and tested, subsystems not vibing --------- Signed-off-by: dependabot[bot] <support@github.com> Co-authored-by: Havish12 <havishchopra10@gmail.com> Co-authored-by: totallysomeoneyoudontknow <dev.renu@gmail.com> Co-authored-by: JoystickMaster-race <foxlocks179@gmail.com> Co-authored-by: Isaac Turner <spacey_sooty@outlook.com> Co-authored-by: JoystickMaster-race <97269195+JoystickMaster-race@users.noreply.github.com> Co-authored-by: totallysomeoneyoudontknow <155517980+totallysomeoneyoudontknow@users.noreply.github.com> Co-authored-by: kill-shots <155516223+kill-shots@users.noreply.github.com> Co-authored-by: prawny-boy <155516197+prawny-boy@users.noreply.github.com> Co-authored-by: Kingsley Wong <kingsley.wong@student.education.wa.edu.au> Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com>
CurtinFRC · Feb 10, 2024 · 5b51622 · 5b51622
1 parent 0d1300d
commit 5b51622
Show file tree

Hide file tree

Showing 41 changed files with 1,724 additions and 486 deletions.
diff --git a/.gitattributes b/.gitattributes
@@ -1,5 +1,6 @@
 * text=auto
 *.sh text eol=lf
+*.bat text eol=crlf
 *.gradle text eol=lf
 *.java text eol=lf
 *.json text eol=lf

diff --git a/.styleguide b/.styleguide
@@ -1,4 +1,3 @@
-
 cppHeaderFileInclude {
    \.h$
    \.hpp$

diff --git a/README.md b/README.md
@@ -42,7 +42,11 @@ Runs a simulation of your code at highest optimisation.
 
 **Debug**
 `./gradlew :simulateNativeDebug`
+<<<<<<< HEAD
+Runs a debug simulation of your code.
+=======
 Runs a debug simulation of your code, including a variety of debugging tools similar to glass but at lower optimisation.
+>>>>>>> 0d1300dafb5ad6f45b5d78b3668778de3bced1e8
 
 Documentation
 =============

diff --git a/gradlew.bat b/gradlew.bat
@@ -1,92 +1,92 @@
-@rem
-@rem Copyright 2015 the original author or authors.
-@rem
-@rem Licensed under the Apache License, Version 2.0 (the "License");
-@rem you may not use this file except in compliance with the License.
-@rem You may obtain a copy of the License at
-@rem
-@rem      https://www.apache.org/licenses/LICENSE-2.0
-@rem
-@rem Unless required by applicable law or agreed to in writing, software
-@rem distributed under the License is distributed on an "AS IS" BASIS,
-@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-@rem See the License for the specific language governing permissions and
-@rem limitations under the License.
-@rem
-
-@if "%DEBUG%"=="" @echo off
-@rem ##########################################################################
-@rem
-@rem  Gradle startup script for Windows
-@rem
-@rem ##########################################################################
-
-@rem Set local scope for the variables with windows NT shell
-if "%OS%"=="Windows_NT" setlocal
-
-set DIRNAME=%~dp0
-if "%DIRNAME%"=="" set DIRNAME=.
-@rem This is normally unused
-set APP_BASE_NAME=%~n0
-set APP_HOME=%DIRNAME%
-
-@rem Resolve any "." and ".." in APP_HOME to make it shorter.
-for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
-
-@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
-
-@rem Find java.exe
-if defined JAVA_HOME goto findJavaFromJavaHome
-
-set JAVA_EXE=java.exe
-%JAVA_EXE% -version >NUL 2>&1
-if %ERRORLEVEL% equ 0 goto execute
-
-echo. 1>&2
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
-echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation. 1>&2
-
-goto fail
-
-:findJavaFromJavaHome
-set JAVA_HOME=%JAVA_HOME:"=%
-set JAVA_EXE=%JAVA_HOME%/bin/java.exe
-
-if exist "%JAVA_EXE%" goto execute
-
-echo. 1>&2
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
-echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation. 1>&2
-
-goto fail
-
-:execute
-@rem Setup the command line
-
-set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
-
-
-@rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
-
-:end
-@rem End local scope for the variables with windows NT shell
-if %ERRORLEVEL% equ 0 goto mainEnd
-
-:fail
-rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
-rem the _cmd.exe /c_ return code!
-set EXIT_CODE=%ERRORLEVEL%
-if %EXIT_CODE% equ 0 set EXIT_CODE=1
-if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
-exit /b %EXIT_CODE%
-
-:mainEnd
-if "%OS%"=="Windows_NT" endlocal
-
-:omega
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
@@ -0,0 +1,113 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArm.h"
+//fiddle with these values
+AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config), _pidWom(_config.path + "/pid", config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/{}
+
+void AlphaArm::OnStart() {
+  started = false;
+  // _pidWom.Reset();
+  // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
+}
+
+void AlphaArm::OnUpdate(units::second_t dt){
+  _table->GetEntry("Error").SetDouble(_pidWom.GetError().value());
+  _table->GetEntry("Current Pos").SetDouble(_config.alphaArmGearbox.encoder->GetEncoderPosition().value());
+  _table->GetEntry("Setpoint").SetDouble(_pidWom.GetSetpoint().value());
+  _table->GetEntry("State ").SetString(_stateName);
+        switch(_state){
+          case AlphaArmState::kIdle:
+            _stateName = "Idle";
+            // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
+
+
+            _setAlphaArmVoltage = 0_V;
+
+          break;
+          case AlphaArmState::kRaw:
+            _stateName = "Raw";
+
+            _setAlphaArmVoltage = _rawArmVoltage;
+
+          break;
+          case AlphaArmState::kAmpAngle:
+          {
+            _stateName = "Amp Angle";
+
+            // _pidWom.SetSetpoint(_goal);
+            // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
+
+            if (started) {
+              if (_controlledRawVoltage.value() == 0) {
+                if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad/2))) {
+                    // _pidWom.SetSetpoint(_encoderSetpoint);
+                    // _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+                //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
+                // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) {
+                //   _pidWom.SetSetpoint(_encoderSetpoint);
+                //   _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+                //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
+                  _setAlphaArmVoltage = 0_V;
+                } else {
+                  _pidWom.SetSetpoint(_encoderSetpoint);
+                  _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+                //   _pidWom.Reset();
+                //   _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition();
+                //   _setAlphaArmVoltage = _controlledRawVoltage;
+                }
+              } else {
+                _pidWom.Reset();
+                _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+                _setAlphaArmVoltage = _controlledRawVoltage;
+              }
+            } else {
+              _pidWom.Reset();
+              _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+              _setAlphaArmVoltage = _controlledRawVoltage;
+
+              if (std::abs(_controlledRawVoltage.value()) > 0) {
+                _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+                started = true;
+              }
+            }
+
+          }
+            break;
+          case AlphaArmState::kSpeakerAngle:
+            _stateName = "Speaker Angle";
+            //_pidWom.SetSetpoint(_goal);
+           // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
+              break;
+          case AlphaArmState::kStowed:
+            _stateName = "Stowed";
+            //_pidWom.SetSetpoint(_goal);
+            //_setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
+          break;
+          default:
+            _stateName = "Error";
+            std::cout << "oops, wrong state" << std::endl;
+          break;
+        }
+        std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value() << std::endl;
+        //std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl;
+        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+}
+
+void AlphaArm::SetState(AlphaArmState state) {
+  _state = state;
+}
+
+void AlphaArm::SetGoal(units::radian_t goal){
+  _goal = goal;
+}
+
+void AlphaArm::SetArmRaw(units::volt_t voltage){
+  std::cout << "VOLTAGE: " << voltage.value() << std::endl;
+  _rawArmVoltage = voltage;
+}
+
+void AlphaArm::setControllerRaw(units::volt_t voltage) {
+  _controlledRawVoltage = voltage;
+}
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -0,0 +1,31 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArmBehaviour.h"
+
+#include <frc/XboxController.h>
+
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _codriver(codriver) {
+  Controls(alphaArm);
+}
+
+void AlphaArmManualControl::OnTick(units::second_t dt) {
+
+  if (_codriver->GetStartButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl) {
+    _alphaArm->SetState(AlphaArmState::kRaw);
+    _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);  
+  } else {
+    _alphaArm->SetState(AlphaArmState::kAmpAngle);
+    _alphaArm->setControllerRaw(wom::deadzone(_codriver->GetRightY()) * 12_V);
+  }
+}
-Original file line number
+Diff line change
@@ -1,4 +1,3 @@
     cppHeaderFileInclude {
        \.h$
        \.hpp$
@@ Expand Down @@