├── .floo ├── .flooignore ├── .gitattributes ├── .gitignore ├── .idea └── runConfigurations │ ├── Build_All.xml │ ├── Build_Robot.xml │ ├── Build_Vision.xml │ ├── Debug_Robot.xml │ ├── Debug_Vision.xml │ ├── Deploy_All.xml │ ├── Deploy_Motion_Profiles.xml │ ├── Deploy_Robot.xml │ ├── Deploy_Robot_Debug.xml │ ├── Deploy_Vision.xml │ └── Deploy_Vision_Debug.xml ├── .travis.yml ├── LICENSE ├── README.md ├── build.gradle ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── gradlew ├── gradlew.bat ├── robot ├── build.gradle ├── profiles │ ├── BACK_UP_C.csv │ ├── BACK_UP_L.csv │ ├── BACK_UP_R.csv │ ├── BASELINE_C.csv │ ├── BASELINE_L.csv │ ├── BASELINE_R.csv │ ├── DS_CENTER-SWITCH_LEFT_C.csv │ ├── DS_CENTER-SWITCH_LEFT_L.csv │ ├── DS_CENTER-SWITCH_LEFT_R.csv │ ├── DS_CENTER-SWITCH_RIGHT_C.csv │ ├── DS_CENTER-SWITCH_RIGHT_L.csv │ ├── DS_CENTER-SWITCH_RIGHT_R.csv │ ├── DS_LEFT-SCALE_LEFT_C.csv │ ├── DS_LEFT-SCALE_LEFT_L.csv │ ├── DS_LEFT-SCALE_LEFT_R.csv │ ├── DS_LEFT-SCALE_RIGHT_C.csv │ ├── DS_LEFT-SCALE_RIGHT_L.csv │ ├── DS_LEFT-SCALE_RIGHT_R.csv │ ├── DS_LEFT-SWITCH_LEFT_C.csv │ ├── DS_LEFT-SWITCH_LEFT_L.csv │ ├── DS_LEFT-SWITCH_LEFT_R.csv │ ├── DS_RIGHT-SCALE_LEFT_C.csv │ ├── DS_RIGHT-SCALE_LEFT_L.csv │ ├── DS_RIGHT-SCALE_LEFT_R.csv │ ├── DS_RIGHT-SCALE_RIGHT_C.csv │ ├── DS_RIGHT-SCALE_RIGHT_L.csv │ ├── DS_RIGHT-SCALE_RIGHT_R.csv │ ├── DS_RIGHT-SWITCH_RIGHT_C.csv │ ├── DS_RIGHT-SWITCH_RIGHT_L.csv │ ├── DS_RIGHT-SWITCH_RIGHT_R.csv │ ├── SCALE_LEFT-SCALE_LEFT_TWOCUBE1_C.csv │ ├── SCALE_LEFT-SCALE_LEFT_TWOCUBE1_L.csv │ ├── SCALE_LEFT-SCALE_LEFT_TWOCUBE1_R.csv │ ├── SCALE_LEFT_TWOCUBE1-SCALE_LEFT_TWOCUBE2_C.csv │ ├── SCALE_LEFT_TWOCUBE1-SCALE_LEFT_TWOCUBE2_L.csv │ ├── SCALE_LEFT_TWOCUBE1-SCALE_LEFT_TWOCUBE2_R.csv │ ├── SCALE_LEFT_TWOCUBE2-SCALE_LEFT_TWOCUBE3_C.csv │ ├── SCALE_LEFT_TWOCUBE2-SCALE_LEFT_TWOCUBE3_L.csv │ ├── SCALE_LEFT_TWOCUBE2-SCALE_LEFT_TWOCUBE3_R.csv │ ├── SCALE_LEFT_TWOCUBE3-SCALE_LEFT_C.csv │ ├── SCALE_LEFT_TWOCUBE3-SCALE_LEFT_L.csv │ ├── SCALE_LEFT_TWOCUBE3-SCALE_LEFT_R.csv │ ├── SCALE_RIGHT-SCALE_RIGHT_TWOCUBE1_C.csv │ ├── SCALE_RIGHT-SCALE_RIGHT_TWOCUBE1_L.csv │ ├── SCALE_RIGHT-SCALE_RIGHT_TWOCUBE1_R.csv │ ├── SCALE_RIGHT_TWOCUBE1-SCALE_RIGHT_TWOCUBE2_C.csv │ ├── SCALE_RIGHT_TWOCUBE1-SCALE_RIGHT_TWOCUBE2_L.csv │ ├── SCALE_RIGHT_TWOCUBE1-SCALE_RIGHT_TWOCUBE2_R.csv │ ├── SCALE_RIGHT_TWOCUBE2-SCALE_RIGHT_TWOCUBE3_C.csv │ ├── SCALE_RIGHT_TWOCUBE2-SCALE_RIGHT_TWOCUBE3_L.csv │ ├── SCALE_RIGHT_TWOCUBE2-SCALE_RIGHT_TWOCUBE3_R.csv │ ├── SCALE_RIGHT_TWOCUBE3-SCALE_RIGHT_C.csv │ ├── SCALE_RIGHT_TWOCUBE3-SCALE_RIGHT_L.csv │ ├── SCALE_RIGHT_TWOCUBE3-SCALE_RIGHT_R.csv │ ├── SWITCH_LEFT-SWITCH_LEFT_TWOCUBE1_C.csv │ ├── SWITCH_LEFT-SWITCH_LEFT_TWOCUBE1_L.csv │ ├── SWITCH_LEFT-SWITCH_LEFT_TWOCUBE1_R.csv │ ├── SWITCH_LEFT_TWOCUBE1-SWITCH_LEFT_TWOCUBE2_C.csv │ ├── SWITCH_LEFT_TWOCUBE1-SWITCH_LEFT_TWOCUBE2_L.csv │ ├── SWITCH_LEFT_TWOCUBE1-SWITCH_LEFT_TWOCUBE2_R.csv │ ├── SWITCH_LEFT_TWOCUBE2-SWITCH_LEFT_TWOCUBE3_C.csv │ ├── SWITCH_LEFT_TWOCUBE2-SWITCH_LEFT_TWOCUBE3_L.csv │ ├── SWITCH_LEFT_TWOCUBE2-SWITCH_LEFT_TWOCUBE3_R.csv │ ├── SWITCH_LEFT_TWOCUBE3-SWITCH_LEFT_C.csv │ ├── SWITCH_LEFT_TWOCUBE3-SWITCH_LEFT_L.csv │ ├── SWITCH_LEFT_TWOCUBE3-SWITCH_LEFT_R.csv │ ├── SWITCH_RIGHT-SWITCH_RIGHT_TWOCUBE1_C.csv │ ├── SWITCH_RIGHT-SWITCH_RIGHT_TWOCUBE1_L.csv │ ├── SWITCH_RIGHT-SWITCH_RIGHT_TWOCUBE1_R.csv │ ├── SWITCH_RIGHT_TWOCUBE1-SWITCH_RIGHT_TWOCUBE2_C.csv │ ├── SWITCH_RIGHT_TWOCUBE1-SWITCH_RIGHT_TWOCUBE2_L.csv │ ├── SWITCH_RIGHT_TWOCUBE1-SWITCH_RIGHT_TWOCUBE2_R.csv │ ├── SWITCH_RIGHT_TWOCUBE2-SWITCH_RIGHT_TWOCUBE3_C.csv │ ├── SWITCH_RIGHT_TWOCUBE2-SWITCH_RIGHT_TWOCUBE3_L.csv │ ├── SWITCH_RIGHT_TWOCUBE2-SWITCH_RIGHT_TWOCUBE3_R.csv │ ├── SWITCH_RIGHT_TWOCUBE3-SWITCH_RIGHT_C.csv │ ├── SWITCH_RIGHT_TWOCUBE3-SWITCH_RIGHT_L.csv │ └── SWITCH_RIGHT_TWOCUBE3-SWITCH_RIGHT_R.csv ├── src │ └── main │ │ └── kotlin │ │ └── org │ │ └── team401 │ │ └── robot2018 │ │ ├── Controls.kt │ │ ├── Sensors.kt │ │ ├── Setup.kt │ │ ├── auto │ │ ├── AutoDelays.kt │ │ ├── AutoTarget.kt │ │ ├── Commands.kt │ │ ├── FieldElements.kt │ │ ├── HeadingTracker.kt │ │ ├── PowerUpAuto.kt │ │ ├── RobotAuto.kt │ │ ├── RobotPosition.kt │ │ ├── Routines.kt │ │ ├── TestAuto.kt │ │ ├── motion │ │ │ ├── DriveController.kt │ │ │ ├── DriveGains.kt │ │ │ ├── HeadingController.kt │ │ │ ├── MotionProfileRunner.kt │ │ │ ├── PDVA.kt │ │ │ ├── ProfileLoader.kt │ │ │ ├── RioProfileRunner.kt │ │ │ ├── TankMotionStep.kt │ │ │ ├── TuningRioProfileRunner.kt │ │ │ └── Waypoint.kt │ │ ├── motionprofile │ │ │ ├── ArcProfileFollower.kt │ │ │ ├── HeadingAdapter.kt │ │ │ ├── MotionProfile.kt │ │ │ ├── ProfileLoader.kt │ │ │ ├── TuningArcProfileFollower.kt │ │ │ ├── Waypoint.kt │ │ │ └── ZeroPoint.kt │ │ ├── steps │ │ │ ├── AutoStep.kt │ │ │ ├── DelayStep.kt │ │ │ ├── IAutoStep.kt │ │ │ ├── LambdaStep.kt │ │ │ ├── ParallelSteps.kt │ │ │ ├── SequentialSteps.kt │ │ │ ├── SingleStep.kt │ │ │ ├── StateStep.kt │ │ │ └── WaitForStep.kt │ │ └── supermotion │ │ │ ├── MotionMath.kt │ │ │ └── transformation │ │ │ ├── Interpolatable.kt │ │ │ ├── RigidTransform.kt │ │ │ ├── Rotation.kt │ │ │ ├── TransformationCompanion.kt │ │ │ ├── Translation.kt │ │ │ └── Twist.kt │ │ ├── constants │ │ ├── CompConstants.kt │ │ ├── Constants.kt │ │ ├── ConstantsBase.kt │ │ └── PracticeConstants.kt │ │ ├── etc │ │ ├── Extensions.kt │ │ ├── LED.kt │ │ ├── Reporting.kt │ │ ├── RobotEvents.kt │ │ ├── RobotMath.kt │ │ └── TalonEnums.kt │ │ ├── subsystems │ │ ├── Drivetrain.kt │ │ ├── Elevator.kt │ │ ├── Intake.kt │ │ └── Rungs.kt │ │ └── vision │ │ ├── CubeDetectorPipeline.kt │ │ ├── MjpegServer.kt │ │ ├── VisionController.kt │ │ ├── VisionData.kt │ │ └── VisionDataClient.kt └── waypoints │ ├── BACK_UP_REV.csv │ ├── BASELINE.csv │ ├── DS_CENTER-SWITCH_LEFT.csv │ ├── DS_CENTER-SWITCH_RIGHT.csv │ ├── DS_LEFT-SCALE_LEFT.csv │ ├── DS_LEFT-SCALE_RIGHT.csv │ ├── DS_LEFT-SWITCH_LEFT.csv │ ├── DS_RIGHT-SCALE_LEFT.csv │ ├── DS_RIGHT-SCALE_RIGHT.csv │ ├── DS_RIGHT-SWITCH_RIGHT.csv │ ├── SCALE_LEFT-SCALE_LEFT_TWOCUBE1_REV.csv │ ├── SCALE_LEFT_TWOCUBE1-SCALE_LEFT_TWOCUBE2.csv │ ├── SCALE_LEFT_TWOCUBE2-SCALE_LEFT_TWOCUBE3_REV.csv │ ├── SCALE_LEFT_TWOCUBE3-SCALE_LEFT.csv │ ├── SCALE_RIGHT-SCALE_RIGHT_TWOCUBE1_REV.csv │ ├── SCALE_RIGHT_TWOCUBE1-SCALE_RIGHT_TWOCUBE2.csv │ ├── SCALE_RIGHT_TWOCUBE2-SCALE_RIGHT_TWOCUBE3_REV.csv │ ├── SCALE_RIGHT_TWOCUBE3-SCALE_RIGHT.csv │ ├── SWITCH_LEFT-SWITCH_LEFT_TWOCUBE1_REV.csv │ ├── SWITCH_LEFT_TWOCUBE1-SWITCH_LEFT_TWOCUBE2.csv │ ├── SWITCH_LEFT_TWOCUBE2-SWITCH_LEFT_TWOCUBE3_REV.csv │ ├── SWITCH_LEFT_TWOCUBE3-SWITCH_LEFT.csv │ ├── SWITCH_RIGHT-SWITCH_RIGHT_TWOCUBE1_REV.csv │ ├── SWITCH_RIGHT_TWOCUBE1-SWITCH_RIGHT_TWOCUBE2.csv │ ├── SWITCH_RIGHT_TWOCUBE2-SWITCH_RIGHT_TWOCUBE3_REV.csv │ └── SWITCH_RIGHT_TWOCUBE3-SWITCH_RIGHT.csv ├── settings.gradle └── vision ├── build.gradle ├── libs └── opencv-340.jar ├── scripts ├── debug.sh ├── overclock.sh ├── start.sh └── stop.sh └── src └── main └── kotlin └── org └── team401 └── vision2018 ├── ControllerServer.kt ├── CubeFinderPipeline.kt ├── Extensions.kt ├── VideoRecorder.kt ├── Vision.kt ├── VisionData.kt ├── VisionDataServer.kt └── VisionParameters.kt /.floo: -------------------------------------------------------------------------------- 1 | { 2 | "url": "https://floobits.com/team401/2018-Robot-Code" 3 | } -------------------------------------------------------------------------------- /.flooignore: -------------------------------------------------------------------------------- 1 | extern 2 | node_modules 3 | tmp 4 | vendor 5 | .idea/workspace.xml 6 | .idea/misc.xml 7 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | vision/scripts/* linguist-vendored -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Eclipse 3 | .classpath 4 | .project 5 | .settings/ 6 | 7 | # IntelliJ 8 | .idea/* 9 | !.idea/runConfigurations 10 | out/ 11 | *.iml 12 | *.iws 13 | *.ipr 14 | 15 | # Mac 16 | .DS_Store 17 | 18 | # Maven 19 | log/ 20 | target/ 21 | 22 | # Gradle 23 | .gradle/ 24 | build/ 25 | 26 | # Gource 27 | grav.perl 28 | 29 | # Add other files here: 30 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Build_All.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 16 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Build_Robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 16 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Build_Vision.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 16 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Debug_Robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 15 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Debug_Vision.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 10 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Deploy_All.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 17 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Deploy_Motion_Profiles.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 16 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Deploy_Robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 17 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Deploy_Robot_Debug.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 17 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Deploy_Vision.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 17 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /.idea/runConfigurations/Deploy_Vision_Debug.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 17 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: java 2 | before_install: 3 | - chmod +x gradlew 4 | before_cache: 5 | - rm -f $HOME/.gradle/caches/modules-2/modules-2.lock 6 | - rm -fr $HOME/.gradle/caches/*/plugin-resolution/ 7 | cache: 8 | directories: 9 | - $HOME/.gradle/caches/ 10 | - $HOME/.gradle/wrapper/ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 2018-Robot-Code 2 | 3 | [![Language](https://img.shields.io/github/languages/top/team401/2018-Robot-Code.svg)](https://github.com/team401/2018-Robot-Code) 4 | [![Waffle.io - Columns and their card count](https://badge.waffle.io/team401/2018-Robot-Code.svg?columns=all)](https://waffle.io/team401/2018-Robot-Code) 5 | [![License](https://img.shields.io/github/license/team401/2018-Robot-Code.svg)](https://github.com/team401/2018-Robot-Code/blob/master/LICENSE) 6 | [![CI](https://api.travis-ci.org/team401/2018-Robot-Code.svg?branch=master)](https://travis-ci.org/team401/2018-Robot-Code) 7 | 8 | Code for Team 401's robot for the 2018 *FIRST* Robotics Competition game Power Up. 9 | 10 | ## General Features: 11 | * Multi-project using [GradleRIO](https://github.com/Open-RIO/GradleRIO) / [EmbeddedTools](https://github.com/JacisNonsense/EmbeddedTools) to allow unified deploys to roboRIO and coprocessor. 12 | * Task tracking and organization using [Waffle.io](https://waffle.io/team401/2018-Robot-Code) 13 | * [Bintray releases](https://bintray.com/team401) for software modules. 14 | * Version controlled [IntelliJ Run Configurations](.idea/runConfigurations) to standardize deploy tasks between all programmers. 15 | * Kotlin used for all robot and coprocessor code 16 | 17 | 18 | 19 | ## Robot Features: 20 | * [Modular domain specific language](https://github.com/team401/SnakeSkin) to make robot code development faster and easier. 21 | * [State machine based subsystems](robot/src/main/kotlin/org/team401/robot2018/subsystems/Rungs.kt) with optional rejection conditions and entry, exit, and loop behaviors for each state. 22 | * Motion profiling with [Processing based GUI](https://github.com/team401/MP-Generator) and trajectories generated by [Pathfinder](https://github.com/JacisNonsense/Pathfinder). 23 | * Motion magic controlled [elevator system](robot/src/main/kotlin/org/team401/robot2018/subsystems/Elevator.kt) with manual adjustment 24 | * Motorized [virtual four bar intake](robot/src/main/kotlin/org/team401/robot2018/subsystems/Intake.kt) with positional PID control to add 'springs' to the system. Current monitoring to detect power cube acquisition. 25 | * Custom [two-speed 775pro drive](robot/src/main/kotlin/org/team401/robot2018/subsystems/Drivetrain.kt) with autoshifting and anti-tip correction. 26 | * '[Virtual sensors](robot/src/main/kotlin/org/team401/robot2018/Sensors.kt)' for controlling LEDs and coprocessors. 27 | 28 | ## Coprocessor Features: 29 | * [SnakeEyes vision library](https://github.com/team401/SnakeEyes) providing custom tiling picture-in-picture multi-camera views, side-by-side views, and targeting crosshairs 30 | * [Automatic recording of match videos](vision/src/main/kotlin/org/team401/vision2018/VideoRecorder.kt) with footage automatically transferred to external storage device. 31 | -------------------------------------------------------------------------------- /build.gradle: -------------------------------------------------------------------------------- 1 | plugins { 2 | id "jaci.openrio.gradle.GradleRIO" version "2018.02.17" apply false 3 | id "jaci.gradle.EmbeddedTools" version "2018.02.04" apply false 4 | id "org.jetbrains.kotlin.jvm" version "1.2.20" apply false 5 | } 6 | 7 | allprojects { 8 | apply plugin: "idea" 9 | } 10 | 11 | subprojects { 12 | apply plugin: "java" 13 | apply plugin: "kotlin" 14 | 15 | group 'org.team401' 16 | 17 | repositories { 18 | mavenCentral() 19 | 20 | maven { 21 | url "http://dl.bintray.com/team401/SnakeSkin" 22 | } 23 | maven { 24 | url "http://dl.bintray.com/team401/Vision" 25 | } 26 | } 27 | 28 | dependencies { 29 | compile "org.jetbrains.kotlin:kotlin-stdlib-jdk8:1.2.20" 30 | } 31 | 32 | sourceCompatibility = 1.8 33 | 34 | compileKotlin { 35 | kotlinOptions.jvmTarget = "1.8" 36 | } 37 | compileTestKotlin { 38 | kotlinOptions.jvmTarget = "1.8" 39 | } 40 | } 41 | 42 | task wrapper(type: Wrapper) { 43 | gradleVersion = '4.4' 44 | } -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/team401/2018-Robot-Code/58788e93e924ab0c77252ab3af659be5cd4f7882/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | #Fri Jan 05 13:30:22 EST 2018 2 | distributionBase=GRADLE_USER_HOME 3 | distributionPath=wrapper/dists 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=wrapper/dists 6 | distributionUrl=https\://services.gradle.org/distributions/gradle-4.4-all.zip 7 | -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @if "%DEBUG%" == "" @echo off 2 | @rem ########################################################################## 3 | @rem 4 | @rem Gradle startup script for Windows 5 | @rem 6 | @rem ########################################################################## 7 | 8 | @rem Set local scope for the variables with windows NT shell 9 | if "%OS%"=="Windows_NT" setlocal 10 | 11 | set DIRNAME=%~dp0 12 | if "%DIRNAME%" == "" set DIRNAME=. 13 | set APP_BASE_NAME=%~n0 14 | set APP_HOME=%DIRNAME% 15 | 16 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 17 | set DEFAULT_JVM_OPTS= 18 | 19 | @rem Find java.exe 20 | if defined JAVA_HOME goto findJavaFromJavaHome 21 | 22 | set JAVA_EXE=java.exe 23 | %JAVA_EXE% -version >NUL 2>&1 24 | if "%ERRORLEVEL%" == "0" goto init 25 | 26 | echo. 27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 28 | echo. 29 | echo Please set the JAVA_HOME variable in your environment to match the 30 | echo location of your Java installation. 31 | 32 | goto fail 33 | 34 | :findJavaFromJavaHome 35 | set JAVA_HOME=%JAVA_HOME:"=% 36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 37 | 38 | if exist "%JAVA_EXE%" goto init 39 | 40 | echo. 41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 42 | echo. 43 | echo Please set the JAVA_HOME variable in your environment to match the 44 | echo location of your Java installation. 45 | 46 | goto fail 47 | 48 | :init 49 | @rem Get command-line arguments, handling Windows variants 50 | 51 | if not "%OS%" == "Windows_NT" goto win9xME_args 52 | 53 | :win9xME_args 54 | @rem Slurp the command line arguments. 55 | set CMD_LINE_ARGS= 56 | set _SKIP=2 57 | 58 | :win9xME_args_slurp 59 | if "x%~1" == "x" goto execute 60 | 61 | set CMD_LINE_ARGS=%* 62 | 63 | :execute 64 | @rem Setup the command line 65 | 66 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 67 | 68 | @rem Execute Gradle 69 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 70 | 71 | :end 72 | @rem End local scope for the variables with windows NT shell 73 | if "%ERRORLEVEL%"=="0" goto mainEnd 74 | 75 | :fail 76 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 77 | rem the _cmd.exe /c_ return code! 78 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 79 | exit /b 1 80 | 81 | :mainEnd 82 | if "%OS%"=="Windows_NT" endlocal 83 | 84 | :omega 85 | -------------------------------------------------------------------------------- /robot/build.gradle: -------------------------------------------------------------------------------- 1 | apply plugin: "jaci.openrio.gradle.GradleRIO" 2 | 3 | def SNAKESKIN_VERSION = "1.2.7a" 4 | def SNAKESKIN_MAIN = "org.snakeskin.Robot" 5 | 6 | def TEAM_NUMBER = 401 7 | 8 | deploy { 9 | targets { 10 | target("rio", jaci.openrio.gradle.frc.RoboRIO) { 11 | team = TEAM_NUMBER 12 | } 13 | } 14 | artifacts { 15 | artifact("robot2018", jaci.openrio.gradle.frc.FRCJavaArtifact) { 16 | targets << "rio" 17 | debug = Boolean.parseBoolean(System.getProperty("deployDebug") ?: "false") 18 | jar = "jar" 19 | } 20 | 21 | fileCollectionArtifact("profiles") { 22 | targets << "rio" 23 | directory = "profiles" 24 | files = fileTree(dir: "profiles") 25 | } 26 | } 27 | } 28 | 29 | wpi { 30 | wpilibVersion = '2018.3.3' 31 | ctreVersion = '5.3.1.0' 32 | } 33 | 34 | dependencies { 35 | compile wpilib() 36 | compile ctre() 37 | 38 | compile openrio.powerup.matchData() 39 | 40 | compile group: "org.snakeskin", name: "SnakeSkin-Core", version: SNAKESKIN_VERSION 41 | compile group: "org.snakeskin", name: "SnakeSkin-FRC", version: SNAKESKIN_VERSION 42 | compile group: "org.snakeskin", name: "SnakeSkin-CTRE", version: SNAKESKIN_VERSION 43 | compile group: "org.team401", name: "lightlink", version: "1.0" 44 | 45 | compile group: 'org.zeromq', name: 'jeromq', version: '0.4.3' 46 | } 47 | 48 | jar { 49 | from configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } 50 | manifest jaci.openrio.gradle.GradleRIOPlugin.javaManifest(SNAKESKIN_MAIN) 51 | } -------------------------------------------------------------------------------- /robot/profiles/BACK_UP_C.csv: -------------------------------------------------------------------------------- 1 | -0.008089910099842616,-0.8089910099842615,20,40.449550499213075,90.0 2 | -0.04044955049921308,-2.4269730299527845,20,80.89910099842615,90.0 3 | -0.105168831297954,-4.044955049921308,20,80.89910099842616,90.0 4 | -0.20224775249606539,-5.66293706988983,20,80.89910099842614,90.0 5 | -0.3316863140935472,-7.280919089858354,20,80.89910099842618,90.0 6 | -0.4934845160903995,-8.898901109826877,20,80.89910099842618,90.0 7 | -0.6876423584866224,-10.5168831297954,20,80.89910099842614,90.0 8 | -0.9141598412822156,-12.134865149763923,20,80.89910099842618,90.0 9 | -1.1730369644771794,-13.752847169732446,20,80.89910099842618,90.0 10 | -1.4642737280715137,-15.37082918970097,20,80.89910099842618,90.0 11 | -1.7878701320652184,-16.988811209669493,20,80.89910099842618,90.0 12 | -2.1438261764582935,-18.606793229638015,20,80.89910099842606,90.0 13 | -2.532141861250739,-20.22477524960654,20,80.89910099842629,90.0 14 | -2.9528171864425548,-21.842757269575063,20,80.89910099842606,90.0 15 | -3.4058521520337415,-23.46073928954359,20,80.89910099842629,90.0 16 | -3.8912467580242986,-25.078721309512108,20,80.89910099842606,90.0 17 | -4.409001004414226,-26.696703329480634,20,80.89910099842629,90.0 18 | -4.959114891203524,-28.314685349449157,20,80.89910099842606,90.0 19 | -5.541588418392193,-29.93266736941768,20,80.89910099842606,90.0 20 | -6.156421585980231,-31.550649389386198,20,80.89910099842606,90.0 21 | -6.80361439396764,-33.168631409354724,20,80.89910099842629,90.0 22 | -7.48316684235442,-34.786613429323246,20,80.89910099842606,90.0 23 | -8.19507893114057,-36.40459544929177,20,80.89910099842629,90.0 24 | -8.939350660326092,-38.02257746926029,20,80.89910099842606,90.0 25 | -9.715982029910982,-39.64055948922882,20,80.89910099842629,90.0 26 | -10.524973039895244,-41.258541509197336,20,80.89910099842585,90.0 27 | -11.366323690278874,-42.87652352916586,20,80.89910099842629,90.0 28 | -12.227651037052363,-43.25621114818295,20,18.98438095085433,90.0 29 | -13.080392316006508,-42.01791674723151,20,-61.914720047571954,90.0 30 | -13.904570830751453,-40.39993472726298,20,-80.89910099842629,90.0 31 | -14.696389705097026,-38.781952707294465,20,-80.89910099842606,90.0 32 | -15.45584893904323,-37.163970687325936,20,-80.89910099842629,90.0 33 | -16.182948532590064,-35.54598866735741,20,-80.89910099842606,90.0 34 | -16.877688485737526,-33.92800664738889,20,-80.89910099842629,90.0 35 | -17.54006879848562,-32.31002462742037,20,-80.89910099842606,90.0 36 | -18.17008947083434,-30.692042607451842,20,-80.89910099842629,90.0 37 | -18.767750502783695,-29.07406058748332,20,-80.89910099842606,90.0 38 | -19.333051894333675,-27.456078567514794,20,-80.89910099842629,90.0 39 | -19.865993645484288,-25.838096547546275,20,-80.89910099842606,90.0 40 | -20.36657575623553,-24.22011452757775,20,-80.89910099842629,90.0 41 | -20.8347982265874,-22.60213250760923,20,-80.89910099842585,90.0 42 | -21.2706610565399,-20.984150487640704,20,-80.89910099842629,90.0 43 | -21.67416424609303,-19.366168467672182,20,-80.89910099842618,90.0 44 | -22.045307795246792,-17.74818644770366,20,-80.89910099842618,90.0 45 | -22.38409170400118,-16.130204427735134,20,-80.89910099842618,90.0 46 | -22.6905159723562,-14.512222407766611,20,-80.89910099842618,90.0 47 | -22.964580600311848,-12.89424038779809,20,-80.89910099842606,90.0 48 | -23.206285587868127,-11.276258367829566,20,-80.89910099842618,90.0 49 | -23.415630935025035,-9.658276347861042,20,-80.89910099842618,90.0 50 | -23.592616641782573,-8.04029432789252,20,-80.89910099842614,90.0 51 | -23.737242708140734,-6.422312307923997,20,-80.89910099842618,90.0 52 | -23.849509134099527,-4.804330287955474,20,-80.89910099842615,90.0 53 | -23.92941591965895,-3.1863482679869506,20,-80.89910099842615,90.0 54 | -23.976963064819003,-1.5683662480184275,20,-80.89910099842616,90.0 55 | -23.996443603489357,-0.37968761901708303,20,-59.43393145006723,90.0 56 | -24.00024047967953,-0.0,20,-18.984380950854153,90.0 57 | -24.00024047967953,-0.0,20,0.0,90.0 58 | -------------------------------------------------------------------------------- /robot/profiles/BACK_UP_L.csv: -------------------------------------------------------------------------------- 1 | -0.008089910099842616,-0.8089910099842615,20,40.449550499213075,90.0 2 | -0.04044955049921733,-1.6179820199687356,20,40.449550499223704,90.0 3 | -0.10516883129794927,-3.2359640399365968,20,80.89910099839307,90.0 4 | -0.2022477524960734,-4.853946059906207,20,80.89910099848049,90.0 5 | -0.3316863140935547,-6.4719280798740675,20,80.89910099839307,90.0 6 | -0.4934845160904108,-8.089910099842804,20,80.89910099843678,90.0 7 | -0.6876423584866416,-9.70789211981154,20,80.89910099843678,90.0 8 | -0.9141598412821946,-11.325874139777651,20,80.89910099830564,90.0 9 | -1.17303696447707,-12.943856159743765,20,80.89910099830564,90.0 10 | -1.4642737280713198,-14.5618381797125,20,80.89910099843678,90.0 11 | -1.7878701320651196,-16.179820199689978,20,80.89910099887388,90.0 12 | -2.1438261764583464,-17.797802219661335,20,80.8991009985679,90.0 13 | -2.5321418612509303,-19.415784239629197,20,80.89910099839307,90.0 14 | -2.952817186442924,-21.03376625959968,20,80.89910099852419,90.0 15 | -3.4058521520337153,-22.651748279539568,20,80.89910099699436,90.0 16 | -3.8912467580237937,-24.269730299503934,20,80.89910099821823,90.0 17 | -4.409001004413195,-25.887712319470047,20,80.89910099830564,90.0 18 | -4.959114891201953,-27.50569433943791,20,80.89910099839307,90.0 19 | -5.541588418390034,-29.12367635940402,20,80.89910099830564,90.0 20 | -6.156421585977453,-30.741658379371007,20,80.89910099834935,90.0 21 | -6.803614393964213,-32.359640399338,20,80.89910099834935,90.0 22 | -7.483166842350313,-33.97762241930498,20,80.89910099834935,90.0 23 | -8.195078931135752,-35.595604439271966,20,80.89910099834935,90.0 24 | -8.939350660320532,-37.213586459238954,20,80.89910099834935,90.0 25 | -9.71598202990465,-38.83156847920594,20,80.89910099834935,90.0 26 | -10.524973039888092,-40.449550499172055,20,80.89910099830564,90.0 27 | -11.36632369027089,-42.06753251913992,20,80.89910099839307,90.0 28 | -12.22765103704478,-43.066367338694434,20,49.941740977725985,90.0 29 | -13.0803923160028,-42.637063947901076,20,-21.46516953966804,90.0 30 | -13.904570830751505,-41.20892573743518,20,-71.40691052329481,90.0 31 | -14.696389705100676,-39.59094371745858,20,-80.89910099883016,90.0 32 | -15.455848939050334,-37.972961697482845,20,-80.89910099878645,90.0 33 | -16.182948532600477,-36.35497967750712,20,-80.89910099878645,90.0 34 | -16.877688485751104,-34.73699765753139,20,-80.89910099878645,90.0 35 | -17.540068798502215,-33.11901563755566,20,-80.89910099878645,90.0 36 | -18.1700894708538,-31.501033617579058,20,-80.89910099883016,90.0 37 | -18.76775050280588,-29.8830515976042,20,-80.89910099874274,90.0 38 | -19.333051894358434,-28.2650695776276,20,-80.89910099883016,90.0 39 | -19.86599364551147,-26.64708755765187,20,-80.89910099878645,90.0 40 | -20.366575756264993,-25.02910553767614,20,-80.89910099878645,90.0 41 | -20.834798226618986,-23.411123517699536,20,-80.89910099883016,90.0 42 | -21.27066105657346,-21.79314149772381,20,-80.89910099878645,90.0 43 | -21.67416424612844,-20.175159477748952,20,-80.89910099874274,90.0 44 | -22.045307795283886,-18.55717745777235,20,-80.89910099883016,90.0 45 | -22.384091704039818,-16.93919543779662,20,-80.89910099878645,90.0 46 | -22.690515972396238,-15.321213417820893,20,-80.89910099878645,90.0 47 | -22.964580600353123,-13.70323139784429,20,-80.89910099883016,90.0 48 | -23.20628558791051,-12.085249377869435,20,-80.89910099874274,90.0 49 | -23.415630935068368,-10.467267357892831,20,-80.89910099883016,90.0 50 | -23.59261664182671,-8.849285337917102,20,-80.89910099878645,90.0 51 | -23.737242708185537,-7.231303317941373,20,-80.89910099878645,90.0 52 | -23.84950913414483,-5.613321297964769,20,-80.89910099883016,90.0 53 | -23.92941591970463,-3.9953392779899146,20,-80.89910099874274,90.0 54 | -23.976963064864897,-2.3773572580133115,20,-80.89910099883016,90.0 55 | -23.996443603535333,-0.974026933521682,20,-70.16651622458147,90.0 56 | -24.00024047972551,-0.18984380950895852,20,-39.20915620063617,90.0 57 | -24.00024047972551,-0.0,20,-9.492190475447927,90.0 58 | -------------------------------------------------------------------------------- /robot/profiles/BACK_UP_R.csv: -------------------------------------------------------------------------------- 1 | -0.008089910099842616,-0.8089910099842615,20,40.449550499213075,90.0 2 | -0.04044955049921733,-1.6179820199687356,20,40.449550499223704,90.0 3 | -0.10516883129794927,-3.2359640399365968,20,80.89910099839307,90.0 4 | -0.2022477524960734,-4.853946059906207,20,80.89910099848049,90.0 5 | -0.3316863140935547,-6.4719280798740675,20,80.89910099839307,90.0 6 | -0.4934845160904108,-8.089910099842804,20,80.89910099843678,90.0 7 | -0.6876423584866416,-9.70789211981154,20,80.89910099843678,90.0 8 | -0.9141598412821946,-11.325874139777651,20,80.89910099830564,90.0 9 | -1.17303696447707,-12.943856159743765,20,80.89910099830564,90.0 10 | -1.4642737280713198,-14.5618381797125,20,80.89910099843678,90.0 11 | -1.7878701320651196,-16.179820199689978,20,80.89910099887388,90.0 12 | -2.1438261764583464,-17.797802219661335,20,80.8991009985679,90.0 13 | -2.5321418612509303,-19.415784239629197,20,80.89910099839307,90.0 14 | -2.952817186442924,-21.03376625959968,20,80.89910099852419,90.0 15 | -3.4058521520337153,-22.651748279539568,20,80.89910099699436,90.0 16 | -3.8912467580237937,-24.269730299503934,20,80.89910099821823,90.0 17 | -4.409001004413195,-25.887712319470047,20,80.89910099830564,90.0 18 | -4.959114891201953,-27.50569433943791,20,80.89910099839307,90.0 19 | -5.541588418390034,-29.12367635940402,20,80.89910099830564,90.0 20 | -6.156421585977453,-30.741658379371007,20,80.89910099834935,90.0 21 | -6.803614393964213,-32.359640399338,20,80.89910099834935,90.0 22 | -7.483166842350313,-33.97762241930498,20,80.89910099834935,90.0 23 | -8.195078931135752,-35.595604439271966,20,80.89910099834935,90.0 24 | -8.939350660320532,-37.213586459238954,20,80.89910099834935,90.0 25 | -9.71598202990465,-38.83156847920594,20,80.89910099834935,90.0 26 | -10.524973039888092,-40.449550499172055,20,80.89910099830564,90.0 27 | -11.36632369027089,-42.06753251913992,20,80.89910099839307,90.0 28 | -12.22765103704478,-43.066367338694434,20,49.941740977725985,90.0 29 | -13.0803923160028,-42.637063947901076,20,-21.46516953966804,90.0 30 | -13.904570830751505,-41.20892573743518,20,-71.40691052329481,90.0 31 | -14.696389705100676,-39.59094371745858,20,-80.89910099883016,90.0 32 | -15.455848939050334,-37.972961697482845,20,-80.89910099878645,90.0 33 | -16.182948532600477,-36.35497967750712,20,-80.89910099878645,90.0 34 | -16.877688485751104,-34.73699765753139,20,-80.89910099878645,90.0 35 | -17.540068798502215,-33.11901563755566,20,-80.89910099878645,90.0 36 | -18.1700894708538,-31.501033617579058,20,-80.89910099883016,90.0 37 | -18.76775050280588,-29.8830515976042,20,-80.89910099874274,90.0 38 | -19.333051894358434,-28.2650695776276,20,-80.89910099883016,90.0 39 | -19.86599364551147,-26.64708755765187,20,-80.89910099878645,90.0 40 | -20.366575756264993,-25.02910553767614,20,-80.89910099878645,90.0 41 | -20.834798226618986,-23.411123517699536,20,-80.89910099883016,90.0 42 | -21.27066105657346,-21.79314149772381,20,-80.89910099878645,90.0 43 | -21.67416424612844,-20.175159477748952,20,-80.89910099874274,90.0 44 | -22.045307795283886,-18.55717745777235,20,-80.89910099883016,90.0 45 | -22.384091704039818,-16.93919543779662,20,-80.89910099878645,90.0 46 | -22.690515972396238,-15.321213417820893,20,-80.89910099878645,90.0 47 | -22.964580600353123,-13.70323139784429,20,-80.89910099883016,90.0 48 | -23.20628558791051,-12.085249377869435,20,-80.89910099874274,90.0 49 | -23.415630935068368,-10.467267357892831,20,-80.89910099883016,90.0 50 | -23.59261664182671,-8.849285337917102,20,-80.89910099878645,90.0 51 | -23.737242708185537,-7.231303317941373,20,-80.89910099878645,90.0 52 | -23.84950913414483,-5.613321297964769,20,-80.89910099883016,90.0 53 | -23.92941591970463,-3.9953392779899146,20,-80.89910099874274,90.0 54 | -23.976963064864897,-2.3773572580133115,20,-80.89910099883016,90.0 55 | -23.996443603535333,-0.974026933521682,20,-70.16651622458147,90.0 56 | -24.00024047972551,-0.18984380950895852,20,-39.20915620063617,90.0 57 | -24.00024047972551,-0.0,20,-9.492190475447927,90.0 58 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_LEFT-SCALE_LEFT_TWOCUBE1_C.csv: -------------------------------------------------------------------------------- 1 | -0.008198173856629402,-0.8198173856629403,20,40.99086928314701,44.996441595058144 2 | -0.04099086928314701,-2.459452156988821,20,81.98173856629403,44.98151319976378 3 | -0.10657626013618224,-4.0990869283147005,20,81.981738566294,44.951267967324526 4 | -0.20495434641573507,-5.738721699640581,20,81.98173856629403,44.904924145465884 5 | -0.33612512812180556,-7.3783564709664615,20,81.98173856629403,44.841296602323524 6 | -0.5000886052543936,-9.017991242292343,20,81.98173856629403,44.758780875287414 7 | -0.6968447778134993,-10.657626013618223,20,81.98173856629408,44.65533027600074 8 | -0.9263936457991225,-12.297260784944104,20,81.98173856629397,44.52842455082339 9 | -1.1887352092112633,-13.936895556269983,20,81.98173856629397,44.37502807758699 10 | -1.4838694680499218,-15.576530327595865,20,81.98173856629408,44.19153493623929 11 | -1.8117964223150977,-17.216165098921746,20,81.98173856629397,43.97369738051097 12 | -2.1725160720067915,-18.855799870247626,20,81.98173856629408,43.71653318552305 13 | -2.5660284171250027,-20.495434641573507,20,81.98173856629408,43.41420595612604 14 | -2.992333457669732,-22.135069412899387,20,81.98173856629408,43.05987062924416 15 | -3.4514311936409783,-23.774704184225268,20,81.98173856629386,42.645473901643015 16 | -3.943321625038742,-25.41433895555115,20,81.98173856629408,42.1614959282021 17 | -4.468004751863024,-27.05397372687703,20,81.98173856629408,41.59661503471642 18 | -5.025480574113823,-28.69360849820291,20,81.98173856629408,40.937270972974005 19 | -5.61574909179114,-30.33324326952879,20,81.98173856629386,40.16709391257007 20 | -6.238810304894974,-31.97287804085467,20,81.98173856629408,39.2661554979303 21 | -6.894664213425327,-33.612512812180555,20,81.98173856629408,38.209984761705755 22 | -7.5833108173821975,-35.25214758350643,20,81.98173856629386,36.968276575657114 23 | -8.304750116765584,-36.89178235483231,20,81.98173856629408,35.503207963161486 24 | -9.058982111575489,-38.53141712615819,20,81.98173856629408,33.76728042605694 25 | -9.846006801811912,-40.17105189748408,20,81.98173856629408,31.700656157961873 26 | -10.665824187474852,-41.81068666880994,20,81.98173856629364,29.228127403112865 27 | -11.51843426856431,-43.45032144013583,20,81.98173856629408,26.256315029006856 28 | -12.403837045080286,-45.08995621146171,20,81.98173856629408,22.672755105129454 29 | -13.322032517022777,-46.72959098278759,20,81.98173856629408,18.350690676848245 30 | -14.273020684391788,-48.36922575411347,20,81.98173856629408,13.166883118822057 31 | -15.246944548238499,-49.0231606305577,20,32.69674382221138,7.107675272965253 32 | -16.217550761900835,-48.037460735676056,20,-49.28499474408226,0.4136266888947375 33 | -17.1619036289011,-46.397825964350176,20,-81.98173856629408,-6.505345338981158 34 | -18.073463800474844,-44.758191193024295,20,-81.98173856629408,-13.267383328464263 35 | -18.952231276622072,-43.11855642169841,20,-81.98173856629408,-19.578360137504006 36 | -19.79820605734278,-41.47892165037253,20,-81.98173856629408,-25.25357211455463 37 | -20.611388142636972,-39.839286879046654,20,-81.98173856629364,-30.225956017624867 38 | -21.391777532504648,-38.19965210772077,20,-81.98173856629408,-34.51467963675918 39 | -22.139374226945804,-36.56001733639489,20,-81.98173856629408,-38.18539726174265 40 | -22.854178225960442,-34.92038256506901,20,-81.98173856629408,-41.320215220063034 41 | -23.536189529548565,-33.280747793743124,20,-81.9817385662943,-44.00048242157155 42 | -24.18540813771017,-31.641113022417247,20,-81.98173856629386,-46.299026751935145 43 | -24.801834050445255,-30.001478251091367,20,-81.98173856629408,-48.277803990557885 44 | -25.385467267753825,-28.361843479765483,20,-81.98173856629408,-49.98814839639053 45 | -25.936307789635872,-26.722208708439606,20,-81.98173856629386,-51.472045294022564 46 | -26.45435561609141,-25.082573937113725,20,-81.98173856629408,-52.76365094532915 47 | -26.939610747120426,-23.442939165787845,20,-81.98173856629408,-53.890726120391804 48 | -27.39207318272292,-21.803304394461968,20,-81.98173856629386,-54.875867296364405 49 | -27.8117429228989,-20.163669623136084,20,-81.98173856629408,-55.73751708140327 50 | -28.198619967648362,-18.524034851810207,20,-81.98173856629397,-56.4907741672577 51 | -28.55270431697131,-16.884400080484326,20,-81.98173856629397,-57.1480348547975 52 | -28.87399597086774,-15.244765309158442,20,-81.98173856629418,-57.71949820490036 53 | -29.162494929337647,-13.605130537832563,20,-81.98173856629397,-58.213562924527025 54 | -29.41820119238104,-11.965495766506683,20,-81.98173856629397,-58.63713890407162 55 | -29.64111475999791,-10.325860995180802,20,-81.98173856629408,-58.99589169449662 56 | -29.83123563218827,-8.686226223854922,20,-81.98173856629397,-59.29443401374468 57 | -29.98856380895211,-7.046591452529042,20,-81.98173856629403,-59.53647519372002 58 | -30.113099290289433,-5.4069566812031615,20,-81.98173856629403,-59.7249368557608 59 | -30.204842076200237,-3.767321909877281,20,-81.98173856629403,-59.86204098039816 60 | -30.26379216668452,-2.1276871385514005,20,-81.98173856629403,-59.949375067982174 61 | -30.29160838683448,-0.6539348764442301,20,-73.68761310535852,-59.990379278618434 62 | -30.29814773559892,-0.0,20,-32.6967438222115,-60.00000000000003 63 | -30.29814773559892,-0.0,20,0.0,-60.00000000000003 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_LEFT-SCALE_LEFT_TWOCUBE1_L.csv: -------------------------------------------------------------------------------- 1 | -0.008198173856629402,-0.8198173856629403,20,40.99086928314701,44.996441595058144 2 | -0.03911491169591853,-1.5458368919644567,20,36.300975315075824,44.98151319976378 3 | -0.1008995737337541,-3.089233101891778,20,77.16981049636605,44.951267967324526 4 | -0.1934539210174078,-4.627717364182685,20,76.92421311454537,44.904924145465884 5 | -0.31662902335330223,-6.158755116794721,20,76.55188763060175,44.841296602323524 6 | -0.4702232549488741,-7.679711579778593,20,76.04782314919362,44.758780875287414 7 | -0.6539794164968363,-9.18780807739811,20,75.4048248809758,44.65533027600074 8 | -0.8675807967942042,-10.68006901486839,20,74.61304687351408,44.52842455082339 9 | -1.1106459179567036,-12.153256058124972,20,73.6593521628291,44.37502807758699 10 | -1.3827216318257614,-13.60378569345289,20,72.5264817663959,44.19153493623929 11 | -1.6832741299867826,-15.027624908051068,20,71.19196072990884,43.97369738051097 12 | -2.0116772975590327,-16.4201583786125,20,69.62667352807146,43.71653318552305 13 | -2.3671976694135335,-17.77601859272506,20,67.79301070562808,43.41420595612604 14 | -2.7489750103623463,-19.08886704744063,20,65.64242273577858,43.05987062924416 15 | -3.155997229710366,-20.351110967400974,20,63.112195998017214,42.645473901643015 16 | -3.5870679125931058,-21.553534144136993,20,60.121158836800966,42.1614959282021 17 | -4.040764174470585,-22.684813093873984,20,56.5639474868495,41.59661503471642 18 | -4.515381758494816,-23.73087920121156,20,52.30330536687879,40.937270972974005 19 | -5.008863256380402,-24.674074894279293,20,47.159784653386744,40.16709391257007 20 | -5.518703956954051,-25.492035028682416,20,40.89800672015615,39.2661554979303 21 | -6.041828131320633,-26.156208718329104,20,33.20868448233442,38.209984761705755 22 | -6.574426664103271,-26.629926639131888,20,23.685896040139095,36.968276575657114 23 | -7.111745383385537,-26.865935964113255,20,11.800466249068423,35.503207963161486 24 | -7.647813807031719,-26.803421182309073,20,-3.1257390902091227,33.76728042605694 25 | -8.175110288873386,-26.364824092083342,20,-21.92985451128649,31.700656157961873 26 | -8.684181126017352,-25.453541857198324,20,-45.564111744250916,29.228127403112865 27 | -9.163288700172862,-23.95537870777554,20,-74.90815747113929,26.256315029006856 28 | -9.598297454721433,-21.75043772742856,20,-110.24704901734883,22.672755105129454 29 | -9.97327796828827,-18.749025678341837,20,-150.07060245433632,18.350690676848245 30 | -10.272748585446685,-14.973530857920741,20,-188.77474102105475,13.166883118822057 31 | -10.485152539019836,-10.62019767865758,20,-217.66665896315806,7.107675272965253 32 | -10.614487585074098,-6.466752302713054,20,-207.67226879722628,0.4136266888947375 33 | -10.689331108050848,-3.7421761488374083,20,-136.22880769378227,-6.505345338981158 34 | -10.751111905729513,-3.089039883933221,20,-32.65681324520938,-13.267383328464263 35 | -10.836775709369677,-4.2831901820082425,20,59.70751490375108,-19.578360137504006 36 | -10.969528798793727,-6.637654471202452,20,117.72321445971046,-25.25357211455463 37 | -11.1578042043293,-9.413770276778687,20,138.80579027881174,-30.225956017624867 38 | -11.399200707686616,-12.069825167865785,20,132.80274455435486,-34.51467963675918 39 | -11.685472675151686,-14.313598373253498,20,112.18866026938566,-38.18539726174265 40 | -12.006303930890974,-16.04156278696445,20,86.39822068554773,-41.320215220063034 41 | -12.351471513476067,-17.258379129254628,20,60.84081711450874,-44.00048242157155 42 | -12.711822390206914,-18.01754383654239,20,37.958235364388266,-46.299026751935145 43 | -13.079569561650425,-18.387358572175568,20,18.49073678165876,-48.277803990557885 44 | -13.448260881129817,-18.43456597396954,20,2.360370089698595,-49.98814839639053 45 | -13.812619238568095,-18.217917871913862,20,-10.832405102783937,-51.472045294022564 46 | -14.16835158018978,-17.786617081084344,20,-21.565039541475837,-52.76365094532915 47 | -14.511968727405314,-17.180857360776738,20,-30.28798601538031,-53.890726120391804 48 | -14.840630621465035,-16.433094702986008,20,-37.388132889536564,-54.875867296364405 49 | -15.152019320807792,-15.569434967137795,20,-43.18298679241061,-55.73751708140327 50 | -15.44423718204928,-14.610893062074386,20,-47.92709525317042,-56.4907741672577 51 | -15.71572622802503,-13.574452298787516,20,-51.82203816434347,-57.1480348547975 52 | -15.965204643447581,-12.473920771127515,20,-55.02657638300005,-57.71949820490036 53 | -16.191616896583284,-11.320612656785084,20,-57.665405717121615,-58.213562924527025 54 | -16.394094569493397,-10.12388364550575,20,-59.83645056396666,-58.63713890407162 55 | -16.571925640207162,-8.891553535688297,20,-61.616505490872605,-58.99589169449662 56 | -16.72453040564193,-7.630238271738279,20,-63.06576319750091,-59.29443401374468 57 | -16.851442694511196,-6.34561444346319,20,-64.23119141375447,-59.53647519372002 58 | -16.95229533973304,-5.042632261092305,20,-65.14910911854426,-59.7249368557608 59 | -17.026809094757166,-3.725687751206154,20,-65.84722549430757,-59.86204098039816 60 | -17.074784454906958,-2.398768007489584,20,-66.34598718582849,-59.949375067982174 61 | -17.097447933388143,-1.1331739240593148,20,-63.27970417151346,-59.990379278618434 62 | -17.102778307816678,-0.2665187214267583,20,-43.33276013162783,-60.00000000000003 63 | -17.102778307816678,-0.0,20,-13.325936071337916,-60.00000000000003 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_LEFT-SCALE_LEFT_TWOCUBE1_R.csv: -------------------------------------------------------------------------------- 1 | -0.008198173856629402,-0.8198173856629403,20,40.99086928314701,44.996441595058144 2 | -0.04286682671923621,-1.7334326431303402,20,45.68076287336999,44.98151319976378 3 | -0.11225294482370587,-3.4693059052234827,20,86.79366310465713,44.951267967324526 4 | -0.2164547648458921,-5.21009100110931,20,87.03925479429137,44.904924145465884 5 | -0.35562121244780476,-6.958322380095635,20,87.41156894931623,44.841296602323524 6 | -0.5299539068144102,-8.716634718330273,20,87.91561691173186,44.758780875287414 7 | -0.7397100367679846,-10.487806497678724,20,88.55858896742264,44.65533027600074 8 | -0.9852062986949105,-12.274813096346293,20,89.35032993337843,44.52842455082339 9 | -1.2668241477831002,-14.080892454409481,20,90.30396790315939,44.37502807758699 10 | -1.5850166992465378,-15.909627573171875,20,91.43675593811967,44.19153493623929 11 | -1.940317714694327,-17.765050772389454,20,92.77115996087895,43.97369738051097 12 | -2.3333532409417126,-19.651776312369282,20,94.33627699899145,43.71653318552305 13 | -2.7648566463511406,-21.5751702704714,20,96.1696979051058,43.41420595612604 14 | -3.2356880278751254,-23.541569076199234,20,98.31994028639173,43.05987062924416 15 | -3.746859279373771,-25.558562574932274,20,100.84967493665205,42.645473901643015 16 | -4.299566534493289,-27.635362755975905,20,103.84000905218144,42.1614959282021 17 | -4.895232277025161,-29.78328712659359,20,107.39621853088423,41.59661503471642 18 | -5.535560186124457,-32.016395454964844,20,111.65541641856268,40.937270972974005 19 | -6.222606836899107,-34.35233253873246,20,116.79685418838095,40.16709391257007 20 | -6.9588757274155135,-36.813444525820344,20,123.0555993543942,39.2661554979303 21 | -7.747440802955858,-39.42825377701722,20,130.74046255984345,38.209984761705755 22 | -8.592108532835487,-42.23338649398141,20,140.2566358482101,36.968276575657114 23 | -9.497629118179486,-45.276029267199924,20,152.13213866092516,35.503207963161486 24 | -10.469967014560368,-48.61689481904408,20,167.04327759220794,33.76728042605694 25 | -11.516634632871348,-52.333380915549014,20,185.82430482524674,31.700656157961873 26 | -12.647071425381077,-56.52183962548644,20,209.4229354968715,29.228127403112865 27 | -13.872993020090362,-61.29607973546423,20,238.7120054988895,26.256315029006856 28 | -15.208501465980746,-66.77542229451913,20,273.9671279527451,22.672755105129454 29 | -16.66947699599897,-73.04877650091127,20,313.6677103196065,18.350690676848245 30 | -18.271334863315303,-80.09289336581672,20,352.2058432452727,13.166883118822057 31 | -20.00587212244051,-86.72686295626023,20,331.6984795221758,7.107675272965253 32 | -21.816646152499036,-90.53870150292629,20,190.59192733330264,0.4136266888947375 33 | -23.629359870180835,-90.63568588408991,20,4.8492190581806796,-6.505345338981158 34 | -25.389639986277473,-88.01400580483191,20,-131.08400396289966,-13.267383328464263 35 | -27.060622611745387,-83.54913127339557,20,-223.24372657181686,-19.578360137504006 36 | -28.61912791364386,-77.92526509492377,20,-281.19330892358994,-25.25357211455463 37 | -30.056706765559078,-71.87894259576092,20,-302.3161249581431,-30.225956017624867 38 | -31.375724990683864,-65.95091125623941,20,-296.4015669760751,-34.51467963675918 39 | -32.58439089832432,-60.43329538202284,20,-275.88079371082864,-38.18539726174265 40 | -33.69298944218466,-55.429927193017015,20,-250.16840945029125,-41.320215220063034 41 | -34.71172016096654,-50.936535939094185,20,-224.66956269614155,-44.00048242157155 42 | -35.64971946893267,-46.89996539830643,20,-201.82852703938772,-46.299026751935145 43 | -36.514762875819244,-43.252170344328526,20,-182.38975269889508,-48.277803990557885 44 | -37.31329466585869,-39.926589501972245,20,-166.27904211781402,-49.98814839639053 45 | -38.050586568787956,-36.86459514646337,20,-153.09971777544348,-51.472045294022564 46 | -38.73092794607397,-34.017068864300704,20,-142.3763141081337,-52.76365094532915 47 | -39.3578054171556,-31.34387355408182,20,-133.65976551094406,-53.890726120391804 48 | -39.934057247776806,-28.812591531060306,20,-126.56410115107569,-54.875867296364405 49 | -40.46200012097442,-26.397143659880623,20,-120.77239355898416,-55.73751708140327 50 | -40.94353077650364,-24.076532776460862,20,-116.03054417098804,-56.4907741672577 51 | -41.38020654769244,-21.83378855944016,20,-112.13721085103519,-57.1480348547975 52 | -41.77330877558306,-19.65511139453094,20,-108.93385824546081,-57.71949820490036 53 | -42.12389265393362,-17.52919391752825,20,-106.29587385013458,-58.213562924527025 54 | -42.43282634137247,-15.446684371942064,20,-104.12547727930927,-58.63713890407162 55 | -42.700821677059146,-13.399766784333464,20,-102.34587938043008,-58.99589169449662 56 | -42.9284582276656,-11.381827530322875,20,-100.89696270052943,-59.29443401374468 57 | -43.11620205594707,-9.387191414073529,20,-99.7318058124673,-59.53647519372002 58 | -43.26442026259866,-7.41091033257958,20,-98.81405407469745,-59.7249368557608 59 | -43.37339203494927,-5.448588617530675,20,-98.11608575244527,-59.86204098039816 60 | -43.44331684364987,-3.4962404350296503,20,-97.6174091250512,-59.949375067982174 61 | -43.47628580426923,-1.6484480309682497,20,-92.38962020307002,-59.990379278618434 62 | -43.48403412975244,-0.3874162741602031,20,-63.051587840402334,-60.00000000000003 63 | -43.48403412975244,-0.0,20,-19.370813708010154,-60.00000000000003 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_LEFT_TWOCUBE3-SCALE_LEFT_C.csv: -------------------------------------------------------------------------------- 1 | 0.008198193206953523,0.8198193206953523,20,40.99096603476762,300.0113558101849 2 | 0.04099096603476762,2.459457962086057,20,81.98193206953523,300.0597164080668 3 | 0.10657651169039581,4.099096603476762,20,81.98193206953523,300.1569870609671 4 | 0.2049548301738381,5.738735244867467,20,81.98193206953523,300.30428208129166 5 | 0.33612592148509446,7.378373886258172,20,81.98193206953523,300.5033121788344 6 | 0.5000897856241648,9.018012527648875,20,81.98193206953518,300.7564336962423 7 | 0.6968464225910495,10.65765116903958,20,81.98193206953529,301.06671805807935 8 | 0.9263958323857481,12.297289810430286,20,81.98193206953523,301.4380449519755 9 | 1.1887380150082607,13.93692845182099,20,81.98193206953523,301.8752241868526 10 | 1.4838729704585878,15.576567093211695,20,81.98193206953523,302.3841527961394 11 | 1.8118006987367286,17.2162057346024,20,81.98193206953523,302.97201617276687 12 | 2.172521199842684,18.855844375993108,20,81.98193206953535,303.64754466395556 13 | 2.566034473776453,20.49548301738381,20,81.98193206953513,304.42134055758663 14 | 2.9923405205380362,22.135121658774512,20,81.98193206953523,305.3062945811854 15 | 3.451439340127434,23.77476030016522,20,81.98193206953523,306.31811589613557 16 | 3.943330932544645,25.414398941555927,20,81.98193206953545,307.47600462919377 17 | 4.4680152977896705,27.054037582946627,20,81.981932069535,308.8034995284111 18 | 5.02549243586251,28.69367622433733,20,81.98193206953523,310.3295316820375 19 | 5.615762346763164,30.33331486572804,20,81.98193206953523,312.08969991587 20 | 6.238825030491633,31.972953507118742,20,81.98193206953523,314.1277361247911 21 | 6.894680487047915,33.61259214850945,20,81.98193206953523,316.49701346200146 22 | 7.583328716432011,35.25223078990015,20,81.98193206953523,319.2616980152947 23 | 8.304769718643922,36.89186943129086,20,81.98193206953523,322.49664240051936 24 | 9.059003493683646,38.53150807268156,20,81.98193206953523,326.28422758793084 25 | 9.846030041551183,40.171146714072265,20,81.98193206953523,330.70506518949827 26 | 10.665849362246535,41.81078535546297,20,81.98193206953523,335.8184069651336 27 | 11.518461455769701,43.45042399685367,20,81.98193206953523,341.6295548133908 28 | 12.403866322120681,45.090062638244376,20,81.98193206953523,348.04973470600873 29 | 13.322063961299476,46.72970127963509,20,81.98193206953523,354.86995064249146 30 | 14.273054373306085,48.36933992102579,20,81.98193206953523,1.7803040494782891 31 | 15.246981111554213,49.02333390378702,20,32.699699138061504,8.380633313954597 32 | 16.217591343043658,48.037689245157544,20,-49.28223293147373,14.272729741823563 33 | 17.161948741532903,46.39805060376684,20,-81.98193206953523,19.27592564294328 34 | 18.07351336719433,44.75841196237614,20,-81.98193206953523,23.44022458047051 35 | 18.952285220027946,43.11877332098543,20,-81.98193206953523,26.89192244512893 36 | 19.798264300033747,41.47913467959473,20,-81.98193206953523,29.755769178100874 37 | 20.611450607211733,39.83949603820402,20,-81.98193206953523,32.14052628308203 38 | 21.391844141561908,38.19985739681332,20,-81.981932069535,34.13576752247549 39 | 22.139444903084268,36.56021875542261,20,-81.98193206953545,35.81338278913161 40 | 22.854252891778813,34.92058011403191,20,-81.98193206953523,37.23050998442927 41 | 23.536268107645544,33.2809414726412,20,-81.98193206953523,38.432518410816165 42 | 24.185490550684463,31.6413028312505,20,-81.98193206953523,39.45557393162152 43 | 24.801920220895564,30.00166418985979,20,-81.98193206953523,40.32869647264324 44 | 25.385557118278854,28.362025548469088,20,-81.98193206953523,41.07535823829747 45 | 25.936401242834325,26.722386907078384,20,-81.98193206953523,41.71470731471207 46 | 26.454452594561985,25.08274826568768,20,-81.98193206953523,42.26249947425401 47 | 26.939711173461834,23.443109624296973,20,-81.98193206953523,42.731807468228645 48 | 27.392176979533865,21.80347098290627,20,-81.98193206953523,43.13356211803837 49 | 27.811850012778084,20.163832341515565,20,-81.98193206953523,43.47696648107085 50 | 28.19873027319449,18.524193700124858,20,-81.98193206953523,43.76981404053231 51 | 28.55281776078308,16.884555058734154,20,-81.98193206953523,44.018733992703524 52 | 28.874112475543853,15.24491641734345,20,-81.98193206953523,44.22938085713987 53 | 29.162614417476817,13.605277775952745,20,-81.98193206953523,44.40658129992712 54 | 29.418323586581963,11.965639134562043,20,-81.98193206953513,44.55444787761672 55 | 29.641239982859297,10.326000493171339,20,-81.98193206953523,44.67646705395649 56 | 29.831363606308816,8.686361851780633,20,-81.98193206953529,44.77556709352099 57 | 29.988694456930524,7.046723210389928,20,-81.98193206953523,44.85417012462612 58 | 30.113232534724414,5.407084568999223,20,-81.98193206953523,44.91423166508365 59 | 30.20497783969049,3.7674459276085184,20,-81.98193206953523,44.95727013284403 60 | 30.263930371828756,2.1278072862178137,20,-81.98193206953523,44.98438824827446 61 | 30.291748384518545,0.6539939827612307,20,-73.69066517282916,44.99703926824252 62 | 30.298288324346156,0.0,20,-32.69969913806153,45.00000000000001 63 | 30.298288324346156,0.0,20,0.0,45.00000000000001 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_LEFT_TWOCUBE3-SCALE_LEFT_L.csv: -------------------------------------------------------------------------------- 1 | 0.008198193206953523,0.8198193206953523,20,40.99096603476762,300.0113558101849 2 | 0.034913794046503244,1.335780041977486,20,25.798036064106682,300.0597164080668 3 | 0.08827594228897302,2.668107412123489,20,66.61636850730015,300.1569870609671 4 | 0.1681445992892967,3.993432850016183,20,66.26627189463471,300.30428208129166 5 | 0.2743047779964407,5.3080089353572,20,65.72880426705082,300.5033121788344 6 | 0.40646034598575675,6.607778399465803,20,64.98847320543018,300.7564336962423 7 | 0.5642253062498848,7.888248013206399,20,64.02348068702977,301.06671805807935 8 | 0.7471120806463277,9.144338719822152,20,62.8045353307876,301.4380449519755 9 | 0.9545161971198415,10.370205823675686,20,61.29335519267678,301.8752241868526 10 | 1.1856965361580154,11.559016951908697,20,59.440556411650576,302.3841527961394 11 | 1.4397500579514255,12.702676089670504,20,57.18295688809033,302.97201617276687 12 | 1.7155795462868895,13.791474416773198,20,54.43991635513465,303.64754466395556 13 | 2.0118525070533337,14.813648038322201,20,51.10868107745023,304.42134055758663 14 | 2.326948818259203,15.754815560293462,20,47.058376098563045,305.3062945811854 15 | 2.6588941068213416,16.597264428106946,20,42.12244339067425,306.31811589613557 16 | 3.0052752142535577,17.319055371610805,20,36.08954717519278,307.47600462919377 17 | 3.3631336476719125,17.89292167091774,20,28.693314965346826,308.8034995284111 18 | 3.728833123427205,18.28497378776463,20,19.602605842344605,310.3295316820375 19 | 4.097899267114335,18.453307184356508,20,8.41666982959378,312.08969991587 20 | 4.4648354420646825,18.346808747517382,20,-5.324921841956308,314.1277361247911 21 | 4.822933250416229,17.90489041757728,20,-22.095916497005067,316.49701346200146 22 | 5.164127942056983,17.059734582037702,20,-42.25779177697881,319.2616980152947 23 | 5.479012150686065,15.74421043145413,20,-65.77620752917869,322.49664240051936 24 | 5.757233513068036,13.911068119098488,20,-91.65711561778211,326.28422758793084 25 | 5.988664228501168,11.571535771656599,20,-116.97661737209444,330.70506518949827 26 | 6.165863904270937,8.859983788488464,20,-135.57759915840677,335.8184069651336 27 | 6.288173936139556,6.115501593430914,20,-137.22410975287747,341.6295548133908 28 | 6.366754264288753,3.929016407459866,20,-109.3242592985524,348.04973470600873 29 | 6.427861336862704,3.0553536286975613,20,-43.68313893811524,354.86995064249146 30 | 6.510421243036936,4.127995308711647,20,53.63208400070427,1.7803040494782891 31 | 6.654847792750738,7.221327485690077,20,154.6666088489215,8.380633313954597 32 | 6.884935463611255,11.504383543025869,20,214.15280286678959,14.272729741823563 33 | 7.200473418185039,15.776897728689208,20,213.6257092831669,19.27592564294328 34 | 7.58865187897836,19.40892303966605,20,181.601265548842,23.44022458047051 35 | 8.03360356561173,22.247584331668506,20,141.93306460012298,26.89192244512893 36 | 8.519650590757433,24.30235125728519,20,102.73834628083407,29.755769178100874 37 | 9.033122487372363,25.67359483074649,20,68.5621786730651,32.14052628308203 38 | 9.56275988632703,26.481869947733326,20,40.413755849341804,34.13576752247549 39 | 10.099526139319682,26.838312649632595,20,17.822135094963386,35.81338278913161 40 | 10.636239000407995,26.835643054415588,20,-0.13347976085033633,37.23050998442927 41 | 11.167195649398979,26.547832449549126,20,-14.390530243322983,38.432518410816165 42 | 11.687850228638718,26.03272896198696,20,-25.755174378108222,39.45557393162152 43 | 12.194555181500794,25.335247643103834,20,-34.87406594415644,40.32869647264324 44 | 12.684360327640775,24.49025730699907,20,-42.249516805238315,41.07535823829747 45 | 13.1548590356995,23.524935402936286,20,-48.26609520313911,41.71470731471207 46 | 13.604071082633062,22.46060234667817,20,-53.216652812905814,42.26249947425401 47 | 14.030353486954974,21.31412021609556,20,-57.324106529130454,42.731807468228645 48 | 14.432332490494273,20.09895017696488,20,-60.758501956533976,43.13356211803837 49 | 14.808851494242457,18.825950187409283,20,-63.64999947777984,43.47696648107085 50 | 15.158931063265003,17.503978451127292,20,-66.09858681409955,43.76981404053231 51 | 15.48173809264886,16.1403514691928,20,-68.18134909672455,44.018733992703524 52 | 15.776561975186972,14.741194126905574,20,-69.95786711436142,44.22938085713987 53 | 16.04279614621203,13.31170855125286,20,-71.4742787826357,44.40658129992712 54 | 16.279923787003323,11.8563820395647,20,-72.76632558440801,44.55444787761672 55 | 16.487506761881104,10.37914874388907,20,-73.86166478378144,44.67646705395649 56 | 16.66517708464481,8.883516138185186,20,-74.78163028519428,44.77556709352099 57 | 16.812630375315763,7.37266453354766,20,-75.54258023187629,44.85417012462612 58 | 16.92962089182456,5.849525825439825,20,-76.15693540539174,44.91423166508365 59 | 17.015957821389925,4.316846478268358,20,-76.63396735857333,44.95727013284403 60 | 17.071502589979406,2.7772384294740093,20,-76.98040243971744,44.98438824827446 61 | 17.09773082852349,1.3114119272041107,20,-73.29132511349492,44.99703926824252 62 | 17.103898711907107,0.3083941691808753,20,-50.150887901161774,45.00000000000001 63 | 17.103898711907107,0.0,20,-15.419708459043767,45.00000000000001 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_LEFT_TWOCUBE3-SCALE_LEFT_R.csv: -------------------------------------------------------------------------------- 1 | 0.008198193206953523,0.8198193206953523,20,40.99096603476762,300.0113558101849 2 | 0.04706813785097514,1.943497232201081,20,56.183895575286435,300.0597164080668 3 | 0.1248770651311897,3.8904463640107267,20,97.3474565904823,300.1569870609671 4 | 0.24176498897825072,5.8443961923530505,20,97.6974914171162,300.30428208129166 5 | 0.3979468629173694,7.809093696955935,20,98.23487523014418,300.5033121788344 6 | 0.5937187564076042,9.788594674511742,20,98.97504887779034,300.7564336962423 7 | 0.8294665886662609,11.787391612932833,20,99.93984692105455,301.06671805807935 8 | 1.1056778289480114,13.81056201408752,20,101.15852005773438,301.4380449519755 9 | 1.4229568068619245,15.863948895695668,20,102.6693440804074,301.8752241868526 10 | 1.7820444383309337,17.954381573450462,20,104.52163388773975,302.3841527961394 11 | 2.183843496151973,20.08995289105195,20,106.77856588007442,302.97201617276687 12 | 2.6294508319503227,22.28036678991751,20,109.52069494327799,303.64754466395556 13 | 3.1201984378856196,24.537380296764837,20,112.85067534236632,304.42134055758663 14 | 3.657705746867319,26.875365449084967,20,116.89925761600652,305.3062945811854 15 | 4.243946167716982,29.312021042483124,20,121.83277966990772,306.31811589613557 16 | 4.881331505803333,31.86926690431759,20,127.86229309172346,307.47600462919377 17 | 5.572818337398068,34.57434157973675,20,135.25373377095798,308.8034995284111 18 | 6.322040189559157,37.461092608054436,20,144.33755141588435,310.3295316820375 19 | 7.133467460480477,40.57136354606603,20,155.5135469005797,312.08969991587 20 | 8.01259098429438,43.95617619069509,20,169.2406322314529,314.1277361247911 21 | 8.96611067491284,47.67598453092305,20,185.99041701139797,316.49701346200146 22 | 10.002078899251106,51.798411216913316,20,206.1213342995132,319.2616980152947 23 | 11.129885172990372,56.39031368696323,20,229.59512350249574,322.49664240051936 24 | 12.359856909773237,61.49858683914331,20,255.41365760900393,326.28422758793084 25 | 13.702089179945068,67.11161350859152,20,280.65133347241033,330.70506518949827 26 | 15.163984505532436,73.0947662793685,20,299.157638538849,335.8184069651336 27 | 16.74616822839033,79.10918614289481,20,300.720993176316,341.6295548133908 28 | 18.43747091902478,84.56513453172236,20,272.7974194413771,348.04973470600873 29 | 20.21167343104143,88.71012560083253,20,207.2495534555086,354.86995064249146 30 | 22.02994101653268,90.91337927456256,20,110.16268368650128,1.7803040494782891 31 | 23.8322918117885,90.11753976279115,20,-39.79197558857037,8.380633313954597 32 | 25.542570351889943,85.51392700507206,20,-230.18063788595458,14.272729741823563 33 | 27.115147893928548,78.62887710193019,20,-344.25249515709373,19.27592564294328 34 | 28.54969785462666,71.72749803490568,20,-345.0689533512253,23.44022458047051 35 | 29.862024345495925,65.61632454346321,20,-305.5586745721233,26.89192244512893 36 | 31.067759486722622,60.286757061334846,20,-266.478374106418,29.755769178100874 37 | 32.180542883279266,55.639169827832205,20,-232.37936167513206,32.14052628308203 38 | 33.211613730289876,51.55354235053055,20,-204.28137386508305,34.13576752247549 39 | 34.16999561242335,47.91909410667354,20,-181.72241219285024,35.81338278913161 40 | 35.06286230143109,44.643334450386824,20,-163.78798281433606,37.23050998442927 41 | 35.89591107751207,41.652438804049126,20,-149.54478231688478,38.432518410816165 42 | 36.67368413933701,38.888653091247086,20,-138.18928564010199,39.45557393162152 43 | 37.399826600168275,36.30712304156306,20,-129.07650248420137,40.32869647264324 44 | 38.077286990485625,33.873019515867774,20,-121.7051762847643,41.07535823829747 45 | 38.708470816361874,31.559191293812376,20,-115.69141110276985,41.71470731471207 46 | 39.29535752724589,29.34433554420077,20,-110.74278748058029,42.26249947425401 47 | 39.839589567675475,27.211602021479294,20,-106.63667613607379,42.731807468228645 48 | 40.34254032276734,25.147537754593376,20,-103.20321334429595,43.13356211803837 49 | 40.805366128918905,23.141290307578295,20,-100.31237235075398,43.47696648107085 50 | 41.2290462385739,21.184005482750173,20,-97.86424124140613,43.76981404053231 51 | 41.61441362743307,19.268369442958676,20,-95.78180198957482,44.018733992703524 52 | 41.96217881254724,17.38825925570806,20,-94.00550936253079,44.22938085713987 53 | 42.2729482954493,15.538474145102835,20,-92.48925553026119,44.40658129992712 54 | 42.54723885100183,13.714527777626632,20,-91.19731837381019,44.55444787761672 55 | 42.78548858453499,11.912486676657803,20,-90.1020550484415,44.67646705395649 56 | 42.98806546119271,10.128843832886517,20,-89.18214218856427,44.77556709352099 57 | 43.155273847114756,8.360419296102041,20,-88.4212268392238,44.85417012462612 58 | 43.28735947477523,6.604281383023715,20,-87.8068956539163,44.91423166508365 59 | 43.38451315103349,4.8576838129127955,20,-87.32987850554596,44.95727013284403 60 | 43.44687344547284,3.118014721967557,20,-86.98345454726193,44.98438824827446 61 | 43.47628123218537,1.4703893356267697,20,-82.38126931703937,44.99703926824252 62 | 43.483193228634875,0.3455998224749041,20,-56.239475657593275,45.00000000000001 63 | 43.483193228634875,0.0,20,-17.279991123745205,45.00000000000001 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_RIGHT-SCALE_RIGHT_TWOCUBE1_C.csv: -------------------------------------------------------------------------------- 1 | -0.0081981738566294,-0.8198173856629402,20,40.990869283147006,135.00355840494183 2 | -0.040990869283147005,-2.4594521569888204,20,81.981738566294,135.01848680023625 3 | -0.10657626013618221,-4.0990869283147005,20,81.981738566294,135.04873203267545 4 | -0.204954346415735,-5.73872169964058,20,81.981738566294,135.09507585453412 5 | -0.3361251281218054,-7.378356470966461,20,81.98173856629403,135.1587033976765 6 | -0.5000886052543935,-9.01799124229234,20,81.98173856629397,135.2412191247126 7 | -0.696844777813499,-10.657626013618222,20,81.98173856629408,135.34466972399923 8 | -0.9263936457991223,-12.2972607849441,20,81.98173856629397,135.4715754491766 9 | -1.188735209211263,-13.936895556269981,20,81.98173856629397,135.624971922413 10 | -1.4838694680499214,-15.57653032759586,20,81.98173856629397,135.8084650637607 11 | -1.8117964223150975,-17.216165098921742,20,81.98173856629408,136.026302619489 12 | -2.172516072006791,-18.855799870247623,20,81.98173856629408,136.28346681447692 13 | -2.5660284171250027,-20.495434641573503,20,81.98173856629397,136.58579404387393 14 | -2.992333457669732,-22.135069412899384,20,81.98173856629408,136.9401293707558 15 | -3.4514311936409783,-23.77470418422526,20,81.98173856629386,137.35452609835698 16 | -3.943321625038742,-25.414338955551145,20,81.98173856629408,137.8385040717979 17 | -4.468004751863024,-27.053973726877025,20,81.98173856629408,138.40338496528358 18 | -5.025480574113823,-28.693608498202906,20,81.98173856629408,139.06272902702602 19 | -5.61574909179114,-30.333243269528783,20,81.98173856629386,139.83290608742993 20 | -6.238810304894974,-31.972878040854667,20,81.98173856629408,140.7338445020697 21 | -6.894664213425327,-33.61251281218054,20,81.98173856629386,141.7900152382943 22 | -7.5833108173821975,-35.25214758350642,20,81.98173856629408,143.03172342434294 23 | -8.304750116765584,-36.8917823548323,20,81.98173856629386,144.49679203683854 24 | -9.058982111575489,-38.53141712615818,20,81.98173856629408,146.2327195739431 25 | -9.846006801811912,-40.17105189748406,20,81.98173856629408,148.29934384203818 26 | -10.665824187474852,-41.81068666880994,20,81.98173856629408,150.7718725968872 27 | -11.51843426856431,-43.450321440135816,20,81.98173856629364,153.74368497099317 28 | -12.403837045080286,-45.089956211461704,20,81.98173856629408,157.3272448948706 29 | -13.322032517022777,-46.729590982787585,20,81.98173856629408,161.64930932315184 30 | -14.273020684391788,-48.369225754113465,20,81.98173856629408,166.833116881178 31 | -15.246944548238499,-49.02316063055769,20,32.69674382221138,172.8923247270348 32 | -16.217550761900835,-48.037460735676035,20,-49.2849947440827,179.58637331110532 33 | -17.1619036289011,-46.397825964350154,20,-81.98173856629408,-173.49465466101879 34 | -18.073463800474844,-44.758191193024274,20,-81.98173856629408,-166.7326166715357 35 | -18.95223127662207,-43.11855642169839,20,-81.98173856629408,-160.421639862496 36 | -19.798206057342774,-41.47892165037251,20,-81.98173856629408,-154.7464278854454 37 | -20.61138814263697,-39.83928687904664,20,-81.98173856629364,-149.77404398237516 38 | -21.391777532504644,-38.19965210772076,20,-81.98173856629408,-145.48532036324087 39 | -22.1393742269458,-36.56001733639487,20,-81.9817385662943,-141.81460273825738 40 | -22.854178225960435,-34.92038256506899,20,-81.98173856629408,-138.67978477993705 41 | -23.536189529548555,-33.28074779374311,20,-81.98173856629386,-135.99951757842854 42 | -24.185408137710162,-31.64111302241723,20,-81.98173856629408,-133.70097324806494 43 | -24.801834050445247,-30.00147825109135,20,-81.98173856629408,-131.72219600944217 44 | -25.385467267753814,-28.361843479765472,20,-81.98173856629386,-130.01185160360956 45 | -25.936307789635865,-26.722208708439588,20,-81.98173856629408,-128.5279547059775 46 | -26.454355616091398,-25.08257393711371,20,-81.98173856629386,-127.23634905467091 47 | -26.939610747120415,-23.44293916578783,20,-81.98173856629408,-126.10927387960828 48 | -27.392073182722914,-21.80330439446195,20,-81.98173856629408,-125.12413270363565 49 | -27.811742922898894,-20.163669623136073,20,-81.98173856629386,-124.26248291859682 50 | -28.198619967648355,-18.52403485181019,20,-81.98173856629418,-123.5092258327424 51 | -28.5527043169713,-16.88440008048431,20,-81.98173856629397,-122.85196514520257 52 | -28.873995970867732,-15.24476530915843,20,-81.98173856629397,-122.28050179509972 53 | -29.16249492933764,-13.605130537832549,20,-81.98173856629397,-121.78643707547303 54 | -29.41820119238103,-11.96549576650667,20,-81.98173856629397,-121.36286109592842 55 | -29.641114759997905,-10.325860995180788,20,-81.98173856629408,-121.00410830550345 56 | -29.831235632188264,-8.686226223854908,20,-81.98173856629403,-120.7055659862554 57 | -29.988563808952104,-7.046591452529029,20,-81.98173856629397,-120.46352480628002 58 | -30.113099290289426,-5.406956681203149,20,-81.98173856629397,-120.27506314423923 59 | -30.20484207620023,-3.7673219098772686,20,-81.98173856629404,-120.13795901960188 60 | -30.263792166684514,-2.1276871385513885,20,-81.981738566294,-120.05062493201785 61 | -30.29160838683447,-0.6539348764442242,20,-73.68761310535821,-120.00962072138162 62 | -30.29814773559891,-0.0,20,-32.69674382221121,-120.00000000000001 63 | -30.29814773559891,-0.0,20,0.0,-120.00000000000001 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_RIGHT_TWOCUBE3-SCALE_RIGHT_C.csv: -------------------------------------------------------------------------------- 1 | 0.008198193206953523,0.8198193206953523,20,40.99096603476762,239.98864418981512 2 | 0.04099096603476762,2.459457962086057,20,81.98193206953523,239.94028359193325 3 | 0.10657651169039581,4.099096603476762,20,81.98193206953523,239.84301293903286 4 | 0.2049548301738381,5.738735244867467,20,81.98193206953523,239.69571791870834 5 | 0.33612592148509446,7.378373886258172,20,81.98193206953523,239.4966878211656 6 | 0.5000897856241648,9.018012527648875,20,81.98193206953518,239.24356630375772 7 | 0.6968464225910495,10.65765116903958,20,81.98193206953529,238.93328194192068 8 | 0.9263958323857481,12.297289810430286,20,81.98193206953523,238.5619550480245 9 | 1.1887380150082607,13.93692845182099,20,81.98193206953523,238.1247758131474 10 | 1.4838729704585878,15.576567093211695,20,81.98193206953523,237.6158472038606 11 | 1.8118006987367286,17.2162057346024,20,81.98193206953523,237.02798382723313 12 | 2.172521199842684,18.855844375993108,20,81.98193206953535,236.35245533604444 13 | 2.566034473776453,20.49548301738381,20,81.98193206953513,235.57865944241334 14 | 2.9923405205380362,22.135121658774512,20,81.98193206953523,234.69370541881455 15 | 3.451439340127434,23.77476030016522,20,81.98193206953523,233.68188410386438 16 | 3.943330932544645,25.414398941555927,20,81.98193206953545,232.52399537080626 17 | 4.4680152977896705,27.054037582946627,20,81.981932069535,231.19650047158885 18 | 5.02549243586251,28.69367622433733,20,81.98193206953523,229.67046831796247 19 | 5.615762346763164,30.33331486572804,20,81.98193206953523,227.91030008412997 20 | 6.238825030491633,31.972953507118742,20,81.98193206953523,225.87226387520883 21 | 6.894680487047915,33.61259214850945,20,81.98193206953523,223.50298653799854 22 | 7.583328716432011,35.25223078990015,20,81.98193206953523,220.73830198470526 23 | 8.304769718643922,36.89186943129086,20,81.98193206953523,217.50335759948058 24 | 9.059003493683646,38.53150807268156,20,81.98193206953523,213.71577241206913 25 | 9.846030041551183,40.171146714072265,20,81.98193206953523,209.29493481050176 26 | 10.665849362246535,41.81078535546297,20,81.98193206953523,204.18159303486638 27 | 11.518461455769701,43.45042399685367,20,81.98193206953523,198.37044518660917 28 | 12.403866322120681,45.090062638244376,20,81.98193206953523,191.95026529399124 29 | 13.322063961299476,46.72970127963509,20,81.98193206953523,185.1300493575085 30 | 14.273054373306085,48.36933992102579,20,81.98193206953523,178.2196959505217 31 | 15.246981111554213,49.02333390378702,20,32.699699138061504,171.6193666860454 32 | 16.217591343043658,48.037689245157544,20,-49.28223293147373,165.72727025817642 33 | 17.161948741532903,46.39805060376684,20,-81.98193206953523,160.7240743570567 34 | 18.07351336719433,44.75841196237614,20,-81.98193206953523,156.55977541952947 35 | 18.952285220027946,43.11877332098543,20,-81.98193206953523,153.10807755487107 36 | 19.798264300033747,41.47913467959473,20,-81.98193206953523,150.24423082189912 37 | 20.611450607211733,39.83949603820402,20,-81.98193206953523,147.859473716918 38 | 21.391844141561904,38.19985739681331,20,-81.98193206953567,145.86423247752452 39 | 22.139444903084264,36.5602187554226,20,-81.98193206953523,144.18661721086838 40 | 22.85425289177881,34.9205801140319,20,-81.98193206953523,142.76949001557074 41 | 23.536268107645537,33.280941472641196,20,-81.981932069535,141.56748158918379 42 | 24.185490550684456,31.641302831250492,20,-81.98193206953523,140.54442606837847 43 | 24.80192022089556,30.001664189859788,20,-81.98193206953523,139.67130352735674 44 | 25.385557118278847,28.362025548469084,20,-81.98193206953523,138.92464176170253 45 | 25.93640124283432,26.72238690707838,20,-81.98193206953523,138.28529268528794 46 | 26.45445259456198,25.082748265687673,20,-81.98193206953523,137.737500525746 47 | 26.939711173461827,23.44310962429697,20,-81.98193206953523,137.26819253177132 48 | 27.39217697953386,21.803470982906266,20,-81.98193206953523,136.86643788196162 49 | 27.81185001277808,20.163832341515562,20,-81.98193206953523,136.52303351892914 50 | 28.198730273194485,18.524193700124854,20,-81.98193206953535,136.23018595946766 51 | 28.552817760783075,16.88455505873415,20,-81.98193206953513,135.98126600729643 52 | 28.87411247554385,15.244916417343447,20,-81.98193206953523,135.77061914286008 53 | 29.16261441747681,13.605277775952741,20,-81.98193206953523,135.59341870007285 54 | 29.41832358658196,11.965639134562036,20,-81.98193206953523,135.44555212238328 55 | 29.641239982859293,10.326000493171332,20,-81.98193206953523,135.32353294604349 56 | 29.831363606308813,8.686361851780626,20,-81.98193206953523,135.22443290647897 57 | 29.98869445693052,7.046723210389922,20,-81.98193206953523,135.14582987537383 58 | 30.11323253472441,5.407084568999218,20,-81.98193206953523,135.08576833491634 59 | 30.204977839690486,3.767445927608513,20,-81.98193206953523,135.04272986715594 60 | 30.26393037182875,2.127807286217808,20,-81.98193206953525,135.01561175172552 61 | 30.29174838451854,0.6539939827612278,20,-73.69066517282901,135.00296073175744 62 | 30.298288324346153,0.0,20,-32.69969913806139,135.0 63 | 30.298288324346153,0.0,20,0.0,135.0 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_RIGHT_TWOCUBE3-SCALE_RIGHT_L.csv: -------------------------------------------------------------------------------- 1 | 0.008198193206953523,0.8198193206953523,20,40.99096603476762,239.98864418981512 2 | 0.04706813785093174,1.9434972321989108,20,56.18389557517793,239.94028359193325 3 | 0.12487706513118532,3.8904463640126785,20,97.34745659068838,239.84301293903286 4 | 0.2417649889781941,5.844396192350439,20,97.69749141688804,239.69571791870834 5 | 0.3979468629173084,7.809093696955715,20,98.2348752302638,239.4966878211656 6 | 0.5937187564075656,9.788594674512854,20,98.975048877857,239.24356630375772 7 | 0.8294665886662221,11.787391612932833,20,99.93984692099889,238.93328194192068 8 | 1.105677828947968,13.810562014087294,20,101.15852005772302,238.5619550480245 9 | 1.4229568068618814,15.863948895695668,20,102.66934408041877,238.1247758131474 10 | 1.7820444383308813,17.954381573449997,20,104.52163388771648,237.6158472038606 11 | 2.183843496151949,20.08995289105337,20,106.77856588016851,237.02798382723313 12 | 2.6294508319502703,22.280366789916073,20,109.52069494313528,236.35245533604444 13 | 3.120198437885582,24.53738029676557,20,112.8506753424747,235.57865944241334 14 | 3.657705746867296,26.87536544908572,20,116.8992576160076,234.69370541881455 15 | 4.243946167716948,29.312021042482613,20,121.83277966984456,233.68188410386438 16 | 4.881331505803323,31.869266904318728,20,127.86229309180587,232.52399537080626 17 | 5.57281833739803,34.574341579735375,20,135.25373377083233,231.19650047158885 18 | 6.322040189559119,37.461092608054436,20,144.3375514159532,229.67046831796247 19 | 7.133467460480439,40.57136354606603,20,155.5135469005797,227.91030008412997 20 | 8.01259098429436,43.956176190695984,20,169.2406322314975,225.87226387520883 21 | 8.966110674912807,47.67598453092243,20,185.99041701132234,223.50298653799854 22 | 10.00207889925106,51.79841121691266,20,206.12133429951146,220.73830198470526 23 | 11.129885172990326,56.39031368696323,20,229.5951235025285,217.50335759948058 24 | 12.359856909773207,61.49858683914402,20,255.41365760903932,213.71577241206913 25 | 13.702089179945016,67.1116135085904,20,280.651333472319,209.29493481050176 26 | 15.1639845055324,73.09476627936928,20,299.1576385389439,204.18159303486638 27 | 16.746168228390314,79.1091861428958,20,300.72099317632603,198.37044518660917 28 | 18.43747091902473,84.56513453172066,20,272.79741944124333,191.95026529399124 29 | 20.211673431041397,88.71012560083341,20,207.24955345563708,185.1300493575085 30 | 22.02994101653265,90.91337927456256,20,110.16268368645757,178.2196959505217 31 | 23.83229181178847,90.11753976279115,20,-39.79197558857037,171.6193666860454 32 | 25.542570351889886,85.51392700507077,20,-230.18063788601927,165.72727025817642 33 | 27.115147893928526,78.62887710193186,20,-344.2524951569451,160.7240743570567 34 | 28.54969785462663,71.72749803490528,20,-345.0689533513293,156.55977541952947 35 | 29.86202434549587,65.61632454346203,20,-305.55867457216266,153.10807755487107 36 | 31.067759486722576,60.286757061335244,20,-266.4783741063389,150.24423082189912 37 | 32.18054288327923,55.63916982783258,20,-232.3793616751329,147.859473716918 38 | 33.21161373028987,51.55354235053201,20,-204.28137386502843,145.86423247752452 39 | 34.169995612423314,47.91909410667211,20,-181.72241219299536,144.18661721086838 40 | 35.06286230143104,44.64333445038613,20,-163.78798281429889,142.76949001557074 41 | 35.895911077512054,41.65243880405088,20,-149.5447823167624,141.56748158918379 42 | 36.67368413933698,38.88865309124641,20,-138.1892856402237,140.54442606837847 43 | 37.39982660016822,36.30712304156205,20,-129.07650248421777,139.67130352735674 44 | 38.07728699048559,33.87301951586844,20,-121.70517628468062,138.92464176170253 45 | 38.708470816361825,31.55919129381172,20,-115.69141110283607,138.28529268528794 46 | 39.29535752724585,29.344335544201094,20,-110.74278748053133,137.737500525746 47 | 39.83958956767544,27.21160202147983,20,-106.63667613606309,137.26819253177132 48 | 40.34254032276732,25.14753775459433,20,-103.20321334427496,136.86643788196162 49 | 40.805366128918884,23.14129030757798,20,-100.31237235081757,136.52303351892914 50 | 41.229046238573865,21.184005482748965,20,-97.86424124145073,136.23018595946766 51 | 41.614413627433066,19.26836944295994,20,-95.78180198945132,135.98126600729643 52 | 41.96217881254722,17.388259255707748,20,-94.00550936260957,135.77061914286008 53 | 42.27294829544929,15.53847414510346,20,-92.4892555302143,135.59341870007285 54 | 42.547238851001794,13.714527777625074,20,-91.19731837391934,135.44555212238328 55 | 42.785488584534974,11.912486676659046,20,-90.1020550483014,135.32353294604349 56 | 42.98806546119271,10.12884383288683,20,-89.18214218861081,135.22443290647897 57 | 43.155273847114735,8.360419296101112,20,-88.42122683928592,135.14582987537383 58 | 43.28735947477524,6.604281383025258,20,-87.80689565379265,135.08576833491634 59 | 43.38451315103348,4.85768381291187,20,-87.3298785056694,135.04272986715594 60 | 43.446873445472804,3.11801472196632,20,-86.9834545472775,135.01561175172552 61 | 43.47628123218536,1.4703893356276967,20,-82.38126931693115,135.00296073175744 62 | 43.483193228634846,0.345599822474286,20,-56.23947565767053,135.0 63 | 43.483193228634846,0.0,20,-17.2799911237143,135.0 64 | -------------------------------------------------------------------------------- /robot/profiles/SCALE_RIGHT_TWOCUBE3-SCALE_RIGHT_R.csv: -------------------------------------------------------------------------------- 1 | 0.008198193206953523,0.8198193206953523,20,40.99096603476762,239.98864418981512 2 | 0.03491379404652038,1.335780041978343,20,25.79803606414953,239.94028359193325 3 | 0.08827594228896869,2.6681074121224153,20,66.61636850720362,239.84301293903286 4 | 0.16814459928932698,3.9934328500179137,20,66.2662718947749,239.69571791870834 5 | 0.2743047779964666,5.308008935356979,20,65.72880426695326,239.4966878211656 6 | 0.40646034598578706,6.607778399466026,20,64.98847320545237,239.24356630375772 7 | 0.5642253062499061,7.888248013205947,20,64.02348068699607,238.93328194192068 8 | 0.7471120806463627,9.14433871982283,20,62.8045353308441,238.5619550480245 9 | 0.9545161971198765,10.370205823675686,20,61.29335519264285,238.1247758131474 10 | 1.1856965361580316,11.559016951907767,20,59.44055641160402,237.6158472038606 11 | 1.4397500579514513,12.702676089670975,20,57.18295688816037,237.02798382723313 12 | 1.7155795462869057,13.791474416772719,20,54.43991635508722,236.35245533604444 13 | 2.0118525070533546,14.813648038322446,20,51.10868107748639,235.57865944241334 14 | 2.326948818259219,15.754815560293212,20,47.05837609853825,234.69370541881455 15 | 2.658894106821378,16.597264428107973,20,42.12244339073807,233.68188410386438 16 | 3.005275214253561,17.31905537160914,20,36.08954717505837,232.52399537080626 17 | 3.363133647671943,17.892921670919115,20,28.693314965498825,231.19650047158885 18 | 3.7288331234272247,18.284973787764073,20,19.602605842247897,229.67046831796247 19 | 4.097899267114355,18.453307184356508,20,8.416669829621643,227.91030008412997 20 | 4.464835442064708,18.346808747517677,20,-5.324921841941447,225.87226387520883 21 | 4.822933250416241,17.90489041757666,20,-22.09591649705096,223.50298653799854 22 | 5.1641279420569965,17.059734582037702,20,-42.257791776947776,220.73830198470526 23 | 5.479012150686079,15.74421043145413,20,-65.77620752917869,217.50335759948058 24 | 5.757233513068077,13.911068119099909,20,-91.65711561771107,213.71577241206913 25 | 5.988664228501202,11.571535771656228,20,-116.97661737218405,209.29493481050176 26 | 6.165863904270956,8.859983788487673,20,-135.5775991584277,204.18159303486638 27 | 6.288173936139571,6.115501593430743,20,-137.22410975284654,198.37044518660917 28 | 6.366754264288768,3.929016407459866,20,-109.32425929854381,191.95026529399124 29 | 6.427861336862719,3.0553536286975613,20,-43.68313893811524,185.1300493575085 30 | 6.5104212430369515,4.127995308711647,20,53.63208400070427,178.2196959505217 31 | 6.654847792750788,7.221327485691818,20,154.66660884900855,171.6193666860454 32 | 6.884935463611297,11.50438354302544,20,214.15280286668116,165.72727025817642 33 | 7.200473418185082,15.776897728689208,20,213.62570928318837,160.7240743570567 34 | 7.588651878978379,19.408923039664824,20,181.60126554878082,156.55977541952947 35 | 8.03360356561174,22.247584331668115,20,141.9330646001645,153.10807755487107 36 | 8.519650590757466,24.302351257286343,20,102.73834628091143,150.24423082189912 37 | 9.033122487372374,25.673594830745365,20,68.56217867295123,147.859473716918 38 | 9.562759886327056,26.48186994773406,20,40.413755849434686,145.86423247752452 39 | 10.099526139319709,26.838312649632595,20,17.82213509492667,144.18661721086838 40 | 10.636239000407993,26.8356430544142,20,-0.13347976091983454,142.76949001557074 41 | 11.167195649398998,26.5478324495502,20,-14.39053024319994,141.56748158918379 42 | 11.687850228638764,26.032728961988322,20,-25.7551743780938,140.54442606837847 43 | 12.194555181500807,25.335247643102157,20,-34.87406594430833,139.67130352735674 44 | 12.684360327640803,24.490257306999734,20,-42.24951680512117,138.92464176170253 45 | 13.154859035699541,23.524935402936944,20,-48.26609520313955,138.28529268528794 46 | 13.604071082633098,22.460602346677845,20,-53.21665281295477,137.737500525746 47 | 14.03035348695502,21.3141202160961,20,-57.3241065290874,137.26819253177132 48 | 14.432332490494325,20.0989501769652,20,-60.758501956544904,136.86643788196162 49 | 14.808851494242516,18.8259501874096,20,-63.64999947777996,136.52303351892914 50 | 15.158931063265038,17.503978451126084,20,-66.09858681417582,136.23018595946766 51 | 15.48173809264888,16.140351469192172,20,-68.1813490966957,135.98126600729643 52 | 15.776561975187011,14.741194126906516,20,-69.95786711428275,135.77061914286008 53 | 16.042796146212055,13.311708551252234,20,-71.47427878271404,135.59341870007285 54 | 16.279923787003355,11.856382039565013,20,-72.76632558436114,135.44555212238328 55 | 16.48750676188114,10.37914874388907,20,-73.86166478379707,135.32353294604349 56 | 16.665177084644835,8.883516138184875,20,-74.7816302852098,135.22443290647897 57 | 16.81263037531578,7.372664533547349,20,-75.54258023187629,135.14582987537383 58 | 16.92962089182462,5.8495258254419875,20,-76.1569354052681,135.08576833491634 59 | 17.015957821389947,4.316846478266195,20,-76.63396735878962,135.04272986715594 60 | 17.07150258997944,2.777238429474628,20,-76.98040243957836,135.01561175172552 61 | 17.097730828523538,1.3114119272050377,20,-73.2913251134795,135.00296073175744 62 | 17.103898711907128,0.308394169179639,20,-50.15088790126993,135.0 63 | 17.103898711907128,0.0,20,-15.419708458981951,135.0 64 | -------------------------------------------------------------------------------- /robot/profiles/SWITCH_LEFT_TWOCUBE3-SWITCH_LEFT_C.csv: -------------------------------------------------------------------------------- 1 | 0.008054091658165001,0.8054091658165002,20,40.27045829082501,44.9866716464776 2 | 0.040270458290825005,2.4162274974495004,20,80.54091658165002,44.93143140822573 3 | 0.10470319155614502,4.027045829082501,20,80.54091658165002,44.82171251781528 4 | 0.20135229145412506,5.637864160715501,20,80.54091658164998,44.65903534311792 5 | 0.33021775798476505,7.248682492348502,20,80.54091658165008,44.445673166122866 6 | 0.49129959114806515,8.859500823981502,20,80.54091658165002,44.18464277790035 7 | 0.6845977909440253,10.470319155614503,20,80.54091658165002,43.879690948433826 8 | 0.9101123573726453,12.081137487247503,20,80.54091658164997,43.53527619630879 9 | 1.1678432904339253,13.691955818880501,20,80.54091658164997,43.156545452461536 10 | 1.4577905901278654,15.302774150513503,20,80.54091658165008,42.74930554974361 11 | 1.7799542564544657,16.913592482146502,20,80.54091658164997,42.31998995943446 12 | 2.134334289413726,18.524410813779504,20,80.54091658165008,41.875621816082166 13 | 2.5209306890056458,20.135229145412502,20,80.54091658164985,41.42377497213789 14 | 2.9397434552302255,21.746047477045508,20,80.54091658165028,40.97253554817796 15 | 3.390772588087466,23.356865808678503,20,80.54091658164985,40.53046713022711 16 | 3.874018087577366,24.967684140311505,20,80.54091658165008,40.10658335200234 17 | 4.389479953699927,26.578502471944503,20,80.54091658164985,39.710332038893874 18 | 4.937158186455147,28.18932080357751,20,80.54091658165028,39.351595353133874 19 | 5.517052785843026,29.80013913521051,20,80.54091658165008,39.040710454969314 20 | 6.129163751863566,31.410957466843506,20,80.54091658164985,38.788515077113274 21 | 6.773491084516766,33.021775798476504,20,80.54091658164985,38.60642210544117 22 | 7.450034783802625,34.63259413010951,20,80.54091658165028,38.50652672947208 23 | 8.158794849721145,36.24341246174251,20,80.54091658164985,38.50174888423026 24 | 8.899771282272324,37.85423079337551,20,80.54091658165008,38.60601233742198 25 | 9.672964081456165,39.465049125008505,20,80.54091658164985,38.83445947982223 26 | 10.478373247272666,41.075867456641504,20,80.54091658165008,39.20369694524457 27 | 11.315998779721825,42.68668578827451,20,80.54091658165008,39.73206045581768 28 | 12.185840678803645,44.29750411990751,20,80.54091658165008,40.43987594208412 29 | 13.087898944518125,45.90832245154051,20,80.54091658165008,41.34967535759222 30 | 14.022173576865265,47.51914078317351,20,80.54091658165008,42.4862961186837 31 | 14.988664575845066,49.12995911480652,20,80.54091658165008,43.876748734258335 32 | 15.973269293055766,49.33051260626361,20,10.027674572854876,45.52435345311106 33 | 16.94577689677928,47.920247766087705,20,-70.5132420087952,47.385914339069764 34 | 17.888073668784703,46.30942943445471,20,-80.54091658165008,49.414040257731166 35 | 18.79815407415747,44.698611102821715,20,-80.54091658164964,51.5810733942479 36 | 19.676018112897573,43.08779277118871,20,-80.54091658165008,53.85969012404016 37 | 20.521665785005013,41.47697443955571,20,-80.54091658165008,56.21981427705972 38 | 21.3350970904798,39.86615610792271,20,-80.54091658165008,58.629574771581716 39 | 22.11631202932192,38.25533777628971,20,-80.54091658165008,61.056603292923164 40 | 22.865310601531387,36.64451944465671,20,-80.54091658164985,63.46949191922476 41 | 23.58209280710819,35.0337011130237,20,-80.54091658165028,65.8391963543975 42 | 24.266658646052335,33.422882781390705,20,-80.54091658164985,68.14018821454856 43 | 24.919008118363816,31.81206444975771,20,-80.54091658164985,70.35122306269423 44 | 25.539141224042645,30.20124611812471,20,-80.54091658165008,72.45567662796424 45 | 26.12705796308881,28.590427786491702,20,-80.54091658165028,74.4414822401413 46 | 26.68275833550231,26.979609454858707,20,-80.54091658164985,76.30075772842848 47 | 27.206242341283154,25.36879112322571,20,-80.54091658164985,78.0292329603456 48 | 27.69750998043134,23.757972791592707,20,-80.54091658165008,79.62558471868152 49 | 28.156561252946865,22.147154459959708,20,-80.54091658165008,81.09076413579537 50 | 28.58339615882973,20.536336128326706,20,-80.54091658165008,82.42737452034085 51 | 28.97801469807993,18.925517796693704,20,-80.54091658165008,83.63913158849738 52 | 29.340416870697478,17.314699465060706,20,-80.54091658164997,84.73041798458453 53 | 29.67060267668236,15.703881133427702,20,-80.54091658165008,85.70593039506542 54 | 29.968572116034586,14.093062801794703,20,-80.54091658164997,86.57040972251808 55 | 30.23432518875415,12.482244470161705,20,-80.54091658164997,87.32844135143576 56 | 30.467861894841054,10.871426138528703,20,-80.54091658165008,87.984311812086 57 | 30.6691822342953,9.260607806895703,20,-80.54091658164997,88.54190915815362 58 | 30.838286207116884,7.649789475262703,20,-80.54091658165002,89.00465606374935 59 | 30.975173813305805,6.038971143629701,20,-80.54091658165008,89.37546663990545 60 | 31.079845052862073,4.428152811996702,20,-80.54091658164998,89.65671980319772 61 | 31.152299925785677,2.817334480363701,20,-80.54091658165002,89.8502437747697 62 | 31.19253843207662,1.206516148730701,20,-80.54091658165001,89.95730765810416 63 | 31.2066091284785,0.20055349145710036,20,-50.298132863680024,89.99467662523226 64 | 31.208614663393075,0.0,20,-10.027674572855018,90.0 65 | 31.208614663393075,0.0,20,0.0,90.0 66 | -------------------------------------------------------------------------------- /robot/profiles/SWITCH_LEFT_TWOCUBE3-SWITCH_LEFT_L.csv: -------------------------------------------------------------------------------- 1 | 0.008054091658165001,0.8054091658165002,20,40.27045829082501,44.9866716464776 2 | 0.04721215000629283,1.957902917406391,20,57.624687579494555,44.93143140822573 3 | 0.12543255397151984,3.9110201982613497,20,97.65586404274792,44.82171251781528 4 | 0.24252423164955417,5.854583883901717,20,97.17818428201836,44.65903534311792 5 | 0.3982014906808711,7.783862951565845,20,96.46395338320637,44.445673166122866 6 | 0.5920852027925785,9.694185605585373,20,95.51613270097643,44.18464277790035 7 | 0.8237045070544647,11.58096521309431,20,94.33898037544681,43.879690948433826 8 | 1.092499103847279,13.439729839640716,20,92.93823132732027,43.53527619630879 9 | 1.397822190901966,15.266154352734342,20,91.3212256546813,43.156545452461536 10 | 1.738944048997596,17.05609290478149,20,89.49692760235749,42.74930554974361 11 | 2.115056224654691,18.80560878285474,20,87.47579390366248,42.31998995943446 12 | 2.5252761783241637,20.510997683473647,20,85.26944503094533,41.875621816082166 13 | 2.968652179025248,22.16880003505423,20,82.89011757902915,41.42377497213789 14 | 3.4441681356399165,23.775797830733413,20,80.34988978395923,40.97253554817796 15 | 3.950747968594535,25.328991647730923,20,77.65969084987549,40.53046713022711 16 | 4.487259051864719,26.8255541635092,20,74.82812578891372,40.10658335200234 17 | 5.052514200584673,28.262757435997703,20,71.86016362442517,39.710332038893874 18 | 5.64527164694818,29.637872318175372,20,68.75574410888345,39.351595353133874 19 | 6.2642324355870125,30.948039431941634,20,65.50835568831312,39.040710454969314 20 | 6.908034687457526,32.19011259352568,20,62.10365807920245,38.788515077113274 21 | 7.575244216336303,33.360476443938836,20,58.51819252065763,38.60642210544117 22 | 8.264341050427486,34.45484170455916,20,54.7182630310162,38.50652672947208 23 | 8.973701516334287,35.468023295340004,20,50.659079539042345,38.50174888423026 24 | 9.701575714498164,36.39370990819384,20,46.28433064269172,38.60601233742198 25 | 10.446060503513907,37.22423945078712,20,41.52647712966388,38.83445947982223 26 | 11.205068604753983,37.95040506200378,20,36.30828056083302,39.20369694524457 27 | 11.976295284609662,38.561333992783986,20,30.546446539010333,39.73206045581768 28 | 12.757185498017328,39.044510670383254,20,24.15883387996354,40.43987594208412 29 | 13.544906719360704,39.386061067168754,20,17.077519839275066,41.34967535759222 30 | 14.336336396098615,39.57148383689554,20,9.271138486339032,42.4862961186837 31 | 15.128078538639837,39.58710712706105,20,0.7811645082757853,43.876748734258335 32 | 15.905612353715346,38.87669075377553,20,-35.52081866427626,45.52435345311106 33 | 16.6441568358474,36.92722410660269,20,-97.47333235864222,47.385914339069764 34 | 17.331555901716957,34.369953293477884,20,-127.86354065623985,49.414040257731166 35 | 17.969280877949643,31.886248811634267,20,-124.18522409218099,51.5810733942479 36 | 18.560766506920654,29.57428144855053,20,-115.59836815418683,53.85969012404016 37 | 19.10979340284011,27.451344795972865,20,-106.14683262888329,56.21981427705972 38 | 19.620367625609514,25.528711138470214,20,-96.13168287513247,58.629574771581716 39 | 20.096557544095727,23.80949592431064,20,-85.96076070797879,61.056603292923164 40 | 20.542310629279118,22.28765425916957,20,-76.09208325705333,63.46949191922476 41 | 20.961277111523106,20.948324112199384,20,-66.96650734850952,65.8391963543975 42 | 21.356665196719323,19.769404259810766,20,-58.945992619430704,68.14018821454856 43 | 21.73114458375225,18.723969351646232,20,-52.271745408226714,70.35122306269423 44 | 22.086804250068074,17.782983315791288,20,-47.049301792747265,72.45567662796424 45 | 22.42516035014759,16.917805003975598,20,-43.25891559078457,74.4414822401413 46 | 22.74720313557222,16.10213927123143,20,-40.78328663720842,76.30075772842848 47 | 23.053468917116327,15.313289077205381,20,-39.44250970130238,78.0292329603456 48 | 23.34412367149423,14.532737718895246,20,-39.027567915506744,79.62558471868152 49 | 23.619047572697486,13.74619506016273,20,-39.32713293662577,81.09076413579537 50 | 23.87791319016169,12.943280873210163,20,-40.14570934762836,82.42737452034085 51 | 24.120253325596437,12.11700677173761,20,-41.31370507362761,83.63913158849738 52 | 24.345516997251497,11.263183582752909,20,-42.691159449235144,84.73041798458453 53 | 24.553113788786032,10.379839576726825,20,-44.16720030130414,85.70593039506542 54 | 24.742447753834945,9.466698252445685,20,-45.65706621405702,86.57040972251808 55 | 24.912942517205913,8.524738168548256,20,-47.098004194871436,87.32844135143576 56 | 25.064059283566337,7.555838318021278,20,-48.444992526348884,87.984311812086 57 | 25.195309354956667,6.562503569516529,20,-49.66673742523748,88.54190915815362 58 | 25.30626253372733,5.547658938533113,20,-50.742231549170775,89.00465606374935 59 | 25.396552550260587,4.51450082666275,20,-51.65790559351816,89.37546663990545 60 | 25.465880404401705,3.46639270705596,20,-52.40540598033951,89.65671980319772 61 | 25.514016314245552,2.406795492192345,20,-52.97986074318074,89.8502437747697 62 | 25.54080077226942,1.3392229011934607,20,-53.37862954994421,89.95730765810416 63 | 25.550175545206287,0.46873864684333666,20,-43.524212717506195,89.99467662523226 64 | 25.551512125658792,0.06682902262525213,20,-20.09548121090423,90.0 65 | 25.551512125658792,0.0,20,-3.3414511312626063,90.0 66 | -------------------------------------------------------------------------------- /robot/profiles/SWITCH_LEFT_TWOCUBE3-SWITCH_LEFT_R.csv: -------------------------------------------------------------------------------- 1 | 0.008054091658165001,0.8054091658165002,20,40.27045829082501,44.9866716464776 2 | 0.03332876413251536,1.2637336237175179,20,22.916222895050883,44.93143140822573 3 | 0.08397380695404727,2.532252141076596,20,63.425925867953886,44.82171251781528 4 | 0.16018026419314457,3.8103228619548655,20,63.90353604391348,44.65903534311792 5 | 0.262233789313325,5.102676256009021,20,64.61766970270777,44.445673166122866 6 | 0.3905134648519799,6.413983776932743,20,65.5653760461861,44.18464277790035 7 | 0.5454901038809313,7.748831951447575,20,66.74240872574161,43.879690948433826 8 | 0.727723960877791,9.11169284984298,20,68.14304491977028,43.53527619630879 9 | 0.9378618015417385,10.506892033197373,20,69.75995916771957,43.156545452461536 10 | 1.176633322149262,11.938576030376181,20,71.58419985894044,42.74930554974361 11 | 1.4448469717656793,13.410682480820864,20,73.6053225222341,42.31998995943446 12 | 1.7433853076391304,14.926916793672545,20,75.81171564258415,41.875621816082166 13 | 2.0732001024520472,16.49073974064585,20,78.19114734866527,41.42377497213789 14 | 2.4353075133370643,18.10537054425083,20,80.73154018024911,40.97253554817796 15 | 2.830783708359458,19.773809751119693,20,83.42196034344296,40.53046713022711 16 | 3.2607614196599557,21.498885565024896,20,86.25379069526022,40.10658335200234 17 | 3.726427948069254,23.283326420464906,20,89.22204277200049,39.710332038893874 18 | 4.229025177364494,25.129861464761987,20,92.32675221485405,39.351595353133874 19 | 4.76985216372655,27.0413493181028,20,95.5743926670406,39.040710454969314 20 | 5.350270854194761,29.020934523410627,20,98.97926026539136,38.788515077113274 21 | 5.971715446404269,31.0722296104754,20,102.56475435323868,38.60642210544117 22 | 6.635705837062823,33.19951953292769,20,106.36449612261437,38.50652672947208 23 | 7.343865499478269,35.407983120772286,20,110.42317939222983,38.50174888423026 24 | 8.097943958090319,37.70392293060255,20,114.79699049151347,38.60601233742198 25 | 8.899843738450963,40.09498901803213,20,119.55330437147882,38.83445947982223 26 | 9.751651175875619,42.590371871232875,20,124.76914266003732,39.20369694524457 27 | 10.65566961860057,45.2009221362475,20,130.5275132507311,39.73206045581768 28 | 11.614452133775353,47.93912575873915,20,136.9101811245825,40.43987594208412 29 | 12.630828482137707,50.81881741811759,20,143.98458296892213,41.34967535759222 30 | 13.707917422631178,53.85444702467354,20,151.78148032779737,42.4862961186837 31 | 14.849109835884988,57.05962066269044,20,160.25868190084512,43.876748734258335 32 | 16.040717596941906,59.58038805284596,20,126.03836950777581,45.52435345311106 33 | 17.24710276149761,60.31925822778522,20,36.94350874696315,47.385914339069764 34 | 18.444198837091808,59.854803779709826,20,-23.222722403769584,49.414040257731166 35 | 19.626526167022238,59.11636649652163,20,-36.92186415941008,51.5810733942479 36 | 20.790652892773842,58.206336287580186,20,-45.501510447072185,53.85969012404016 37 | 21.932801744058487,57.1074425642322,20,-54.94468616739909,56.21981427705972 38 | 23.048970198861998,55.8084227401756,20,-64.95099120283002,58.629574771581716 39 | 24.135093316026985,54.30615585824915,20,-75.1133440963227,61.056603292923164 40 | 25.18722665255348,52.606666826324805,20,-84.97445159621705,63.46949191922476 41 | 26.20172238044751,50.72478639470156,20,-94.09402158116215,65.8391963543975 42 | 27.175373944993048,48.68257822727685,20,-102.11040837123589,68.14018821454856 43 | 28.10551253033581,46.50692926713808,20,-108.78244800693842,70.35122306269423 44 | 28.99004934610676,44.226840788547314,20,-114.00442392953819,72.45567662796424 45 | 29.827467862151117,41.87092580221785,20,-117.79574931647342,74.4414822401413 46 | 30.616777051242337,39.46545945456093,20,-120.273317382846,76.30075772842848 47 | 31.357439575529018,37.03312621433396,20,-121.61666201134824,78.0292329603456 48 | 32.049288317348974,34.59243709099793,20,-122.03445616680128,79.62558471868152 49 | 32.69244194351154,32.15768130812824,20,-121.73778914348487,81.09076413579537 50 | 33.28722677939839,29.739241794342686,20,-120.92197568927762,82.42737452034085 51 | 33.83410901315915,27.34411168803796,20,-119.75650531523624,83.63913158849738 52 | 34.33363873044023,24.97648586405386,20,-118.38129119920501,84.73041798458453 53 | 34.78640557527091,22.638342241534414,20,-116.90718112597224,85.70593039506542 54 | 35.193004836097536,20.329963041331133,20,-115.4189600101641,86.57040972251808 55 | 35.55401234151431,18.050375270838526,20,-113.97938852463034,87.32844135143576 56 | 35.86996643683349,15.79770476595897,20,-112.63352524397777,87.984311812086 57 | 36.14135545607624,13.569450962137637,20,-111.41269019106664,88.54190915815362 58 | 36.3686093034031,11.362692366342884,20,-110.33792978973767,89.00465606374935 59 | 36.55209402170739,9.174235915214513,20,-109.4228225564185,89.37546663990545 60 | 36.69210843601881,7.000720715571353,20,-108.67575998215808,89.65671980319772 61 | 36.78888220270554,4.8386883343361164,20,-108.10161906176178,89.8502437747697 62 | 36.84257474610027,2.684627169736593,20,-107.70305822997616,89.95730765810416 63 | 36.86134136486657,0.9383309383148069,20,-87.31481157108931,89.99467662523226 64 | 36.864015855350345,0.13372452418892827,20,-40.23032070629393,90.0 65 | 36.864015855350345,0.0,20,-6.686226209446413,90.0 66 | -------------------------------------------------------------------------------- /robot/profiles/SWITCH_RIGHT_TWOCUBE3-SWITCH_RIGHT_C.csv: -------------------------------------------------------------------------------- 1 | 0.008120214300587464,0.8120214300587463,20,40.601071502937316,149.99758204821157 2 | 0.04060107150293731,2.4360642901762386,20,81.20214300587462,149.98744630702024 3 | 0.10556278590763703,4.060107150293732,20,81.20214300587463,149.9668885544205 4 | 0.2030053575146866,5.684150010411224,20,81.20214300587463,149.93533517733917 5 | 0.332928786324086,7.308192870528718,20,81.20214300587466,149.89192300694862 6 | 0.49533307233583534,8.932235730646209,20,81.20214300587462,149.83549582992026 7 | 0.6902182155499345,10.556278590763702,20,81.20214300587462,149.7645994210318 8 | 0.9175842159663835,12.180321450881195,20,81.20214300587462,149.67747482642287 9 | 1.1774310735851823,13.804364310998686,20,81.20214300587462,149.57204956045572 10 | 1.469758788406331,15.428407171116179,20,81.20214300587462,149.44592631350116 11 | 1.7945673604298296,17.052450031233672,20,81.20214300587462,149.29636870408385 12 | 2.1518567896556777,18.676492891351167,20,81.20214300587472,149.1202835510861 13 | 2.541627076083876,20.300535751468658,20,81.20214300587462,148.9141990891104 14 | 2.9638782197144242,21.92457861158615,20,81.20214300587462,148.67423851369443 15 | 3.4186102205473223,23.54862147170364,20,81.20214300587462,148.39608822568428 16 | 3.90582307858257,25.172664331821135,20,81.20214300587462,148.07496016614786 17 | 4.425516793820168,26.796707191938626,20,81.20214300587462,147.7055477090741 18 | 4.977691366260116,28.420750052056125,20,81.20214300587482,147.28197475039812 19 | 5.562346795902412,30.044792912173612,20,81.20214300587439,146.797737939111 20 | 6.179483082747059,31.668835772291104,20,81.20214300587462,146.24564252034855 21 | 6.829100226794057,33.292878632408595,20,81.20214300587462,145.61773310691612 22 | 7.511198228043404,34.91692149252609,20,81.20214300587482,144.90522201284054 23 | 8.225777086495102,36.540964352643584,20,81.20214300587462,144.09841979271607 24 | 8.972836802149148,38.165007212761076,20,81.20214300587439,143.18667560814785 25 | 9.752377375005544,39.78905007287857,20,81.20214300587482,142.15833935225857 26 | 10.564398805064291,41.41309293299606,20,81.20214300587462,141.00076350655394 27 | 11.408901092325387,43.037135793113556,20,81.20214300587462,139.70037085635508 28 | 12.285884236788833,44.66117865323105,20,81.20214300587462,138.24282458209942 29 | 13.19534823845463,46.28522151334854,20,81.20214300587462,136.6133493751603 30 | 14.137293097322775,47.90926437346603,20,81.20214300587462,134.7972642597495 31 | 15.11171881339327,49.53330723358352,20,81.20214300587462,132.7807955079335 32 | 16.10642018996347,49.936830423436355,20,20.17615949264157,130.58010975753322 33 | 17.09295160172955,48.71631075317171,20,-61.02598351323217,128.26099215257676 34 | 18.05103738819181,47.09226789305422,20,-81.20214300587462,125.88729892043992 35 | 18.97664231745172,45.46822503293673,20,-81.20214300587462,123.4918064541344 36 | 19.86976638950928,43.844182172819224,20,-81.20214300587504,121.09843261858711 37 | 20.73040960436449,42.22013931270173,20,-81.20214300587462,118.73041926385105 38 | 21.55857196201735,40.59609645258425,20,-81.20214300587418,116.40936351626401 39 | 22.354253462467856,38.97205359246675,20,-81.20214300587504,114.15442034335031 40 | 23.117454105716014,37.34801073234926,20,-81.20214300587462,111.9817515144622 41 | 23.848173891761828,35.72396787223177,20,-81.20214300587439,109.904249598161 42 | 24.546412820605287,34.09992501211428,20,-81.20214300587462,107.93152137285264 43 | 25.212170892246398,32.47588215199678,20,-81.20214300587482,106.07008284076257 44 | 25.84544810668516,30.851839291879287,20,-81.20214300587462,104.32370202654035 45 | 26.44624446392157,29.2277964317618,20,-81.20214300587439,102.69382459165097 46 | 27.01455996395563,27.603753571644308,20,-81.20214300587462,101.18002647394856 47 | 27.55039460678734,25.97971071152681,20,-81.20214300587482,99.78045193554321 48 | 28.0537483924167,24.35566785140932,20,-81.20214300587462,98.49221031149817 49 | 28.524621320843714,22.731624991291827,20,-81.20214300587462,97.31171759055617 50 | 28.963013392068376,21.107582131174333,20,-81.20214300587462,96.23497867255288 51 | 29.36892460609069,19.48353927105684,20,-81.20214300587462,95.25781258141548 52 | 29.742354962910653,17.85949641093935,20,-81.20214300587462,94.37602659805017 53 | 30.083304462528265,16.235453550821855,20,-81.20214300587462,93.5855469585545 54 | 30.391773104943525,14.611410690704362,20,-81.20214300587472,92.88251403061714 55 | 30.667760890156437,12.987367830586873,20,-81.2021430058745,92.26334941662067 56 | 30.911267818166998,11.363324970469378,20,-81.20214300587472,91.72480150394374 57 | 31.12229388897521,9.739282110351885,20,-81.20214300587462,91.2639749611546 58 | 31.300839102581076,8.115239250234394,20,-81.20214300587462,90.87834865242698 59 | 31.44690345898459,6.491196390116901,20,-81.20214300587462,90.56578549193013 60 | 31.560486958185752,4.867153529999409,20,-81.20214300587458,90.32453701499918 61 | 31.641589600184567,3.243110669881916,20,-81.20214300587466,90.1532447038345 62 | 31.69021138498103,1.6190678097644235,20,-81.20214300587462,90.05093963226132 63 | 31.7104372949772,0.40352318985283864,20,-60.77723099557925,90.00846753558393 64 | 31.71447252687573,0.0,20,-20.176159492641933,89.99999999999999 65 | 31.71447252687573,0.0,20,0.0,89.99999999999999 66 | -------------------------------------------------------------------------------- /robot/profiles/SWITCH_RIGHT_TWOCUBE3-SWITCH_RIGHT_R.csv: -------------------------------------------------------------------------------- 1 | 0.008120214300587464,0.8120214300587463,20,40.601071502937316,149.99758204821157 2 | 0.03932737665336547,1.5603581176389,20,37.416834379007696,149.98744630702024 3 | 0.10170572728906951,3.1189175317852027,20,77.92797070731513,149.9668885544205 4 | 0.1951831833332032,4.6738728022066836,20,77.74776352107403,149.93533517733917 5 | 0.31965127480733063,6.223404573706371,20,77.47658857498436,149.89192300694862 6 | 0.47496470623241016,7.765671571253973,20,77.11334987738013,149.83549582992026 7 | 0.6609407319181878,9.298801284288885,20,76.65648565174557,149.7645994210318 8 | 0.8773583117785027,10.82087899301574,20,76.10388543634271,149.67747482642287 9 | 1.123957004732345,12.329934647692125,20,75.45278273381938,149.57204956045572 10 | 1.4004355487493345,13.823927200849468,20,74.69962765786714,149.44592631350116 11 | 1.7064500700684255,15.300726065954551,20,73.83994325525407,149.29636870408385 12 | 2.041611854005174,16.758089196837442,20,72.86815654414464,149.1202835510861 13 | 2.4054846065174007,18.19363762561133,20,71.77742143869432,148.9141990891104 14 | 2.7975811277147806,19.604826059869005,20,70.55942171288372,148.67423851369443 15 | 3.21735931962089,20.988909595305465,20,69.20417677182306,148.39608822568428 16 | 3.664217449815305,22.342906509720745,20,67.69984572076407,148.07496016614786 17 | 4.137488605941106,23.663557806290044,20,66.03256482846493,147.7055477090741 18 | 4.6364342933819644,24.94728437204296,20,64.18632828764578,147.28197475039812 19 | 5.160237170520034,26.1901438569035,20,62.14297424302691,146.797737939111 20 | 5.707992980485562,27.387790498276406,20,59.882332068645255,146.24564252034855 21 | 6.278701843136379,28.535443132540806,20,57.38263171322011,145.61773310691612 22 | 6.871259239903875,29.627869838374817,20,54.6213352917005,144.90522201284054 23 | 7.484447273520571,30.659401680834804,20,51.57659212299933,144.09841979271607 24 | 8.116927160956404,31.623994371791625,20,48.22963454784106,143.18667560814785 25 | 8.767234458621381,32.51536488324882,20,44.568525572859734,142.15833935225857 26 | 9.433779279663364,33.32724105209916,20,40.59380844251703,141.00076350655394 27 | 10.114854787249238,34.0537753792937,20,36.32671635972676,139.70037085635508 28 | 10.80865855518655,34.690188396865594,20,31.820650878594847,138.24282458209942 29 | 11.513332912988126,35.23371789007884,20,27.176474660662404,136.6133493751603 30 | 12.227031905836878,35.684949642437644,20,22.561587617940123,134.7972642597495 31 | 12.948023470388424,36.04957822757734,20,18.231429256984942,132.7807955079335 32 | 13.66613436894099,35.90554492762821,20,-7.201664997456362,130.58010975753322 33 | 14.36118940758492,34.75275193219646,20,-57.63964977158777,128.26099215257676 34 | 15.020940906995207,32.98757497051441,20,-88.2588480841023,125.88729892043992 35 | 15.645473868569567,31.22664807871802,20,-88.04634458981954,123.4918064541344 36 | 16.237794628963478,29.616038019695413,20,-80.53050295113037,121.09843261858711 37 | 16.800824416593237,28.151489381488,20,-73.22743191037075,118.73041926385105 38 | 17.337277605723845,26.822659456530417,20,-66.44149624787917,116.40936351626401 39 | 17.849561510399507,25.61419523378308,20,-60.42321113736689,114.15442034335031 40 | 18.339707154186396,24.50728218934456,20,-55.34565222192575,111.9817515144622 41 | 18.80933461042021,23.481372811690566,20,-51.29546888269989,109.904249598161 42 | 19.25965094532735,22.51581674535703,20,-48.27780331667678,107.93152137285264 43 | 19.69147475239705,21.59119035348494,20,-46.231319593604454,106.07008284076257 44 | 20.105279255685357,20.69022516441533,20,-45.04825945348041,104.32370202654035 45 | 20.501245817072682,19.798328069366196,20,-44.59485475245682,102.69382459165097 46 | 20.879320833568265,18.903750824779102,20,-44.72886222935462,101.18002647394856 47 | 21.23927079953618,17.997498298395822,20,-45.31262631916404,99.78045193554321 48 | 21.58073217209471,17.073068627926368,20,-46.22148352347264,98.49221031149817 49 | 21.903254302295096,16.12610651001929,20,-47.34810589535391,97.31171759055617 50 | 22.20633490805259,15.154030287874608,20,-48.60381110723416,96.23497867255288 51 | 22.4894483756999,14.15567338236575,20,-49.91784527544291,95.25781258141548 52 | 22.752067643807642,13.130963405387085,20,-51.23549884893323,94.37602659805017 53 | 22.993680623879847,12.080649003610244,20,-52.51572008884208,93.5855469585545 54 | 23.213802160361723,11.006076824093833,20,-53.7286089758205,92.88251403061714 55 | 23.411982459583808,9.909014961104312,20,-54.85309314947609,92.26334941662067 56 | 23.587812812313594,8.791517636489491,20,-55.87486623074107,91.72480150394374 57 | 23.740929298437376,7.655824306189008,20,-56.784666515024114,91.2639749611546 58 | 23.87101503401826,6.50428677904427,20,-57.57687635723688,90.87834865242698 59 | 23.977801412226864,5.339318910430163,20,-58.248393430705406,90.56578549193013 60 | 24.06106867134898,4.163362956105831,20,-58.79779771621654,90.32453701499918 61 | 24.1206460640754,2.9788696363209706,20,-59.22466598924306,90.1532447038345 62 | 24.156411809496568,1.7882872710583806,20,-59.5291182631295,90.05093963226132 63 | 24.171300518008007,0.7444354255717202,20,-52.192592274333016,90.00846753558393 64 | 24.174271688330002,0.14855851609978468,20,-29.79384547359677,89.99999999999999 65 | 24.174271688330002,0.0,20,-7.4279258049892345,89.99999999999999 66 | -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/Sensors.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018 2 | 3 | import edu.wpi.first.wpilibj.DriverStation 4 | import org.snakeskin.dsl.Sensors 5 | import org.snakeskin.dsl.machine 6 | import org.team401.robot2018.constants.Constants 7 | import org.team401.robot2018.subsystems.* 8 | 9 | /* 10 | * 2018-Robot-Code - Created on 1/30/18 11 | * Author: Cameron Earle 12 | * 13 | * This code is licensed under the GNU GPL v3 14 | * You can find more info in the LICENSE file at project root 15 | */ 16 | 17 | /** 18 | * @author Cameron Earle 19 | * @version 1/30/18 20 | */ 21 | 22 | /* 23 | val CubeVisionSensor = Sensors.booleanSensor({ VisionData.read().isCubePresent}) { 24 | pollAt(20) 25 | 26 | whenTriggered { 27 | 28 | IntakeSubsystem.machine(INTAKE_WHEELS_MACHINE).setState(IntakeWheelsStates.INTAKE_FORCE) 29 | IntakeSubsystem.machine(INTAKE_FOLDING_MACHINE).setState(IntakeFoldingStates.INTAKE_FORCE) 30 | } 31 | 32 | whenUntriggered { 33 | 34 | IntakeSubsystem.machine(INTAKE_WHEELS_MACHINE).setState(IntakeWheelsStates.IDLE) 35 | if(boxHeld()) IntakeSubsystem.machine(INTAKE_FOLDING_MACHINE).setState(IntakeFoldingStates.STOWED) 36 | else IntakeSubsystem.machine(INTAKE_FOLDING_MACHINE).setState(IntakeFoldingStates.GRAB) 37 | } 38 | } 39 | */ -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/Setup.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018 2 | 3 | import com.ctre.phoenix.sensors.PigeonIMU 4 | import edu.wpi.first.networktables.NetworkTableInstance 5 | import edu.wpi.first.wpilibj.CameraServer 6 | import edu.wpi.first.wpilibj.DriverStation 7 | import edu.wpi.first.wpilibj.PowerDistributionPanel 8 | import edu.wpi.first.wpilibj.Servo 9 | import edu.wpi.first.wpilibj.livewindow.LiveWindow 10 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard 11 | import edu.wpi.first.wpilibj.vision.VisionRunner 12 | import edu.wpi.first.wpilibj.vision.VisionThread 13 | import org.snakeskin.annotation.PostStartup 14 | import org.snakeskin.annotation.Setup 15 | import org.snakeskin.auto.AutoManager 16 | import org.snakeskin.dsl.Subsystem 17 | import org.snakeskin.dsl.buildSubsystem 18 | import org.snakeskin.dsl.on 19 | import org.snakeskin.dsl.send 20 | import org.snakeskin.event.Events 21 | import org.snakeskin.registry.Controllers 22 | import org.snakeskin.registry.Sensors 23 | import org.snakeskin.registry.Subsystems 24 | import org.team401.robot2018.auto.PowerUpAuto 25 | import org.team401.robot2018.auto.TestAuto 26 | //import org.team401.robot2018.auto.TestAuto 27 | import org.team401.robot2018.constants.Constants 28 | import org.team401.robot2018.constants.CompConstants 29 | import org.team401.robot2018.etc.LED 30 | import org.team401.robot2018.etc.Reporting 31 | import org.team401.robot2018.etc.RobotEvents 32 | import org.team401.robot2018.subsystems.* 33 | import org.team401.robot2018.vision.CubeDetectorPipeline 34 | import org.team401.robot2018.vision.MjpegServer 35 | import org.team401.robot2018.vision.VisionController 36 | 37 | /* 38 | * 2018-Robot-Code - Created on 1/5/18 39 | * Author: Cameron Earle 40 | * 41 | * This code is licensed under the GNU GPL v3 42 | * You can find more info in the LICENSE file at project root 43 | */ 44 | 45 | /** 46 | * @author Cameron Earle 47 | * @version 1/5/18 48 | */ 49 | 50 | //val Vision = VisionController("10.4.1.3") 51 | //val VisionData = VisionDataClient(ADDRESS, Integer.valueOf(PORT)) 52 | val PDP = PowerDistributionPanel() 53 | //val MJPEG = MjpegServer(1180) 54 | 55 | @Setup 56 | fun setup() { 57 | //Uncomment which one you are using 58 | Constants = CompConstants() 59 | //Constants = PracticeConstants() 60 | 61 | //Uncomment which one you are using 62 | AutoManager.auto = PowerUpAuto //Real auto 63 | //AutoManager.auto = TestAuto //Test auto 64 | 65 | //Uncomment which one you are using 66 | PowerUpAuto.publish() //Real auto 67 | //TestAuto.publish() //Test auto 68 | 69 | //Uncomment which one you are using 70 | PowerUpAuto.startTasks() //Real auto 71 | //TestAuto.startTasks() //Test auto 72 | 73 | val camera = CameraServer.getInstance().startAutomaticCapture() 74 | 75 | Subsystems.add(DrivetrainSubsystem, ElevatorSubsystem, IntakeSubsystem) 76 | Controllers.add(LeftStick, RightStick, Gamepad) 77 | 78 | LiveWindow.disableTelemetry(PDP) //Fix bugs with PDP errors 79 | 80 | on(Events.DISABLED) { 81 | LED.rainbowAll() 82 | } 83 | 84 | on(Events.ENABLED) { 85 | LED.offAll() 86 | } 87 | 88 | on(RobotEvents.HAVE_CUBE) { 89 | LED.signalHaveCube() 90 | } 91 | 92 | on(RobotEvents.EJECT_CUBE) { 93 | LED.signalEjectCube() 94 | } 95 | 96 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/AutoDelays.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 3/3/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 3/3/18 14 | */ 15 | object AutoDelays { 16 | const val SCORE_DELAY = 0.5 17 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/AutoTarget.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser 4 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 1/23/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 1/23/18 17 | */ 18 | enum class AutoTarget(val prettyName: String, val robotPosition: RobotPosition, vararg val firstProfiles: String) { 19 | NOTHING("Do nothing", RobotPosition.DS_CENTER), 20 | CENTER_SWITCH("Center 2 Switch", RobotPosition.DS_CENTER, "DS_CENTER-SWITCH_LEFT", "DS_CENTER-SWITCH_RIGHT"), 21 | SWITCH_LEFT("Baseline/Switch left", RobotPosition.DS_LEFT, "DS_LEFT-SWITCH_LEFT", "BASELINE"), 22 | SWITCH_RIGHT("Baseline/Switch right", RobotPosition.DS_RIGHT, "DS_RIGHT-SWITCH_RIGHT", "BASELINE"), 23 | FULL_SCALE_LEFT("Full Scale left", RobotPosition.DS_LEFT, "DS_LEFT-SCALE_LEFT", "DS_LEFT-SCALE_RIGHT"), 24 | FULL_SCALE_RIGHT("Full Scale right", RobotPosition.DS_RIGHT, "DS_RIGHT-SCALE_RIGHT", "DS_RIGHT-SCALE_LEFT"), 25 | STAY_NEAR_LEFT("Stay near left", RobotPosition.DS_LEFT, "DS_LEFT-SCALE_LEFT", "DS_LEFT-SWITCH_LEFT", "BASELINE"), 26 | STAY_NEAR_RIGHT("Stay near right", RobotPosition.DS_RIGHT, "DS_RIGHT-SCALE_RIGHT", "DS_RIGHT-SWITCH_RIGHT", "BASELINE"); 27 | 28 | companion object { 29 | fun toSendableChooser(): SendableChooser { 30 | val chooser = SendableChooser() 31 | values().forEachIndexed { 32 | index, target -> 33 | if (index == 0) { 34 | chooser.addDefault(target.prettyName, target) 35 | } else { 36 | chooser.addObject(target.prettyName, target) 37 | } 38 | } 39 | return chooser 40 | } 41 | } 42 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/Commands.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | import org.snakeskin.dsl.machine 4 | import org.team401.robot2018.auto.steps.* 5 | import org.team401.robot2018.etc.LED 6 | import org.team401.robot2018.subsystems.* 7 | 8 | /* 9 | * 2018-Robot-Code - Created on 1/23/18 10 | * Author: Cameron Earle 11 | * 12 | * This code is licensed under the GNU GPL v3 13 | * You can find more info in the LICENSE file at project root 14 | */ 15 | 16 | /** 17 | * @author Cameron Earle 18 | * @version 1/23/18 19 | */ 20 | 21 | object Commands { 22 | fun DeployElevator() = StateStep(ElevatorSubsystem, ELEVATOR_DEPLOY_MACHINE, ElevatorDeployStates.DEPLOY) 23 | fun HomeElevator() = object : AutoStep() { 24 | override fun entry(currentTime: Double) { 25 | ElevatorSubsystem.machine(ELEVATOR_MACHINE).setState(ElevatorStates.HOMING) 26 | } 27 | 28 | override fun action(currentTime: Double, lastTime: Double) { 29 | done = Elevator.homed 30 | } 31 | 32 | override fun exit(currentTime: Double) { 33 | println("Auto: Elevator Homed") 34 | } 35 | } 36 | 37 | fun WaitForDeploy() = WaitForStep { 38 | ElevatorSubsystem.machine(ELEVATOR_DEPLOY_MACHINE).getState() == ElevatorDeployStates.DEPLOYED 39 | } 40 | 41 | fun WaitForHasCube() = WaitForStep { 42 | ElevatorSubsystem.machine(ELEVATOR_MACHINE).getState() == ElevatorStates.POS_DRIVE 43 | } 44 | 45 | 46 | fun WaitForAtSwitch() = WaitForStep { Elevator.atSwitch() } 47 | 48 | fun HoldElevator() = StateStep(ElevatorSubsystem, ELEVATOR_MACHINE, ElevatorStates.HOLD_POS_UNKNOWN) 49 | fun ScaleAfterUnfold() = StateStep(ElevatorSubsystem, ELEVATOR_MACHINE, ElevatorStates.SCALE_POS_UNKNOWN) 50 | 51 | fun ElevatorToDrive() = StateStep(ElevatorSubsystem, ELEVATOR_MACHINE, ElevatorStates.POS_DRIVE) 52 | fun ElevatorToGround() = StateStep(ElevatorSubsystem, ELEVATOR_MACHINE, ElevatorStates.POS_COLLECTION) 53 | fun ElevatorToSwitch() = StateStep(ElevatorSubsystem, ELEVATOR_MACHINE, ElevatorStates.POS_SWITCH) 54 | fun ElevatorToScale() = StateStep(ElevatorSubsystem, ELEVATOR_MACHINE, ElevatorStates.POS_SCALE_HIGH) 55 | fun ElevatorKickerScore() = StateStep(ElevatorSubsystem, ELEVATOR_KICKER_MACHINE, ElevatorKickerStates.KICK) 56 | fun ElevatorKickerRetract() = StateStep(ElevatorSubsystem, ELEVATOR_KICKER_MACHINE, ElevatorKickerStates.STOW) 57 | fun ElevatorHolderClamp() = StateStep(ElevatorSubsystem, ELEVATOR_CLAMP_MACHINE, ElevatorClampStates.CLAMPED) 58 | fun ElevatorHolderUnclamp() = StateStep(ElevatorSubsystem, ELEVATOR_CLAMP_MACHINE, ElevatorClampStates.UNCLAMPED) 59 | 60 | fun ElevatorHigh() = StateStep(ElevatorSubsystem, ELEVATOR_SHIFTER_MACHINE, ElevatorShifterStates.HIGH) 61 | 62 | fun IntakeToStow() = StateStep(IntakeSubsystem, INTAKE_FOLDING_MACHINE, IntakeFoldingStates.HOMING) 63 | fun IntakeToGrab() = StateStep(IntakeSubsystem, INTAKE_FOLDING_MACHINE, IntakeFoldingStates.GRAB) 64 | fun IntakeToIntake() = StateStep(IntakeSubsystem, INTAKE_FOLDING_MACHINE, IntakeFoldingStates.GO_TO_INTAKE) 65 | 66 | fun IntakeWheelsIdle() = StateStep(IntakeSubsystem, INTAKE_WHEELS_MACHINE, IntakeWheelsStates.IDLE) 67 | fun IntakeWheelsRun() = StateStep(IntakeSubsystem, INTAKE_WHEELS_MACHINE, IntakeWheelsStates.INTAKE) 68 | 69 | fun HighLockDeployAndWait() = arrayOf(DelayStep(.5), ElevatorHigh(), HoldElevator(), DeployElevator(), WaitForDeploy()) 70 | fun UnhomeElevator() = LambdaStep { Elevator.homed = false } 71 | 72 | fun Score() = arrayOf(ElevatorHolderUnclamp(), ElevatorKickerScore(), LambdaStep { LED.signalScoreCube() }, DelayStep(AutoDelays.SCORE_DELAY), ElevatorKickerRetract()) 73 | 74 | fun ResetHeading() = LambdaStep { HeadingTracker.reset() } 75 | fun PrintTime(name: String) = object : SingleStep() { 76 | override fun entry(currentTime: Double) { 77 | println("$name: $currentTime") 78 | } 79 | } 80 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/FieldElements.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | import openrio.powerup.MatchData 4 | 5 | /* 6 | * 2018-Robot-Code - Created on 3/3/18 7 | * Author: Cameron Earle 8 | * 9 | * This code is licensed under the GNU GPL v3 10 | * You can find more info in the LICENSE file at project root 11 | */ 12 | 13 | /** 14 | * @author Cameron Earle 15 | * @version 3/3/18 16 | */ 17 | object FieldElements { 18 | fun switch(side: MatchData.OwnedSide) = "SWITCH_$side" 19 | fun switch(side: MatchData.OwnedSide, stepNumber: Int) = "SWITCH_${side}_TWOCUBE$stepNumber" 20 | fun scale(side: MatchData.OwnedSide) = "SCALE_$side" 21 | fun scale(side: MatchData.OwnedSide, stepNumber: Int) = "SCALE_${side}_TWOCUBE$stepNumber" 22 | fun baseline() = "BASELINE" 23 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/HeadingTracker.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | import org.team401.robot2018.auto.motionprofile.MotionProfile 4 | import org.team401.robot2018.etc.RobotMath 5 | import org.team401.robot2018.subsystems.Drivetrain 6 | 7 | object HeadingTracker { 8 | private val imu = Drivetrain.imu 9 | private var lastSetpoint = 0.0 10 | private var headingError = 0.0 11 | private const val TIMEOUT = 1000 12 | private val imuData = DoubleArray(3) 13 | 14 | fun reset() { 15 | lastSetpoint = 0.0 16 | headingError = 0.0 17 | imuData[0] = 0.0 18 | imuData[1] = 0.0 19 | imuData[2] = 0.0 20 | imu.setYaw(0.0, TIMEOUT) 21 | } 22 | 23 | fun configureImu() { 24 | imu.getYawPitchRoll(imuData) 25 | headingError = lastSetpoint - imuData[0] 26 | imu.setYaw( 27 | RobotMath.UnitConversions.degreesToCTREDumbUnit(-headingError), 28 | TIMEOUT 29 | ) 30 | } 31 | 32 | fun finishedHeading(headingSetpoint: Double) { 33 | lastSetpoint = headingSetpoint 34 | } 35 | 36 | fun finishedProfile(profile: MotionProfile) = finishedHeading(profile.getLastHeading()) 37 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/RobotAuto.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | import edu.wpi.first.wpilibj.DriverStation 4 | import edu.wpi.first.wpilibj.Timer 5 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard 6 | import openrio.powerup.MatchData 7 | import org.snakeskin.auto.AutoLoop 8 | import org.snakeskin.factory.ExecutorFactory 9 | import org.team401.robot2018.auto.steps.AutoStep 10 | import java.sql.Driver 11 | import java.util.concurrent.TimeUnit 12 | 13 | /* 14 | * 2018-Robot-Code - Created on 3/3/18 15 | * Author: Cameron Earle 16 | * 17 | * This code is licensed under the GNU GPL v3 18 | * You can find more info in the LICENSE file at project root 19 | */ 20 | 21 | /** 22 | * @author Cameron Earle 23 | * @version 3/3/18 24 | */ 25 | abstract class RobotAuto: AutoLoop() { 26 | private val executor = ExecutorFactory.getExecutor("Auto") 27 | 28 | //DATA 29 | private val autoTargetSelector = AutoTarget.toSendableChooser() 30 | 31 | fun startTasks() { 32 | val ds = DriverStation.getInstance() 33 | executor.scheduleAtFixedRate({ 34 | if (ds.isDisabled) { 35 | fetchSD() 36 | preAuto() 37 | } 38 | }, 0L, 100L, TimeUnit.MILLISECONDS) 39 | } 40 | 41 | fun publish() { 42 | SmartDashboard.putData("Auto Mode", autoTargetSelector) 43 | SmartDashboard.putNumber("Base Delay", 0.0) 44 | } 45 | 46 | protected var robotPos = RobotPosition.DS_CENTER; private set 47 | protected var target = AutoTarget.NOTHING; private set 48 | protected var switchSide = MatchData.OwnedSide.UNKNOWN; private set 49 | protected var scaleSide = MatchData.OwnedSide.UNKNOWN; private set 50 | protected var baseDelay = 0.0; private set 51 | 52 | /** 53 | * Polls the field for data until valid data is found 54 | * Runs at a 1 ms rate to ensure we get data as fast as possible 55 | */ 56 | private fun fetchFieldLayout() { 57 | while (switchSide == MatchData.OwnedSide.UNKNOWN || scaleSide == MatchData.OwnedSide.UNKNOWN) { 58 | switchSide = MatchData.getOwnedSide(MatchData.GameFeature.SWITCH_NEAR) 59 | scaleSide = MatchData.getOwnedSide(MatchData.GameFeature.SCALE) 60 | Thread.sleep(1) 61 | } 62 | } 63 | 64 | /** 65 | * Gets various info from SmartDashboard 66 | */ 67 | private fun fetchSD() { 68 | target = autoTargetSelector.selected 69 | robotPos = target.robotPosition 70 | baseDelay = SmartDashboard.getNumber("Base Delay", 0.0) 71 | } 72 | 73 | //AUTO MANAGER 74 | private val sequence = arrayListOf() 75 | private var sequenceIdx = 0 76 | private val adder: (AutoStep) -> Unit = { sequence.add(it) } 77 | 78 | override val rate = 5L 79 | 80 | abstract fun preAuto() 81 | abstract fun assembleAuto(add: (AutoStep) -> Unit) 82 | 83 | override fun entry() { 84 | Routines.add = adder //Set the routines object to use this loop's adder 85 | done = false 86 | fetchSD() 87 | fetchFieldLayout() 88 | sequence.clear() 89 | sequenceIdx = 0 90 | assembleAuto(adder) 91 | sequence.forEach { 92 | it.reset() 93 | } 94 | } 95 | 96 | override fun action(currentTime: Double, lastTime: Double) { 97 | if (sequenceIdx < sequence.size) { 98 | sequence[sequenceIdx].tick(currentTime, lastTime) 99 | if (sequence[sequenceIdx].doContinue()) { 100 | sequenceIdx++ 101 | } 102 | } else { 103 | done = true 104 | } 105 | } 106 | 107 | override fun exit() { 108 | val time = Timer.getFPGATimestamp() 109 | sequence.forEach { 110 | if (it.state != AutoStep.State.CONTINUE) { 111 | it.exit(time) 112 | } 113 | } 114 | } 115 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/RobotPosition.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser 4 | import openrio.powerup.MatchData 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 1/23/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 1/23/18 17 | */ 18 | 19 | enum class RobotPosition() { 20 | DS_CENTER, 21 | DS_LEFT, 22 | DS_RIGHT; 23 | 24 | infix fun alignedWith(side: MatchData.OwnedSide) = 25 | when (this) { 26 | DS_LEFT -> side == MatchData.OwnedSide.LEFT 27 | DS_CENTER -> false 28 | DS_RIGHT -> side == MatchData.OwnedSide.RIGHT 29 | } 30 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/Routines.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | import org.team401.robot2018.auto.motion.RioProfileRunner 4 | import org.team401.robot2018.auto.motionprofile.ArcProfileFollower 5 | import org.team401.robot2018.auto.steps.AutoStep 6 | import org.team401.robot2018.auto.steps.DelayStep 7 | import org.team401.robot2018.auto.steps.LambdaStep 8 | import org.team401.robot2018.auto.steps.ParallelSteps 9 | import org.team401.robot2018.constants.Constants 10 | import org.team401.robot2018.etc.LED 11 | import org.team401.robot2018.etc.StepAdder 12 | import org.team401.robot2018.subsystems.Drivetrain 13 | 14 | /* 15 | * 2018-Robot-Code - Created on 3/3/18 16 | * Author: Cameron Earle 17 | * 18 | * This code is licensed under the GNU GPL v3 19 | * You can find more info in the LICENSE file at project root 20 | */ 21 | 22 | /** 23 | * @author Cameron Earle 24 | * @version 3/3/18 25 | */ 26 | object Routines { 27 | lateinit var add: StepAdder 28 | 29 | fun drive(profile: String, vararg otherActions: AutoStep) { 30 | val step = ArcProfileFollower(Drivetrain) 31 | 32 | step.load("/home/lvuser/profiles/${profile}_C.csv") 33 | 34 | add(ParallelSteps(step, *otherActions)) 35 | } 36 | 37 | fun drive(start: Any, end: Any, vararg otherActions: AutoStep) = drive("$start-$end", *otherActions) 38 | 39 | fun score() { 40 | add(Commands.ElevatorHolderUnclamp()) 41 | add(Commands.ElevatorKickerScore()) 42 | add(LambdaStep { LED.signalScoreCube() }) 43 | add(DelayStep(0.5)) 44 | add(Commands.ElevatorKickerRetract()) 45 | } 46 | 47 | fun intake() { 48 | add(Commands.ElevatorToGround()) 49 | add(Commands.IntakeWheelsRun()) 50 | add(Commands.IntakeToGrab()) 51 | } 52 | 53 | fun setup() { 54 | add(Commands.UnhomeElevator()) 55 | add(Commands.ResetHeading()) 56 | add(Commands.IntakeToStow()) //This actually homes the intake, not stow 57 | } 58 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/TestAuto.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto 2 | 3 | import com.ctre.phoenix.motorcontrol.FeedbackDevice 4 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard 5 | import org.team401.robot2018.auto.motionprofile.ProfileLoader 6 | import org.team401.robot2018.auto.motionprofile.ArcProfileFollower 7 | import org.team401.robot2018.auto.motionprofile.TuningArcProfileFollower 8 | import org.team401.robot2018.auto.motionprofile.ZeroPoint 9 | import org.team401.robot2018.auto.steps.* 10 | import org.team401.robot2018.etc.StepAdder 11 | import org.team401.robot2018.subsystems.Drivetrain 12 | 13 | object TestAuto : RobotAuto() { 14 | override fun preAuto() { 15 | } 16 | 17 | override fun assembleAuto(add: StepAdder) { 18 | Routines.setup() 19 | Routines.drive("DS_LEFT-SCALE_LEFT", SequentialSteps(*Commands.HighLockDeployAndWait(), Commands.ScaleAfterUnfold())) 20 | Routines.score() 21 | add(ParallelSteps(ZeroPoint(-105.0, 0.0, .006, 0.0, 0.0, 20.0, 2.0, 1.5), 22 | SequentialSteps(DelayStep(.1), Commands.HomeElevator()))) //1 23 | Routines.intake() 24 | Routines.drive("SCALE_LEFT-SCALE_LEFT_TWOCUBE2") 25 | add(Commands.WaitForHasCube()) 26 | add(ParallelSteps(ZeroPoint(165.0, 0.0, .006, 0.0, 0.0, 20.0, 2.0, 1.5), Commands.ElevatorToScale())) 27 | Routines.drive("SCALE_LEFT_TWOCUBE3-SCALE_LEFT") 28 | Routines.score() 29 | } 30 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motion/DriveController.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motion 2 | 3 | class DriveController(val gains: DriveGains, val magnitude: Double = 1.0) { 4 | fun calculate(position: Double, 5 | velocity: Double, 6 | desiredPosition: Double, 7 | desiredVelocity: Double, 8 | desiredAccel: Double) = 9 | Math.max(-magnitude, Math.min(magnitude, 10 | (gains.P * (desiredPosition - position)) + 11 | (gains.V * (desiredVelocity - velocity)) + 12 | (gains.ffV * desiredVelocity) + 13 | (gains.ffA * desiredAccel) 14 | )) 15 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motion/DriveGains.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motion 2 | 3 | data class DriveGains(val P: Double = 0.0, 4 | val V: Double = 0.0, 5 | val ffV: Double = 0.0, 6 | val ffA: Double = 0.0, 7 | val H: Double = 0.0) -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motion/HeadingController.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motion 2 | 3 | import org.team401.robot2018.etc.RobotMath 4 | 5 | class HeadingController(val gains: DriveGains, val magnitude: Double = 1.0) { 6 | fun calculate(heading: Double, desiredHeading: Double) = 7 | Math.max(-magnitude, Math.min(magnitude, gains.H * RobotMath.limit180(desiredHeading - heading))) 8 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motion/PDVA.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motion 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 2/6/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 2/6/18 14 | */ 15 | 16 | /** 17 | * Gains for a PDVA controller 18 | * 19 | * @param p Proportional gain 20 | * @param d Derivative gain 21 | * @param v Velocity scalar. Should be 1 / system max velocity 22 | * @param a Acceleration scalar. Used to increase or decrease overall acceleration. Usually 0.0 23 | */ 24 | data class PDVA(val p: Double = 0.0, 25 | val d: Double = 0.0, 26 | val v: Double = 0.0, 27 | val a: Double = 0.0) -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motion/ProfileLoader.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motion 2 | 3 | import org.snakeskin.factory.ExecutorFactory 4 | import java.io.File 5 | import java.util.concurrent.ConcurrentHashMap 6 | import java.util.concurrent.CountDownLatch 7 | 8 | /* 9 | * 2018-Robot-Code - Created on 3/13/18 10 | * Author: Cameron Earle 11 | * 12 | * This code is licensed under the GNU GPL v3 13 | * You can find more info in the LICENSE file at project root 14 | */ 15 | 16 | /** 17 | * @author Cameron Earle 18 | * @version 3/13/18 19 | */ 20 | 21 | object ProfileLoader { 22 | class LoadPromise(private val points: ArrayList) { 23 | private val latch = CountDownLatch(1) 24 | 25 | fun load(pointsIn: ArrayList) { 26 | points.clear() 27 | points.addAll(pointsIn) 28 | latch.countDown() 29 | } 30 | 31 | fun await() { 32 | latch.await() 33 | } 34 | } 35 | 36 | private val cache = ConcurrentHashMap>() 37 | private val executor = ExecutorFactory.getExecutor("Point loader") 38 | 39 | @Synchronized private fun loadFromFile(filename: String): ArrayList { 40 | val f = File(filename) 41 | val points = arrayListOf() 42 | val lines = f.readLines() 43 | lines.forEach { 44 | points.add(Waypoint.fromCsv(it)) 45 | } 46 | println("LOADED $filename DONE ------") 47 | return points 48 | } 49 | 50 | /** 51 | * Clears all profiles from the cache, freeing memory 52 | */ 53 | fun clear() = cache.clear() 54 | 55 | /** 56 | * Preloads a profile into the cache if it isn't already present 57 | * @param profile The name of the profile to preload 58 | */ 59 | fun preload(profile: String) { 60 | if (!cache.containsKey(profile)) { 61 | cache.put(profile, loadFromFile(profile)) 62 | } 63 | } 64 | 65 | /** 66 | * Preloads a set of profiles, removing any that aren't in the set 67 | * @param profiles The names of the profiles to load 68 | */ 69 | fun preloadThese(vararg profiles: String) { 70 | //Remove any values that shouldn't be in the cache 71 | val toRemove = arrayListOf() 72 | cache.forEach { 73 | key, _ -> 74 | if (profiles.none { it == key }) { 75 | toRemove.add(key) 76 | } 77 | } 78 | toRemove.forEach { 79 | cache.remove(it) 80 | } 81 | //Load the cache 82 | profiles.forEach { 83 | preload(it) 84 | } 85 | } 86 | 87 | /** 88 | * Populates the input ArrayList immediately from the points in the profile, loading from the cache if possible 89 | * @param profile The profile to load 90 | * @param points The ArrayList to load the points into 91 | */ 92 | fun populateNow(profile: String, points: ArrayList) { 93 | points.clear() 94 | if (cache.containsKey(profile)) { 95 | points.addAll(cache[profile]!!) 96 | } else { 97 | points.addAll(loadFromFile(profile)) 98 | } 99 | } 100 | 101 | /** 102 | * Populates the input ArrayList sometime later from the points in the profile, loading from the cache if possible 103 | * @param profile The profile to load 104 | * @param points The ArrayList to load the points into 105 | * 106 | * @return A promise that tells us when the loading is complete 107 | */ 108 | fun populateLater(profile: String, points: ArrayList): LoadPromise { 109 | val promise = LoadPromise(points) 110 | executor.submit { 111 | if (cache.containsKey(profile)) { 112 | promise.load(cache[profile]!!) 113 | } else { 114 | promise.load(loadFromFile(profile)) 115 | } 116 | } 117 | return promise 118 | } 119 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motion/TankMotionStep.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motion 2 | 3 | import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced 4 | import org.team401.robot2018.auto.steps.AutoStep 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 2/6/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 2/6/18 17 | */ 18 | 19 | abstract class TankMotionStep: AutoStep() { 20 | abstract val leftController: IMotorControllerEnhanced 21 | abstract val rightController: IMotorControllerEnhanced 22 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motion/Waypoint.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motion 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 3/13/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 3/13/18 14 | */ 15 | /** 16 | * Represents a single point in a profile 17 | */ 18 | data class Waypoint(val position: Double, val velocity: Double, val timestep: Int, val acceleration: Double, val heading: Double) { 19 | companion object { 20 | /** 21 | * Generates a Waypoint from an input CSV line 22 | * @param line The CSV line 23 | * @return The Waypoint 24 | */ 25 | fun fromCsv(line: String): Waypoint { 26 | val split = line.split(",") 27 | val position = split[0].toDouble() 28 | val velocity = split[1].toDouble() 29 | val timestep = split[2].toInt() 30 | val acceleration = try { 31 | split[3].toDouble() 32 | } catch (e: Exception) { 33 | 0.0 34 | } 35 | val heading = try { 36 | split[4].toDouble() 37 | } catch (e: Exception) { 38 | 90.0 39 | } 40 | 41 | return Waypoint(position, velocity, timestep, acceleration, heading) 42 | } 43 | } 44 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motionprofile/HeadingAdapter.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motionprofile 2 | 3 | import org.team401.robot2018.etc.withinTolerance 4 | import java.io.File 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 4/9/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 4/9/18 17 | */ 18 | 19 | /** 20 | * Converts output heading units of Pathfinder, which wrap around at the 0 border, to absolute heading units. 21 | * It accomplishes this by tracking the heading change across the current and last points. If the delta is 22 | * 180 or greater (or the opposite), it will increment (or decrement) a rotation tracker and return a 23 | * new heading. 24 | */ 25 | 26 | class HeadingAdapter { 27 | private var numRotations = 0 28 | 29 | private fun track(lastAngle: Double, angle: Double, subtract: Double): Double { 30 | if (angle - lastAngle > 180) { 31 | numRotations-- 32 | } else if (angle - lastAngle < -180) { 33 | numRotations++ 34 | } 35 | return ((numRotations * 360.0) + angle) - subtract 36 | } 37 | 38 | fun findNewHeadings(headings: List): List { 39 | val subtract = (headings.getOrNull(0) ?: 0.0) % 360.0 40 | val newHeadings = arrayListOf() 41 | for (i in 0 until headings.size) { 42 | val lastAngle = (headings.getOrNull(i - 1) ?: headings[i]) % 360 43 | val angle = headings[i] % 360 44 | newHeadings.add(track(lastAngle, angle, subtract)) 45 | } 46 | return newHeadings 47 | } 48 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motionprofile/MotionProfile.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motionprofile 2 | 3 | import com.ctre.phoenix.motion.TrajectoryPoint 4 | 5 | /* 6 | * 2018-Robot-Code - Created on 4/9/18 7 | * Author: Cameron Earle 8 | * 9 | * This code is licensed under the GNU GPL v3 10 | * You can find more info in the LICENSE file at project root 11 | */ 12 | 13 | /** 14 | * @author Cameron Earle 15 | * @version 4/9/18 16 | */ 17 | 18 | data class MotionProfile(val points: List = arrayListOf()) { 19 | private fun pointsAsArrayList() = points as ArrayList 20 | private var idx = 0 21 | var name = ""; private set 22 | 23 | private fun fixHeadings() { 24 | val adapter = HeadingAdapter() 25 | val newHeadings = adapter.findNewHeadings(points.map { it.heading }) 26 | newHeadings.forEachIndexed { 27 | index, heading -> 28 | points[index].heading = heading 29 | } 30 | } 31 | 32 | fun fromCSV(lines: List) { 33 | val points = pointsAsArrayList() 34 | points.clear() 35 | lines.forEach { 36 | points.add(Waypoint.fromCSVLine(it)) 37 | } 38 | fixHeadings() 39 | } 40 | 41 | fun fromPoints(pointsIn: List, name: String = "") { 42 | val points = pointsAsArrayList() 43 | points.clear() 44 | points.addAll(pointsIn) 45 | this.name = name 46 | fixHeadings() 47 | } 48 | 49 | fun numPoints() = points.size 50 | fun lastIndex() = points.size - 1 51 | 52 | fun isFirst(idx: Int) = idx == 0 53 | fun isLast(idx: Int) = idx == lastIndex() 54 | 55 | fun getFirstTimestep() = TrajectoryPoint.TrajectoryDuration.Trajectory_Duration_0ms.valueOf( 56 | points.getOrNull(0)?.timestep ?: 0 57 | ) 58 | 59 | fun getLastHeading() = points.lastOrNull()?.heading ?: 0.0 60 | 61 | fun getPoint(idx: Int) = points[idx] 62 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motionprofile/ProfileLoader.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motionprofile 2 | 3 | import org.snakeskin.factory.ExecutorFactory 4 | import java.io.File 5 | import java.io.FileNotFoundException 6 | import java.util.concurrent.ConcurrentHashMap 7 | import java.util.concurrent.CountDownLatch 8 | 9 | /* 10 | * 2018-Robot-Code - Created on 3/13/18 11 | * Author: Cameron Earle 12 | * 13 | * This code is licensed under the GNU GPL v3 14 | * You can find more info in the LICENSE file at project root 15 | */ 16 | 17 | /** 18 | * @author Cameron Earle 19 | * @version 3/13/18 20 | */ 21 | 22 | object ProfileLoader { 23 | class LoadPromise(private val mp: MotionProfile) { 24 | private val latch = CountDownLatch(1) 25 | 26 | fun load(pointsIn: ArrayList) { 27 | mp.fromPoints(pointsIn) 28 | latch.countDown() 29 | } 30 | 31 | fun await() { 32 | latch.await() 33 | } 34 | } 35 | 36 | private val cache = ConcurrentHashMap>() 37 | private val executor = ExecutorFactory.getExecutor("Talon Point loader") 38 | 39 | @Synchronized private fun loadFromFile(filename: String): ArrayList { 40 | println("ProfileLoader: Loading '$filename'") 41 | val f = File(filename) 42 | if (!f.exists()) { 43 | throw FileNotFoundException("Could not find profile '$filename'") 44 | } 45 | val points = arrayListOf() 46 | val lines = f.readLines() 47 | lines.forEach { 48 | points.add(Waypoint.fromCSVLine(it)) 49 | } 50 | return points 51 | } 52 | 53 | /** 54 | * Clears all profiles from the cache, freeing memory 55 | */ 56 | fun clear() = cache.clear() 57 | 58 | /** 59 | * Preloads a profile into the cache if it isn't already present 60 | * @param profile The name of the profile to preload 61 | */ 62 | fun preload(profile: String) { 63 | if (!cache.containsKey(profile)) { 64 | cache.put(profile, loadFromFile(profile)) 65 | } 66 | } 67 | 68 | /** 69 | * Preloads a set of profiles, removing any that aren't in the set 70 | * @param profiles The names of the profiles to load 71 | */ 72 | fun preloadThese(profiles: List) { 73 | //Remove any values that shouldn't be in the cache 74 | val toRemove = arrayListOf() 75 | cache.forEach { 76 | key, _ -> 77 | if (profiles.none { it == key }) { 78 | toRemove.add(key) 79 | } 80 | } 81 | toRemove.forEach { 82 | cache.remove(it) 83 | } 84 | //Load the cache 85 | profiles.forEach { 86 | preload(it) 87 | } 88 | } 89 | 90 | /** 91 | * Populates the input ArrayList immediately from the points in the profile, loading from the cache if possible 92 | * @param profile The profile to load 93 | * @param points The ArrayList to load the points into 94 | */ 95 | fun populateNow(profile: String, mp: MotionProfile) { 96 | if (cache.containsKey(profile)) { 97 | mp.fromPoints(cache[profile]!!) 98 | } else { 99 | mp.fromPoints(loadFromFile(profile)) 100 | } 101 | } 102 | 103 | /** 104 | * Populates the input ArrayList sometime later from the points in the profile, loading from the cache if possible 105 | * @param profile The profile to load 106 | * @param points The ArrayList to load the points into 107 | * 108 | * @return A promise that tells us when the loading is complete 109 | */ 110 | fun populateLater(profile: String, mp: MotionProfile): LoadPromise { 111 | val promise = LoadPromise(mp) 112 | executor.submit { 113 | try { 114 | if (cache.containsKey(profile)) { 115 | promise.load(cache[profile]!!) 116 | } else { 117 | promise.load(loadFromFile(profile)) 118 | } 119 | } catch(e: FileNotFoundException) { 120 | e.printStackTrace() 121 | } 122 | } 123 | return promise 124 | } 125 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motionprofile/TuningArcProfileFollower.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motionprofile 2 | 3 | import com.google.gson.Gson 4 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard 5 | import org.snakeskin.component.TankDrivetrain 6 | import org.team401.robot2018.auto.steps.AutoStep 7 | import org.team401.robot2018.etc.RobotMath 8 | 9 | class TuningArcProfileFollower(drivetrain: TankDrivetrain, val name: String): ArcProfileFollower(drivetrain) { 10 | private var timer = 0L 11 | 12 | private fun ready() = SmartDashboard.putString("tuningRunner-$name-state", "ready") 13 | private fun loading() = SmartDashboard.putString("tuningRunner-$name-state", "loading") 14 | private fun running() = SmartDashboard.putString("tuningRunner-$name-state", "running") 15 | 16 | private data class PublishData(val left: Side, 17 | val right: Side, 18 | val head: Double, 19 | val desHead: Double, 20 | val time: Long) { 21 | data class Side(val pos: Double, 22 | val vel: Double, 23 | val desPos: Double, 24 | val desVel: Double) 25 | } 26 | 27 | val gson = Gson() 28 | 29 | private fun publishData() { 30 | val imuData = DoubleArray(3) 31 | drivetrain.imu.getYawPitchRoll(imuData) 32 | val currentData = PublishData( 33 | PublishData.Side( 34 | RobotMath.Drivetrain.nativeUnitsToInches(controller.getSelectedSensorPosition(0).toDouble()), 35 | RobotMath.Drivetrain.nativeUnitsToIps(controller.getSelectedSensorVelocity(0).toDouble()), 36 | RobotMath.Drivetrain.nativeUnitsToInches(controller.activeTrajectoryPosition.toDouble()), 37 | RobotMath.Drivetrain.nativeUnitsToIps(controller.activeTrajectoryVelocity.toDouble()) 38 | ), 39 | PublishData.Side( 40 | 0.0, 41 | 0.0, 42 | 0.0, 43 | 0.0 44 | ), imuData[0], controller.activeTrajectoryHeading / 10.0, timer 45 | ) 46 | 47 | SmartDashboard.putString("tuningRunner-$name-current", gson.toJson(currentData)) 48 | } 49 | 50 | override fun entry(currentTime: Double) { 51 | timer = 0 52 | SmartDashboard.putString("tuningRunner-$name-current", "{}") 53 | loading() 54 | super.entry(currentTime) 55 | } 56 | 57 | override fun action(currentTime: Double, lastTime: Double) { 58 | super.action(currentTime, lastTime) 59 | running() 60 | timer += ((currentTime - lastTime) * 1000).toLong() 61 | publishData() 62 | } 63 | 64 | override fun exit(currentTime: Double) { 65 | ready() 66 | super.exit(currentTime) 67 | } 68 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motionprofile/Waypoint.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motionprofile 2 | 3 | import com.ctre.phoenix.motion.TrajectoryPoint 4 | import org.team401.robot2018.etc.RobotMath 5 | import org.team401.robot2018.etc.TalonEnums 6 | 7 | /* 8 | * 2018-Robot-Code - Created on 4/9/18 9 | * Author: Cameron Earle 10 | * 11 | * This code is licensed under the GNU GPL v3 12 | * You can find more info in the LICENSE file at project root 13 | */ 14 | 15 | /** 16 | * @author Cameron Earle 17 | * @version 4/9/18 18 | */ 19 | 20 | data class Waypoint(var position: Double, 21 | var velocity: Double, 22 | var timestep: Int, 23 | var acceleration: Double, 24 | var heading: Double) { 25 | 26 | fun toTrajectoryPoint(isFirst: Boolean, isLast: Boolean): TrajectoryPoint { 27 | val tp = TrajectoryPoint() 28 | tp.position = RobotMath.Drivetrain.inchesToNativeUnits(position) 29 | tp.velocity = RobotMath.Drivetrain.ipsToNativeUnits(velocity) 30 | tp.auxiliaryPos = heading * 10.0 31 | tp.headingDeg = heading 32 | tp.timeDur = TrajectoryPoint.TrajectoryDuration.Trajectory_Duration_0ms.valueOf(timestep) 33 | tp.profileSlotSelect0 = 0 34 | tp.profileSlotSelect1 = 1 35 | tp.zeroPos = isFirst 36 | tp.isLastPoint = isLast 37 | return tp 38 | } 39 | 40 | companion object { 41 | fun fromCSVLine(line: String): Waypoint { 42 | val split = line.split(',') 43 | val position = (split.getOrNull(0) ?: "0.0").toDoubleOrNull() ?: 0.0 44 | val velocity = (split.getOrNull(1) ?: "0.0").toDoubleOrNull() ?: 0.0 45 | val timestep = (split.getOrNull(2) ?: "0").toIntOrNull() ?: 0 46 | val acceleration = (split.getOrNull(3) ?: "0.0").toDoubleOrNull() ?: 0.0 47 | val heading = (split.getOrNull(4) ?: "90.0").toDoubleOrNull() ?: 90.0 48 | return Waypoint(position, velocity, timestep, acceleration, heading) 49 | } 50 | } 51 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/motionprofile/ZeroPoint.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.motionprofile 2 | 3 | import com.ctre.phoenix.motorcontrol.ControlMode 4 | import com.ctre.phoenix.motorcontrol.NeutralMode 5 | import org.team401.robot2018.auto.HeadingTracker 6 | import org.team401.robot2018.auto.steps.AutoStep 7 | import org.team401.robot2018.etc.withinTolerance 8 | import org.team401.robot2018.subsystems.Drivetrain 9 | 10 | class ZeroPoint(val heading: Double, val kF: Double, val kP: Double, val kI: Double, val kD: Double, val kIZone: Double, val tolerance: Double, val timeout: Double = 0.0): AutoStep() { 11 | private val left = Drivetrain.left.master 12 | private var right = Drivetrain.right.master 13 | private val imu = Drivetrain.imu 14 | private var imuData = DoubleArray(3) 15 | private var error = 0.0 16 | private var lastError = 0.0 17 | private var output = 0.0 18 | private var dt = 0.0 19 | private var counter = 0 20 | private var accum = 0.0 21 | private var startTime = 0.0 22 | 23 | override fun entry(currentTime: Double) { 24 | done = false 25 | HeadingTracker.configureImu() 26 | imuData[0] = 0.0 27 | imuData[1] = 0.0 28 | imuData[2] = 0.0 29 | error = 0.0 30 | lastError = 0.0 31 | output = 0.0 32 | dt = 0.0 33 | counter = 0 34 | accum = 0.0 35 | startTime = currentTime 36 | } 37 | 38 | override fun action(currentTime: Double, lastTime: Double) { 39 | if (timeout > 0.0 && currentTime - startTime >= timeout) done = true 40 | imu.getYawPitchRoll(imuData) 41 | dt = currentTime - lastTime 42 | error = heading - imuData[0] 43 | if (Math.abs(error) > kIZone) { 44 | accum = 0.0 45 | } else { 46 | accum += kI * error 47 | } 48 | output = kF * Math.signum(error) + (kP * error) + accum + (kD * ((error - lastError) / dt)) 49 | lastError = error 50 | left.set(ControlMode.PercentOutput, -output) 51 | right.set(ControlMode.PercentOutput, output) 52 | if (error.withinTolerance(0.0, tolerance)) { 53 | counter++ 54 | } else { 55 | counter = 0 56 | } 57 | println("err: $error counter: $counter") 58 | if (counter >= 10) { 59 | done = true 60 | } 61 | } 62 | 63 | override fun exit(currentTime: Double) { 64 | left.set(ControlMode.PercentOutput, 0.0) 65 | right.set(ControlMode.PercentOutput, 0.0) 66 | HeadingTracker.finishedHeading(heading) 67 | println("hdg done") 68 | } 69 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/AutoStep.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 1/15/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 1/15/18 14 | */ 15 | abstract class AutoStep(override var done: Boolean = false): IAutoStep { 16 | val isSingleStep = done 17 | enum class State { 18 | ENTRY, 19 | ACTION, 20 | EXIT, 21 | CONTINUE 22 | } 23 | 24 | var state = State.ENTRY; private set 25 | 26 | override fun reset() { 27 | state = State.ENTRY 28 | if (!isSingleStep) { 29 | done = false 30 | } 31 | } 32 | 33 | fun doContinue() = (state == State.CONTINUE) 34 | 35 | override fun tick(currentTime: Double, lastTime: Double) { 36 | when (state) { 37 | State.ENTRY -> { 38 | entry(currentTime) 39 | state = State.ACTION 40 | } 41 | State.ACTION -> { 42 | action(currentTime, lastTime) 43 | if (done) { 44 | state = State.EXIT 45 | } 46 | } 47 | State.EXIT -> { 48 | exit(currentTime) 49 | state = State.CONTINUE 50 | } 51 | else -> {} 52 | } 53 | } 54 | 55 | override abstract fun entry(currentTime: Double) 56 | override abstract fun exit(currentTime: Double) 57 | override abstract fun action(currentTime: Double, lastTime: Double) 58 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/DelayStep.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | class DelayStep(val time: Double): AutoStep() { 4 | var startTime = 0.0 5 | 6 | override fun entry(currentTime: Double) { 7 | done = false 8 | startTime = currentTime 9 | } 10 | 11 | override fun action(currentTime: Double, lastTime: Double) { 12 | if (currentTime - startTime >= time) { 13 | done = true 14 | } 15 | } 16 | 17 | override fun exit(currentTime: Double) {} 18 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/IAutoStep.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 2/20/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 2/20/18 14 | */ 15 | 16 | interface IAutoStep { 17 | fun entry(currentTime: Double) 18 | fun action(currentTime: Double, lastTime: Double) 19 | fun exit(currentTime: Double) 20 | 21 | fun tick(currentTime: Double, lastTime: Double) 22 | fun reset() 23 | 24 | var done: Boolean 25 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/LambdaStep.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 1/23/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 1/23/18 14 | */ 15 | 16 | class LambdaStep(val lambda: () -> Unit): SingleStep() { 17 | override fun entry(currentTime: Double) { 18 | lambda() 19 | } 20 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/ParallelSteps.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 1/25/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 1/25/18 14 | */ 15 | 16 | class ParallelSteps(vararg val steps: AutoStep): AutoStep() { 17 | override fun entry(currentTime: Double) { 18 | done = false 19 | } 20 | 21 | override fun action(currentTime: Double, lastTime: Double) { 22 | steps.forEach { 23 | it.tick(currentTime, lastTime) 24 | } 25 | if (steps.all { it.doContinue() } || steps.isEmpty()) { 26 | done = true 27 | } 28 | } 29 | 30 | override fun exit(currentTime: Double) { 31 | steps.forEach { 32 | if (it.state != AutoStep.State.CONTINUE) { 33 | it.exit(currentTime) 34 | } 35 | } 36 | } 37 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/SequentialSteps.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 2/20/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 2/20/18 14 | */ 15 | 16 | class SequentialSteps(vararg val steps: AutoStep): AutoStep() { 17 | private var idx = 0 18 | 19 | override fun entry(currentTime: Double) { 20 | done = false 21 | idx = 0 22 | } 23 | 24 | override fun action(currentTime: Double, lastTime: Double) { 25 | if (idx < steps.size) { 26 | steps[idx].tick(currentTime, lastTime) 27 | if (steps[idx].doContinue()) { 28 | idx++ 29 | } 30 | } else { 31 | done = true 32 | } 33 | } 34 | 35 | override fun exit(currentTime: Double) { 36 | steps.forEach { 37 | if (it.state != AutoStep.State.CONTINUE) { 38 | it.exit(currentTime) 39 | } 40 | } 41 | } 42 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/SingleStep.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 1/23/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 1/23/18 14 | */ 15 | abstract class SingleStep: AutoStep(true) { 16 | override fun action(currentTime: Double, lastTime: Double) {} 17 | override fun exit(currentTime: Double) {} 18 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/StateStep.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | import org.snakeskin.dsl.Subsystem 4 | 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 1/23/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 1/23/18 17 | */ 18 | 19 | class StateStep(val subsystem: Subsystem, val machine: String, val toState: String, val wait: Boolean = false): SingleStep() { 20 | override fun entry(currentTime: Double) { 21 | val waitable = subsystem.getStateMachine(machine).setState(toState) 22 | if (wait) { 23 | waitable.waitFor() 24 | } 25 | } 26 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/steps/WaitForStep.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.steps 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 2/17/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 2/17/18 14 | */ 15 | 16 | /** 17 | * Waits for a condition to become true before continuing 18 | */ 19 | class WaitForStep(val timeout: Double = -1.0, val condition: () -> Boolean): AutoStep() { 20 | var startTime = 0.0 21 | 22 | override fun entry(currentTime: Double) { 23 | startTime = currentTime 24 | } 25 | 26 | override fun action(currentTime: Double, lastTime: Double) { 27 | done = ((timeout > 0.0 && currentTime - startTime >= timeout) || condition()) 28 | 29 | } 30 | override fun exit(currentTime: Double) {} 31 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/supermotion/MotionMath.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.supermotion 2 | 3 | import java.text.DecimalFormat 4 | 5 | /* 6 | * 2018-Robot-Code - Created on 3/31/18 7 | * Author: Cameron Earle 8 | * 9 | * This code is licensed under the GNU GPL v3 10 | * You can find more info in the LICENSE file at project root 11 | */ 12 | 13 | /** 14 | * @author Cameron Earle 15 | * @version 3/31/18 16 | */ 17 | object MotionMath { 18 | val df = DecimalFormat("#0.000") 19 | 20 | fun epsilonEquals(a: Double, b: Double, epsilon: Double): Boolean = a - epsilon <= b && a + epsilon >= b 21 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/supermotion/transformation/Interpolatable.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.supermotion.transformation 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 3/31/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 3/31/18 14 | */ 15 | interface Interpolatable { 16 | fun interpolate(other: T, x: Double): T 17 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/supermotion/transformation/RigidTransform.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.supermotion.transformation 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 3/31/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 3/31/18 14 | */ 15 | class RigidTransform(var translation: Translation = Translation(), var rotation: Rotation = Rotation()) { 16 | constructor(other: RigidTransform) : this(Translation(other.translation), Rotation(other.rotation)) 17 | 18 | 19 | companion object: TransformationCompanion { 20 | override val identity = RigidTransform() 21 | const val EPSILON = 1E-9 22 | 23 | fun fromTranslation(translation: Translation) = RigidTransform(translation, Rotation()) 24 | fun fromRotation(rotation: Rotation) = RigidTransform(Translation(), rotation) 25 | 26 | fun exp(delta: Twist): RigidTransform { 27 | val sinTheta = Math.sin(delta.dTheta) 28 | val cosTheta = Math.cos(delta.dTheta) 29 | val s: Double 30 | val c: Double 31 | if (Math.abs(delta.dTheta) < EPSILON) { 32 | s = 1.0 - 1.0 / 6.0 * delta.dTheta * delta.dTheta 33 | c = .5 * delta.dTheta 34 | } else { 35 | s = sinTheta / delta.dTheta 36 | c = (1.0 - cosTheta) / delta.dTheta 37 | } 38 | return RigidTransform( 39 | Translation(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s), 40 | Rotation(cosTheta, sinTheta, false) 41 | ) 42 | } 43 | 44 | fun log(transform: RigidTransform) { 45 | val dTheta = transform.rotation.getRadians() 46 | } 47 | } 48 | 49 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/supermotion/transformation/Rotation.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.supermotion.transformation 2 | 3 | import org.team401.robot2018.auto.supermotion.MotionMath 4 | 5 | /* 6 | * 2018-Robot-Code - Created on 3/31/18 7 | * Author: Cameron Earle 8 | * 9 | * This code is licensed under the GNU GPL v3 10 | * You can find more info in the LICENSE file at project root 11 | */ 12 | 13 | /** 14 | * @author Cameron Earle 15 | * @version 3/31/18 16 | */ 17 | class Rotation(var cos: Double = 1.0, var sin: Double = 0.0, normalize: Boolean = false): Interpolatable { 18 | init { 19 | if (normalize) normalize() 20 | } 21 | 22 | constructor(other: Rotation) : this(other.cos, other.sin) 23 | constructor(direction: Translation, normalize: Boolean) : this(direction.x, direction.y, normalize) 24 | 25 | fun normalize() { 26 | val magnitude = Math.hypot(cos, sin) 27 | if (magnitude > EPSILON) { 28 | sin /= magnitude 29 | cos /= magnitude 30 | } else { 31 | sin = 0.0 32 | cos = 1.0 33 | } 34 | } 35 | 36 | val tan: Double 37 | get() = if (Math.abs(cos) < EPSILON) 38 | if (sin >= 0.0) Double.POSITIVE_INFINITY 39 | else Double.NEGATIVE_INFINITY 40 | else sin / cos 41 | 42 | fun getRadians() = Math.atan2(sin, cos) 43 | fun getDegrees() = Math.toDegrees(getRadians()) 44 | 45 | fun rotateBy(other: Rotation) = Rotation( 46 | cos * other.cos - sin * other.sin, 47 | cos * other.sin + sin * other.cos, 48 | true 49 | ) 50 | 51 | fun normal() = Rotation(-sin, cos, false) 52 | fun inverse() = Rotation(cos, -sin, false) 53 | 54 | fun toTranslation() = Translation(cos, sin) 55 | 56 | fun isParallel(other: Rotation) = MotionMath.epsilonEquals( 57 | Translation.cross(toTranslation(), other.toTranslation()), 58 | 0.0, 59 | EPSILON 60 | ) 61 | 62 | override fun interpolate(other: Rotation, x: Double): Rotation { 63 | if (x <= 0) return Rotation(this) 64 | else if (x >= 1) return Rotation(other) 65 | val angleDiff = inverse().rotateBy(other).getRadians() 66 | return rotateBy(fromRadians(angleDiff * x)) 67 | } 68 | 69 | companion object: TransformationCompanion { 70 | override val identity = Rotation() 71 | 72 | const val EPSILON = 1E-9 73 | 74 | fun fromRadians(angle: Double) = Rotation(Math.cos(angle), Math.sin(angle), false) 75 | fun fromDegrees(angle: Double) = fromRadians(Math.toRadians(angle)) 76 | } 77 | 78 | override fun toString() = "(${MotionMath.df.format(getDegrees())} deg)" 79 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/supermotion/transformation/TransformationCompanion.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.supermotion.transformation 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 3/31/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 3/31/18 14 | */ 15 | interface TransformationCompanion { 16 | val identity: T 17 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/supermotion/transformation/Translation.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.supermotion.transformation 2 | 3 | import org.team401.robot2018.auto.supermotion.MotionMath 4 | 5 | /* 6 | * 2018-Robot-Code - Created on 3/31/18 7 | * Author: Cameron Earle 8 | * 9 | * This code is licensed under the GNU GPL v3 10 | * You can find more info in the LICENSE file at project root 11 | */ 12 | 13 | /** 14 | * @author Cameron Earle 15 | * @version 3/31/18 16 | */ 17 | class Translation(var x: Double = 0.0, var y: Double = 0.0): Interpolatable { 18 | constructor(other: Translation) : this(other.x, other.y) 19 | constructor(start: Translation, end: Translation) : this(end.x - start.x, 20 | end.y - start.y) 21 | 22 | fun norm() = Math.hypot(x, y) 23 | fun norm2() = x * x + y * y 24 | 25 | fun translateBy(other: Translation) = Translation(x + other.x, y + other.y) 26 | fun rotateBy(rotation: Rotation) = Translation( 27 | x * rotation.cos - y * rotation.sin, 28 | x * rotation.sin + y * rotation.cos 29 | ) 30 | fun direction() = Rotation(x, y, true) 31 | fun inverse() = Translation(-x, -y) 32 | 33 | override fun interpolate(other: Translation, x: Double) = when { 34 | x <= 0 -> Translation(this) 35 | x >= 1 -> Translation(other) 36 | else -> extrapolate(other, x) 37 | } 38 | fun extrapolate(other: Translation, x: Double) = Translation( 39 | x * (other.x - this.x) + this.x, 40 | x * (other.y - this.y) + this.y 41 | ) 42 | fun scale(scale: Double) = Translation(x * scale, y * scale) 43 | 44 | companion object: TransformationCompanion { 45 | override val identity = Translation() 46 | 47 | fun dot(a: Translation, b: Translation) = a.x * b.x + a.y * b.y 48 | fun cross(a: Translation, b: Translation) = a.x * b.y - a.y * b.x 49 | 50 | fun getAngle(a: Translation, b: Translation): Rotation { 51 | val cos = dot(a, b) / (a.norm() * b.norm()) 52 | if (cos.isNaN()) return Rotation() 53 | return Rotation.fromRadians(Math.acos(Math.min(1.0, Math.max(cos, -1.0)))) 54 | } 55 | } 56 | 57 | override fun toString() = "(${MotionMath.df.format(x)},${MotionMath.df.format(y)})" 58 | } 59 | -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/auto/supermotion/transformation/Twist.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.auto.supermotion.transformation 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 3/31/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 3/31/18 14 | */ 15 | class Twist(val dx: Double, val dy: Double, val dTheta: Double) { 16 | fun scaled(scale: Double) = Twist(dx * scale, dy * scale, dTheta * scale) 17 | 18 | companion object: TransformationCompanion { 19 | override val identity = Twist(0.0, 0.0, 0.0) 20 | } 21 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/constants/CompConstants.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.constants 2 | 3 | import org.team401.robot2018.auto.motion.DriveGains 4 | import org.team401.robot2018.auto.motion.PDVA 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 3/7/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 3/7/18 17 | */ 18 | class CompConstants: ConstantsBase() { 19 | override val DrivetrainParameters = object: DrivetrainParametersConfig() { 20 | override val WHEEL_DIAMETER = 5.95 21 | 22 | override val DELTA = 0.5 23 | override val SPEED_THRESHOLD = 7.5 24 | override val SPEED_SPLIT = 3.5 25 | 26 | override val LEFT_PDVA = PDVA(.35, 0.0, 1/650.0, 0.0005) 27 | override val RIGHT_PDVA = PDVA(.35, 0.0, 1/650.0, 0.0005) 28 | override val DRIVE_GAINS = DriveGains(0.65, 0.0026, 1/450.0, .0005, .045) 29 | 30 | override val HEADING_GAIN = .017 31 | override val HEADING_D = .5 32 | 33 | override val TIP_CORRECTION_SCALAR = 10 34 | override val PITCH_CORRECTION_MIN = 10 35 | override val ROLL_CORRECTION_MIN = 10 36 | } 37 | 38 | override val ElevatorParameters = object: ElevatorParametersConfig() { 39 | override val RATCHET_UNLOCKED_SERVO_POS = 74.0 40 | override val RATCHET_LOCKED_SERVO_POS = 90.0 41 | 42 | override val PIDF = object: PIDF { 43 | override val P = 0.5 44 | override val I = 0.0 45 | override val D = 0.0 46 | override val F = 1/100.0 47 | } 48 | } 49 | 50 | override val IntakeParameters = object: IntakeParametersConfig() { 51 | override val HAVE_CUBE_CURRENT_CLAMP = 10.0 52 | override val HAVE_CUBE_CURRENT_LEFT_HOLD = 0.0 53 | override val HAVE_CUBE_CURRENT_LEFT_INTAKE = 4.5 54 | override val HAVE_CUBE_CURRENT_RIGHT_HOLD = 0.0 55 | override val HAVE_CUBE_CURRENT_RIGHT_INTAKE = 4.5 56 | 57 | override val PIDF = object: PIDF { 58 | override val P = 4.0 59 | override val I = 0.0 60 | override val D = 35.0 61 | override val F = 0.0 62 | } 63 | } 64 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/constants/Constants.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.constants 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 3/7/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 3/7/18 14 | */ 15 | lateinit var Constants: ConstantsBase -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/constants/PracticeConstants.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.constants 2 | 3 | import org.team401.robot2018.auto.motion.DriveGains 4 | import org.team401.robot2018.auto.motion.PDVA 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 3/7/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 3/7/18 17 | */ 18 | class PracticeConstants: ConstantsBase() { 19 | override val DrivetrainParameters = object: DrivetrainParametersConfig() { 20 | override val WHEEL_DIAMETER = 6.0 21 | 22 | override val DELTA = 0.5 23 | override val SPEED_THRESHOLD = 7.5 24 | override val SPEED_SPLIT = 3.5 25 | 26 | override val LEFT_PDVA = PDVA(1/19.0, 0/3.0, 1/1200.0, 0.0) 27 | override val RIGHT_PDVA = PDVA(1/19.0, 0/3.0, 1/1200.0, 0.0) 28 | override val DRIVE_GAINS = DriveGains() 29 | 30 | override val HEADING_GAIN = .0025 31 | override val HEADING_D = 0.5 32 | 33 | override val TIP_CORRECTION_SCALAR = 10 34 | override val PITCH_CORRECTION_MIN = 10 35 | override val ROLL_CORRECTION_MIN = 10 36 | } 37 | 38 | override val ElevatorParameters = object: ElevatorParametersConfig() { 39 | override val RATCHET_UNLOCKED_SERVO_POS = 84.0 40 | override val RATCHET_LOCKED_SERVO_POS = 97.0 41 | 42 | override val PIDF = object: PIDF { 43 | override val P = 0.5 44 | override val I = 0.0 45 | override val D = 0.0 46 | override val F = 1/100.0 47 | } 48 | } 49 | 50 | override val IntakeParameters = object: IntakeParametersConfig() { 51 | override val HAVE_CUBE_CURRENT_CLAMP = 0.0 52 | override val HAVE_CUBE_CURRENT_LEFT_HOLD = 0.0 53 | override val HAVE_CUBE_CURRENT_LEFT_INTAKE = 0.0 54 | override val HAVE_CUBE_CURRENT_RIGHT_HOLD = 0.0 55 | override val HAVE_CUBE_CURRENT_RIGHT_INTAKE = 0.0 56 | 57 | override val PIDF = object: PIDF { 58 | override val P = 3.5 59 | override val I = 0.0 60 | override val D = 35.0 61 | override val F = 0.0 62 | } 63 | } 64 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/etc/Extensions.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.etc 2 | 3 | import com.ctre.phoenix.ParamEnum 4 | import com.ctre.phoenix.motorcontrol.* 5 | import com.ctre.phoenix.motorcontrol.can.TalonSRX 6 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser 7 | import openrio.powerup.MatchData 8 | import org.snakeskin.LightLink 9 | import org.snakeskin.component.Gearbox 10 | import org.snakeskin.component.TankDrivetrain 11 | import org.team401.robot2018.PDP 12 | import org.team401.robot2018.auto.steps.AutoStep 13 | import org.team401.robot2018.constants.ConstantsBase 14 | import org.team401.robot2018.subsystems.ShiftCommand 15 | 16 | /* 17 | * 2018-Robot-Code - Created on 1/17/18 18 | * Author: Cameron Earle 19 | * 20 | * This code is licensed under the GNU GPL v3 21 | * You can find more info in the LICENSE file at project root 22 | */ 23 | 24 | /** 25 | * @author Cameron Earle 26 | * @version 1/17/18 27 | */ 28 | typealias StepAdder = (AutoStep) -> Unit 29 | 30 | fun IMotorControllerEnhanced.configZeroPosOnReverseLimit(enable: Boolean, timeout: Int = 0) { 31 | configSetParameter(ParamEnum.eClearPosOnLimitR, if (enable) 1.0 else 0.0, 0, 0, timeout) 32 | } 33 | 34 | fun IMotorControllerEnhanced.configZeroPosOnForwardLimit(enable: Boolean, timeout: Int = 0) { 35 | configSetParameter(ParamEnum.eClearPosOnLimitF, if (enable) 1.0 else 0.0, 0, 0, timeout) 36 | } 37 | 38 | fun MatchData.OwnedSide.invert(): MatchData.OwnedSide { 39 | return when (this) { 40 | MatchData.OwnedSide.RIGHT -> MatchData.OwnedSide.LEFT 41 | MatchData.OwnedSide.LEFT -> MatchData.OwnedSide.RIGHT 42 | else -> MatchData.OwnedSide.UNKNOWN 43 | } 44 | } 45 | 46 | operator fun MatchData.OwnedSide.not() = this.invert() 47 | 48 | fun IMotorControllerEnhanced.pidf(p: Double = 0.0, i: Double = 0.0, d: Double = 0.0, f: Double = 0.0, slot: Int = 0, timeout: Int = 0) { 49 | config_kP(slot, p, timeout) 50 | config_kI(slot, i, timeout) 51 | config_kD(slot, d, timeout) 52 | config_kF(slot, f, timeout) 53 | } 54 | 55 | fun IMotorControllerEnhanced.pidf(pidf: ConstantsBase.PIDF) { 56 | pidf(pidf.P, pidf.I, pidf.D, pidf.F) 57 | } 58 | 59 | fun IMotorControllerEnhanced.voltageCompensation(desiredOutput : Double, nominal: Double) { 60 | set(ControlMode.PercentOutput, desiredOutput * (nominal/ busVoltage)) 61 | } 62 | 63 | fun TankDrivetrain.getCurrent() = Math.max(left.master.outputCurrent, right.master.outputCurrent) 64 | 65 | fun Gearbox.getCurrent(vararg pdpIds: Int): Double { 66 | val amps = arrayListOf(master.outputCurrent) 67 | slaves.forEachIndexed { 68 | i, slave -> 69 | if (slave is TalonSRX) { 70 | amps.add(slave.outputCurrent) 71 | } else { 72 | amps.add(PDP.getCurrent(pdpIds[i])) 73 | } 74 | } 75 | 76 | return amps.average() 77 | } 78 | 79 | fun TankDrivetrain.shiftUpdate(state: ShiftCommand): Boolean { 80 | if (shifterState != state.state) { 81 | shift(state.state) 82 | println("Drivetrain Shifted: $state") 83 | return true 84 | } 85 | return false 86 | } 87 | 88 | fun TankDrivetrain.unlinkSides() { 89 | right.master.set(ControlMode.PercentOutput, 0.0) 90 | left.master.set(ControlMode.PercentOutput, 0.0) 91 | } 92 | 93 | fun Gearbox.encoderMissing() = (master as TalonSRX).sensorCollection.pulseWidthRiseToRiseUs == 0 94 | 95 | fun Number.withinTolerance(other: Number, tolerance: Number) = Math.abs(this.toDouble() - other.toDouble()) < tolerance.toDouble() -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/etc/LED.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.etc 2 | 3 | import org.snakeskin.LightLink 4 | 5 | object LED { 6 | private val ll = LightLink() 7 | const val LEFT_STRIP = 0 8 | const val TOP_STRIP = 1 9 | const val RIGHT_STRIP = 2 10 | 11 | fun rainbowAll(speed: Int = LightLink.Speed.SLOW) { 12 | ll.rainbow(speed, LEFT_STRIP) 13 | ll.rainbow(speed, TOP_STRIP) 14 | ll.rainbow(speed, RIGHT_STRIP) 15 | } 16 | 17 | fun offAll() { 18 | ll.off(LEFT_STRIP) 19 | ll.off(TOP_STRIP) 20 | ll.off(RIGHT_STRIP) 21 | } 22 | 23 | fun intakeGrab() { 24 | ll.solid(LightLink.Color.BLUE, LEFT_STRIP) 25 | ll.solid(LightLink.Color.BLUE, TOP_STRIP) 26 | ll.solid(LightLink.Color.BLUE, RIGHT_STRIP) 27 | } 28 | 29 | fun intakeOut() { 30 | ll.solid(LightLink.Color.BLUE, LEFT_STRIP) 31 | ll.solid(LightLink.Color.BLUE, TOP_STRIP) 32 | ll.solid(LightLink.Color.BLUE, RIGHT_STRIP) 33 | } 34 | 35 | fun intakeRetract() { 36 | offAll() 37 | } 38 | 39 | fun signalWantCube() { 40 | ll.signal(LightLink.Color.YELLOW, LEFT_STRIP) 41 | ll.signal(LightLink.Color.YELLOW, TOP_STRIP) 42 | ll.signal(LightLink.Color.YELLOW, RIGHT_STRIP) 43 | } 44 | 45 | fun signalHaveCube() { 46 | ll.signal(LightLink.Color.GREEN, LEFT_STRIP) 47 | ll.signal(LightLink.Color.GREEN, TOP_STRIP) 48 | ll.signal(LightLink.Color.GREEN, RIGHT_STRIP) 49 | } 50 | 51 | fun signalScoreCube() { 52 | ll.signal(LightLink.Color.BLUE, LEFT_STRIP) 53 | ll.signal(LightLink.Color.BLUE, TOP_STRIP) 54 | ll.signal(LightLink.Color.BLUE, RIGHT_STRIP) 55 | } 56 | 57 | fun signalEjectCube() { 58 | //Forgot to add red in LightLink 59 | ll.signal(0x01, LEFT_STRIP) 60 | ll.signal(0x01, TOP_STRIP) 61 | ll.signal(0x01, RIGHT_STRIP) 62 | } 63 | 64 | fun startClimb() { 65 | ll.race(LightLink.Color.YELLOW, LightLink.Speed.SLOW, LEFT_STRIP) 66 | ll.race(LightLink.Color.YELLOW, LightLink.Speed.SLOW, RIGHT_STRIP) 67 | ll.blink(LightLink.Color.YELLOW, LightLink.Speed.SLOW, TOP_STRIP) 68 | } 69 | 70 | fun gotClimb() { 71 | ll.signal(LightLink.Color.GREEN, TOP_STRIP) 72 | } 73 | 74 | fun finishClimb() { 75 | ll.breathe(LightLink.Color.GREEN, LightLink.Speed.FAST, LEFT_STRIP) 76 | ll.breathe(LightLink.Color.GREEN, LightLink.Speed.FAST, RIGHT_STRIP) 77 | ll.blink(LightLink.Color.GREEN, LightLink.Speed.SLOW, TOP_STRIP) 78 | } 79 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/etc/RobotEvents.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.etc 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 2/19/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 2/19/18 14 | */ 15 | 16 | object RobotEvents { 17 | val HAVE_CUBE = "haveCube" 18 | val EJECT_CUBE = "reverseCube" 19 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/etc/RobotMath.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.etc 2 | 3 | import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced 4 | import org.team401.robot2018.constants.Constants 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 2/9/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 2/9/18 17 | */ 18 | 19 | object RobotMath { 20 | object Elevator { 21 | fun inchesToTicks(inches: Double, 22 | pitchDiameter: Double = 1.805, 23 | ticksPerRev: Double = 4096.0) = (ticksPerRev * inches) / (Math.PI * pitchDiameter) 24 | 25 | fun feetToTicks(feet: Double, 26 | pitchDiameter: Double = 1.805, 27 | ticksPerRev: Double = 4096.0) = inchesToTicks(feet * 12.0, pitchDiameter, ticksPerRev) 28 | } 29 | 30 | object Drivetrain { 31 | /** 32 | * Converts an input inches to native units (ticks), based on the constants encoder ticks per rev and wheel diameter 33 | */ 34 | fun inchesToNativeUnits(inches: Double) = (Constants.DrivetrainParameters.ENCODER_TICKS * inches) / (Math.PI * Constants.DrivetrainParameters.WHEEL_DIAMETER) 35 | 36 | /** 37 | * Converts an input inches per second to native units (ticks / 100ms), based on the constants encoder ticks per rev and wheel diameter 38 | */ 39 | fun ipsToNativeUnits(ips: Double) = (Constants.DrivetrainParameters.ENCODER_TICKS * ips) / (10.0 * Math.PI * Constants.DrivetrainParameters.WHEEL_DIAMETER) 40 | 41 | fun nativeUnitsToInches(native: Double) = (Math.PI * Constants.DrivetrainParameters.WHEEL_DIAMETER * native) / Constants.DrivetrainParameters.ENCODER_TICKS 42 | fun nativeUnitsToIps(native: Double) = (10.0 * Math.PI * Constants.DrivetrainParameters.WHEEL_DIAMETER * native) / Constants.DrivetrainParameters.ENCODER_TICKS 43 | } 44 | 45 | object UnitConversions { 46 | fun revolutionsToNativeUnits(rotations: Double, ticksPerRev: Double = 4096.0) = rotations * ticksPerRev 47 | 48 | fun nativeUnitsToRevolutions(native: Double, ticksPerRev: Double = 4096.0) = native / ticksPerRev 49 | fun nativeUnitsToRpm(native: Double, ticksPerRev: Double = 4096.0) = native * 600.0 / ticksPerRev 50 | 51 | fun degreesToCTREDumbUnit(degrees: Double) = degrees * 64.0 //Because that makes sense 52 | 53 | fun inchesToTicks(inches: Double, radius: Double, ticksPerRev: Double = 4096.0) = (ticksPerRev * inches) / (2 * Math.PI * radius) 54 | } 55 | 56 | fun averageCurrent(vararg motors: IMotorControllerEnhanced): Double = motors.map { it.outputCurrent }.average() 57 | fun limit180(degIn: Double): Double { 58 | var degOut = degIn 59 | while (degOut >= 180.0) degOut -= 360.0 60 | while (degOut < -180.0) degOut += 360.0 61 | return degOut 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/etc/TalonEnums.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.etc 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 4/10/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 4/10/18 14 | */ 15 | object TalonEnums { 16 | const val DISTANCE_PID = 0 17 | const val HEADING_PID = 1 18 | 19 | const val REMOTE_O = 0 20 | const val REMOTE_1 = 1 21 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/subsystems/Rungs.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.subsystems 2 | 3 | import edu.wpi.first.wpilibj.Solenoid 4 | import org.snakeskin.dsl.Subsystem 5 | import org.snakeskin.dsl.buildSubsystem 6 | import org.team401.robot2018.constants.Constants 7 | 8 | /* 9 | * 2018-Robot-Code - Created on 1/15/18 10 | * Author: Cameron Earle 11 | * 12 | * This code is licensed under the GNU GPL v3 13 | * You can find more info in the LICENSE file at project root 14 | */ 15 | 16 | /** 17 | * @author Cameron Earle 18 | * @version 1/15/18 19 | */ 20 | 21 | const val RUNGS_MACHINE = "rungs" 22 | object RungsStates { 23 | const val STOWED = "stowed" 24 | const val DEPLOY = "deploy" 25 | const val DEPLOYED = "deployed" 26 | } 27 | 28 | object Rungs { 29 | lateinit var deployer: Solenoid 30 | } 31 | 32 | val RungsSubsystem: Subsystem = buildSubsystem { 33 | val deployer = Solenoid(Constants.Pneumatics.RUNGS_DEPLOY_SOLENOID) 34 | 35 | setup { 36 | Rungs.deployer = deployer 37 | } 38 | 39 | val rungsMachine = stateMachine(RUNGS_MACHINE) { 40 | //Constants for setting solenoid polarity 41 | val locked = false 42 | val unlocked = true 43 | 44 | state(RungsStates.STOWED) { 45 | entry { 46 | deployer.set(locked) 47 | } 48 | } 49 | 50 | state(RungsStates.DEPLOY) { 51 | timeout(Constants.RungsParameters.DEPLOY_TIMER, RungsStates.DEPLOYED) 52 | 53 | entry { 54 | deployer.set(unlocked) 55 | } 56 | } 57 | 58 | state(RungsStates.DEPLOYED) { 59 | entry { 60 | deployer.set(locked) 61 | } 62 | } 63 | 64 | default { 65 | entry { 66 | deployer.set(false) 67 | } 68 | } 69 | } 70 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/vision/CubeDetectorPipeline.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.vision 2 | 3 | import edu.wpi.first.wpilibj.vision.VisionPipeline 4 | import org.opencv.core.Core 5 | import org.opencv.core.Mat 6 | import org.opencv.core.MatOfPoint 7 | import org.opencv.imgproc.Imgproc 8 | import org.snakeskin.logic.LockingDelegate 9 | import org.team401.robot2018.constants.Constants 10 | 11 | object CubeDetectorPipeline: VisionPipeline { 12 | var closeArms by LockingDelegate(false); private set 13 | var haveCube by LockingDelegate(false); private set 14 | 15 | private var closeArmsSubmat = Mat() 16 | private var haveCubeSubmat = Mat() 17 | private val closeArmsHierarchy = Mat() 18 | private val haveCubeHierarchy = Mat() 19 | private val closeArmsContours = arrayListOf() 20 | private val haveCubeContorus = arrayListOf() 21 | 22 | override fun process(image: Mat) { 23 | closeArmsSubmat = image.submat(Constants.VisionParameters.CLOSE_ARMS_RECT) 24 | haveCubeSubmat = image.submat(Constants.VisionParameters.HAVE_CUBE_RECT) 25 | 26 | Imgproc.cvtColor(closeArmsSubmat, closeArmsSubmat, Imgproc.COLOR_BGR2HSV_FULL) 27 | Core.inRange(closeArmsSubmat, Constants.VisionParameters.CUBE_COLOR_MIN, Constants.VisionParameters.CUBE_COLOR_MAX, closeArmsSubmat) 28 | Imgproc.cvtColor(haveCubeSubmat, haveCubeSubmat, Imgproc.COLOR_BGR2HSV_FULL) 29 | Core.inRange(haveCubeSubmat, Constants.VisionParameters.CUBE_COLOR_MIN, Constants.VisionParameters.CUBE_COLOR_MAX, haveCubeSubmat) 30 | 31 | closeArmsContours.clear() 32 | haveCubeContorus.clear() 33 | 34 | Imgproc.findContours(closeArmsSubmat, closeArmsContours, closeArmsHierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE) 35 | Imgproc.findContours(haveCubeSubmat, haveCubeContorus, haveCubeHierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE) 36 | 37 | closeArms = closeArmsContours.isNotEmpty() 38 | haveCube = haveCubeContorus.isNotEmpty() 39 | } 40 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/vision/MjpegServer.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.vision 2 | 3 | import org.opencv.core.Mat 4 | import org.opencv.core.MatOfByte 5 | import org.opencv.core.MatOfInt 6 | import org.opencv.imgcodecs.Imgcodecs 7 | import org.opencv.videoio.VideoCapture 8 | import java.io.ByteArrayOutputStream 9 | import java.net.InetSocketAddress 10 | import java.net.ServerSocket 11 | import java.net.Socket 12 | import java.net.SocketTimeoutException 13 | import java.util.* 14 | import java.util.concurrent.Executors 15 | import java.util.concurrent.ScheduledFuture 16 | import java.util.concurrent.TimeUnit 17 | 18 | /* 19 | * 2018-Robot-Code - Created on 3/9/18 20 | * Author: Cameron Earle 21 | * 22 | * This code is licensed under the GNU GPL v3 23 | * You can find more info in the LICENSE file at project root 24 | */ 25 | 26 | /** 27 | * @author Cameron Earle 28 | * @version 3/9/18 29 | */ 30 | class MjpegServer (val port: Int, val camera: Int= 0, val width: Int = 640, val height: Int = 480, 31 | framerate: Int = 30, val quality: Int = 30, val timeout: Int = 2000) { 32 | 33 | private companion object { 34 | val TERM = "\r\n\r\n".toByteArray() 35 | val HEAD = ( 36 | "HTTP/1.0 200 OK\r\n" + 37 | "Server: SnakeEyes\r\n" + 38 | "Connection: close\r\n" + 39 | "Max-Age: 0\r\n" + 40 | "Expires: 0\r\n" + 41 | "Cache-Control: no-cache, private\r\n" + 42 | "Pragma: no-cache\r\n" + 43 | "Content-Type: multipart/x-mixed-replace; " + 44 | "boundary=--BoundaryString\r\n\r\n" 45 | ).toByteArray() 46 | } 47 | 48 | private val msPerFrame = (1000/framerate).toLong() 49 | 50 | private lateinit var server: ServerSocket 51 | private var serverThread: Thread? = null 52 | private var future: ScheduledFuture<*>? = null 53 | private val clients = Vector() 54 | private val executor = Executors.newSingleThreadScheduledExecutor() 55 | private lateinit var cap: VideoCapture 56 | 57 | private inner class ConnHandler: Runnable { 58 | override fun run() { 59 | while (!Thread.interrupted()) { 60 | try { 61 | val client = server.accept() 62 | client.soTimeout = timeout 63 | client.getOutputStream().write(HEAD) 64 | clients.add(client) 65 | } catch (e: SocketTimeoutException) {} 66 | } 67 | } 68 | } 69 | 70 | private inner class StreamHandler: Runnable { 71 | val baos = ByteArrayOutputStream() 72 | val mob = MatOfByte() 73 | val mat = Mat() 74 | 75 | val options = MatOfInt(Imgcodecs.CV_IMWRITE_JPEG_QUALITY, quality) 76 | val toRemove = arrayListOf() 77 | 78 | override fun run() { 79 | if (clients.isNotEmpty()) { 80 | cap.read(mat) 81 | Imgcodecs.imencode(".jpg", mat, mob, options) 82 | mat.release() 83 | baos.write(mob.toArray()) 84 | mob.release() 85 | clients.forEach { 86 | try { 87 | val os = it.getOutputStream() 88 | os.write(( 89 | "--BoundaryString\r\n" + 90 | "Content-type: image/jpeg\r\n" + 91 | "Content-Length: " + 92 | baos.size() + 93 | "\r\n\r\n" 94 | ).toByteArray()) 95 | baos.writeTo(os) 96 | os.write(TERM) 97 | os.flush() 98 | } catch (e: Exception) { //Client disconnected 99 | toRemove.add(it) 100 | } 101 | } 102 | clients.removeAll(toRemove) 103 | toRemove.clear() 104 | baos.reset() 105 | } 106 | } 107 | } 108 | 109 | fun start() { 110 | cap = VideoCapture(camera) 111 | server = ServerSocket() 112 | server.setPerformancePreferences(0, 1, 2) 113 | server.soTimeout = 100 114 | server.bind(InetSocketAddress(port)) 115 | serverThread = Thread(ConnHandler()) 116 | serverThread?.start() 117 | future = executor.scheduleAtFixedRate(StreamHandler(), 0L, msPerFrame, TimeUnit.MILLISECONDS) 118 | } 119 | 120 | fun stop() { 121 | future?.cancel(false) 122 | serverThread?.interrupt() 123 | serverThread?.join() 124 | server.close() 125 | cap.release() 126 | } 127 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/vision/VisionController.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.vision 2 | 3 | import org.snakeskin.factory.ExecutorFactory 4 | import org.zeromq.ZMQ 5 | 6 | /* 7 | * 2018-Robot-Code - Created on 1/18/18 8 | * Author: Cameron Earle 9 | * 10 | * This code is licensed under the GNU GPL v3 11 | * You can find more info in the LICENSE file at project root 12 | */ 13 | 14 | /** 15 | * @author Cameron Earle 16 | * @version 1/18/18 17 | */ 18 | class VisionController(address: String, port: Int = 5801) { 19 | val executor = ExecutorFactory.getSingleExecutor("visionController") 20 | 21 | private val context = ZMQ.context(0) 22 | private lateinit var socket: ZMQ.Socket 23 | 24 | init { 25 | executor.submit { 26 | socket = context.socket(ZMQ.REQ) 27 | socket.connect("tcp://$address:$port") 28 | } 29 | } 30 | 31 | fun sendCommand(command: String) { 32 | executor.submit { 33 | socket.send(command) 34 | socket.recv() 35 | } 36 | } 37 | 38 | fun setup(time: Long, matchNo: String) = sendCommand("SETUP,$time,$matchNo") 39 | fun start() = sendCommand("START") 40 | fun stop() = sendCommand("STOP") 41 | } -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/vision/VisionData.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.vision 2 | 3 | /* 4 | * 2018-Robot-Code - Created on 2/6/18 5 | * Author: Cameron Earle 6 | * 7 | * This code is licensed under the GNU GPL v3 8 | * You can find more info in the LICENSE file at project root 9 | */ 10 | 11 | /** 12 | * @author Cameron Earle 13 | * @version 2/6/18 14 | */ 15 | 16 | data class VisionData(val isCubePresent: Boolean, 17 | val cubeX: Double = 0.0, 18 | val cubeY: Double = 0.0) -------------------------------------------------------------------------------- /robot/src/main/kotlin/org/team401/robot2018/vision/VisionDataClient.kt: -------------------------------------------------------------------------------- 1 | package org.team401.robot2018.vision 2 | 3 | import com.google.gson.Gson 4 | import org.snakeskin.logic.LockingDelegate 5 | import org.zeromq.ZMQ 6 | 7 | /* 8 | * 2018-Robot-Code - Created on 2/6/18 9 | * Author: Cameron Earle 10 | * 11 | * This code is licensed under the GNU GPL v3 12 | * You can find more info in the LICENSE file at project root 13 | */ 14 | 15 | /** 16 | * @author Cameron Earle 17 | * @version 2/6/18 18 | */ 19 | 20 | class VisionDataClient(val address: String, val port: Int) { 21 | private var latestData by LockingDelegate(VisionData(false, 0.0, 0.0)) 22 | 23 | private val context = ZMQ.context(1) 24 | private lateinit var socket: ZMQ.Socket 25 | private val gson = Gson() 26 | private val thread = Thread(this::run) 27 | 28 | private fun run() { 29 | socket = context.socket(ZMQ.SUB) 30 | socket.subscribe("".toByteArray()) 31 | socket.connect("tcp:$address:$port") 32 | socket.receiveTimeOut = 10 //ms 33 | var latest: String? = "" 34 | 35 | while (!Thread.interrupted()) { 36 | try { 37 | latest = socket.recvStr() 38 | if (latest != null && latest != "") { 39 | latestData = gson.fromJson(latest, VisionData::class.java) 40 | } 41 | } catch (e: InterruptedException) { 42 | Thread.currentThread().interrupt() 43 | } 44 | } 45 | } 46 | 47 | fun start() { 48 | thread.start() 49 | } 50 | 51 | fun stop() { 52 | thread.interrupt() 53 | } 54 | 55 | fun read() = latestData 56 | } -------------------------------------------------------------------------------- /robot/waypoints/BACK_UP_REV.csv: -------------------------------------------------------------------------------- 1 | 3.5,12.0,270.0 2 | 3.5,10.0,270.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/BASELINE.csv: -------------------------------------------------------------------------------- 1 | 4.0,3.0,90.0 2 | 4.0,12.0,90.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/DS_CENTER-SWITCH_LEFT.csv: -------------------------------------------------------------------------------- 1 | 15.0,3.0,90.0 2 | 9.5,12.0,90.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/DS_CENTER-SWITCH_RIGHT.csv: -------------------------------------------------------------------------------- 1 | 15.0,3.0,90.0 2 | 17.5,12.0,90.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/DS_LEFT-SCALE_LEFT.csv: -------------------------------------------------------------------------------- 1 | 4.0,3.0,90.0 2 | 4.0,16.0,90.0 3 | 6.0,25.0,45.0 4 | -------------------------------------------------------------------------------- /robot/waypoints/DS_LEFT-SCALE_RIGHT.csv: -------------------------------------------------------------------------------- 1 | 4.0,3.0,90.0 2 | 4.0,12.0,90.0 3 | 10.0,20.0,0.0 4 | 18.0,20.0,0.0 5 | 20.5,25.0,90.0 6 | -------------------------------------------------------------------------------- /robot/waypoints/DS_LEFT-SWITCH_LEFT.csv: -------------------------------------------------------------------------------- 1 | 4.0,3.0,90.0 2 | 4.0,10.0,90.0 3 | 7.5,14.0,0.0 4 | -------------------------------------------------------------------------------- /robot/waypoints/DS_RIGHT-SCALE_LEFT.csv: -------------------------------------------------------------------------------- 1 | 23.0,3.0,90.0 2 | 23.0,12.0,90.0 3 | 17.0,20.0,180.0 4 | 9.0,20.0,180.0 5 | 6.5,25.0,90.0 6 | -------------------------------------------------------------------------------- /robot/waypoints/DS_RIGHT-SCALE_RIGHT.csv: -------------------------------------------------------------------------------- 1 | 23.0,3.0,90.0 2 | 23.0,16.0,90.0 3 | 21.0,25.0,135.0 4 | -------------------------------------------------------------------------------- /robot/waypoints/DS_RIGHT-SWITCH_RIGHT.csv: -------------------------------------------------------------------------------- 1 | 23.0,3.0,90.0 2 | 23.0,10.0,90.0 3 | 19.5,14.0,180.0 4 | -------------------------------------------------------------------------------- /robot/waypoints/SCALE_LEFT-SCALE_LEFT_TWOCUBE1_REV.csv: -------------------------------------------------------------------------------- 1 | 6.0,25.0,45.0 2 | 4.0,25.0,120.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SCALE_LEFT_TWOCUBE1-SCALE_LEFT_TWOCUBE2.csv: -------------------------------------------------------------------------------- 1 | 4.0,25.0,300.0 2 | 8.0,18.0,270.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SCALE_LEFT_TWOCUBE2-SCALE_LEFT_TWOCUBE3_REV.csv: -------------------------------------------------------------------------------- 1 | 8.0,18.0,90.0 2 | 4.0,25.0,120.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SCALE_LEFT_TWOCUBE3-SCALE_LEFT.csv: -------------------------------------------------------------------------------- 1 | 4.0,25.0,300.0 2 | 6.0,25.0,45.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SCALE_RIGHT-SCALE_RIGHT_TWOCUBE1_REV.csv: -------------------------------------------------------------------------------- 1 | 21.0,25.0,135.0 2 | 23.0,25.0,60.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SCALE_RIGHT_TWOCUBE1-SCALE_RIGHT_TWOCUBE2.csv: -------------------------------------------------------------------------------- 1 | 23.0,25.0,240.0 2 | 19.0,18.0,270.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SCALE_RIGHT_TWOCUBE2-SCALE_RIGHT_TWOCUBE3_REV.csv: -------------------------------------------------------------------------------- 1 | 19.0,18.0,90.0 2 | 23.0,25.0,60.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SCALE_RIGHT_TWOCUBE3-SCALE_RIGHT.csv: -------------------------------------------------------------------------------- 1 | 23.0,25.0,240.0 2 | 21.0,25.0,135.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SWITCH_LEFT-SWITCH_LEFT_TWOCUBE1_REV.csv: -------------------------------------------------------------------------------- 1 | 9.5,12.0,270.0 2 | 8.0,8.0,210.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SWITCH_LEFT_TWOCUBE1-SWITCH_LEFT_TWOCUBE2.csv: -------------------------------------------------------------------------------- 1 | 8.0,8.0,30.0 2 | 13.5,10.0,30.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SWITCH_LEFT_TWOCUBE2-SWITCH_LEFT_TWOCUBE3_REV.csv: -------------------------------------------------------------------------------- 1 | 13.5,10.0,210.0 2 | 8.0,10.0,225.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SWITCH_LEFT_TWOCUBE3-SWITCH_LEFT.csv: -------------------------------------------------------------------------------- 1 | 8.0,10.0,45.0 2 | 9.5,12.0,90.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SWITCH_RIGHT-SWITCH_RIGHT_TWOCUBE1_REV.csv: -------------------------------------------------------------------------------- 1 | 17.5,12.0,270.0 2 | 19.0,8.0,330.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SWITCH_RIGHT_TWOCUBE1-SWITCH_RIGHT_TWOCUBE2.csv: -------------------------------------------------------------------------------- 1 | 19.0,8.0,150.0 2 | 13.5,10.0,180.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SWITCH_RIGHT_TWOCUBE2-SWITCH_RIGHT_TWOCUBE3_REV.csv: -------------------------------------------------------------------------------- 1 | 13.5,10.0,0.0 2 | 20.0,10.0,330.0 3 | -------------------------------------------------------------------------------- /robot/waypoints/SWITCH_RIGHT_TWOCUBE3-SWITCH_RIGHT.csv: -------------------------------------------------------------------------------- 1 | 20.0,10.0,150.0 2 | 18.5,12.0,90.0 3 | -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | include ":robot", ":vision" -------------------------------------------------------------------------------- /vision/build.gradle: -------------------------------------------------------------------------------- 1 | apply plugin: 'jaci.gradle.EmbeddedTools' 2 | 3 | def JETSON_IP = "10.4.1.3" 4 | def JETSON_USER = "ubuntu" 5 | def JETSON_PASS = "ubuntu" 6 | 7 | dependencies { 8 | compile group: "org.team401", name: "snakeeyes", version: "1.1" 9 | 10 | compile group: 'org.zeromq', name: 'jeromq', version: '0.4.3' 11 | 12 | compile fileTree(dir: 'libs', include: '**/*.jar') 13 | } 14 | 15 | deploy { 16 | targets { 17 | target("jetson") { 18 | addresses << JETSON_IP 19 | user = JETSON_USER 20 | password = JETSON_PASS 21 | directory = "/home/$JETSON_USER/vision2018" 22 | } 23 | } 24 | artifacts { 25 | javaArtifact("vision2018") { 26 | predeploy << { execute "/home/$JETSON_USER/vision2018/stop.sh"} 27 | postdeploy << { if (Boolean.parseBoolean(System.getProperty("deployDebug") ?: "false")) { execute "/home/$JETSON_USER/vision2018/debug.sh"} else { execute "/home/$JETSON_USER/vision2018/start.sh"} } 28 | targets << "jetson" 29 | jar = "jar" 30 | } 31 | } 32 | } 33 | 34 | jar { 35 | from configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } 36 | manifest { 37 | attributes 'Main-Class': "org.team401.vision2018.VisionKt" 38 | } 39 | } -------------------------------------------------------------------------------- /vision/libs/opencv-340.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/team401/2018-Robot-Code/58788e93e924ab0c77252ab3af659be5cd4f7882/vision/libs/opencv-340.jar -------------------------------------------------------------------------------- /vision/scripts/debug.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | screen -dmS vision2018 java -agentlib:jdwp=transport=dt_socket,server=y,suspend=n,address=8348 -jar vision.jar -------------------------------------------------------------------------------- /vision/scripts/overclock.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # turn on fan for safety 4 | echo "Enabling fan for safety..." 5 | if [ ! -w /sys/kernel/debug/tegra_fan/target_pwm ] ; then 6 | echo "Cannot set fan -- exiting..." 7 | fi 8 | echo 255 > /sys/kernel/debug/tegra_fan/target_pwm 9 | 10 | echo 0 > /sys/devices/system/cpu/cpuquiet/tegra_cpuquiet/enable 11 | echo 1 > /sys/kernel/cluster/immediate 12 | echo 1 > /sys/kernel/cluster/force 13 | echo G > /sys/kernel/cluster/active 14 | echo "Cluster: `cat /sys/kernel/cluster/active`" 15 | 16 | # online all CPUs - ignore errors for already-online units 17 | echo "onlining CPUs: ignore errors..." 18 | for i in 0 1 2 3 ; do 19 | echo 1 > /sys/devices/system/cpu/cpu${i}/online 20 | done 21 | echo "Online CPUs: `cat /sys/devices/system/cpu/online`" 22 | 23 | # set CPUs to max freq (perf governor not enabled on L4T yet) 24 | echo userspace > /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor 25 | cpumax=`cat /sys/devices/system/cpu/cpu0/cpufreq/scaling_available_frequencies | awk '{print $NF}'` 26 | echo "${cpumax}" > /sys/devices/system/cpu/cpu0/cpufreq/scaling_setspeed 27 | for i in 0 1 2 3 ; do 28 | echo "CPU${i}: `cat /sys/devices/system/cpu/cpu0/cpufreq/scaling_cur_freq`" 29 | done 30 | 31 | # max GPU clock (should read from debugfs) 32 | cat /sys/kernel/debug/clock/gbus/max > /sys/kernel/debug/clock/override.gbus/rate 33 | echo 1 > /sys/kernel/debug/clock/override.gbus/state 34 | echo "GPU: `cat /sys/kernel/debug/clock/gbus/rate`" 35 | 36 | # max EMC clock (should read from debugfs) 37 | cat /sys/kernel/debug/clock/emc/max > /sys/kernel/debug/clock/override.emc/rate 38 | echo 1 > /sys/kernel/debug/clock/override.emc/state 39 | echo "EMC: `cat /sys/kernel/debug/clock/emc/rate`" -------------------------------------------------------------------------------- /vision/scripts/start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | screen -dmS vision2018 java -jar vision.jar -------------------------------------------------------------------------------- /vision/scripts/stop.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | screen -X -S vision2018 quit 4 | sleep 2 -------------------------------------------------------------------------------- /vision/src/main/kotlin/org/team401/vision2018/CubeFinderPipeline.kt: -------------------------------------------------------------------------------- 1 | package org.team401.vision2018 2 | 3 | import org.opencv.core.* 4 | import org.opencv.imgproc.Imgproc 5 | import org.team401.snakeeyes.MatProvider 6 | import org.team401.snakeeyes.pipeline.Pipeline 7 | 8 | class CubeFinderPipeline(provider: MatProvider, server: VisionDataServer, val p: VisionParameters): Pipeline(provider) { 9 | val procRangeRect = Rect(Point(p.procRegionStart[0].toDouble(), p.procRegionStart[1].toDouble()), 10 | Point(p.procRegionEnd[0].toDouble(), p.procRegionEnd[1].toDouble())) 11 | 12 | val blurKernel = Size(2.0 * (p.blurRadius + 0.5).toInt() + 1, 2.0 * (p.blurRadius + 0.5).toInt() + 1) 13 | 14 | val threshLower = Scalar(p.hThresh[0], p.sThresh[0], p.vThresh[0]) 15 | val threshUpper = Scalar(p.hThresh[1], p.sThresh[1], p.vThresh[1]) 16 | 17 | val contourMode = if (p.externalContoursOnly) Imgproc.RETR_EXTERNAL else Imgproc.RETR_LIST 18 | 19 | 20 | val contours = arrayListOf() 21 | override fun process(mat: Mat) { 22 | //Grab the region to process 23 | val procMat = mat.submat(procRangeRect) 24 | 25 | //Blur the frame to reduce noise 26 | Imgproc.blur(procMat, procMat, blurKernel) 27 | 28 | //Convert the image to HSV color space 29 | Imgproc.cvtColor(procMat, procMat, Imgproc.COLOR_BGR2HSV) 30 | 31 | //Threshold out unwanted colors 32 | Core.inRange(procMat, threshLower, threshUpper, procMat) 33 | 34 | //Find contours in the image 35 | val hierarchy = Mat() 36 | contours.clear() 37 | Imgproc.findContours(procMat, contours, hierarchy, contourMode, Imgproc.CHAIN_APPROX_SIMPLE) 38 | 39 | //Filter contours 40 | val filteredContours = filterContours(contours) 41 | 42 | val data: VisionData 43 | if (filteredContours.isNotEmpty()) { 44 | val rect = Imgproc.boundingRect(filteredContours[0]) 45 | val center = rect.centerpoint() 46 | 47 | val w2 = procMat.width() / 2 48 | val h2 = procMat.height() / 2 49 | 50 | val cubeX = (center.x - w2)/w2 51 | val cubeY = (center.y - h2)/h2 52 | 53 | data = VisionData(true, cubeX, cubeY) 54 | } else { 55 | data = VisionData(false) 56 | } 57 | } 58 | 59 | private fun map(x: Double, min: Double, max: Double, newMin: Double, newMax: Double): Double { 60 | return (newMax - newMin) * (x - min) / (max - min) + newMin 61 | } 62 | 63 | private fun checkBounds(value: Number, bounds: List) = bounds.size >= 2 && (value.toDouble() < bounds[0].toDouble() || value.toDouble() > bounds[1].toDouble()) 64 | 65 | private fun filterContours(contours: List): List { 66 | val filtered = arrayListOf() 67 | val hull = MatOfInt() 68 | 69 | p.filtering.run { 70 | for (contour in contours) { 71 | val bb = Imgproc.boundingRect(contour) 72 | if (checkBounds(bb.width, width)) continue 73 | if (checkBounds(bb.height, height)) continue 74 | val area = Imgproc.contourArea(contour) 75 | if (area < minArea) continue 76 | if (Imgproc.arcLength(MatOfPoint2f(contour), true) < minPerimeter) continue 77 | Imgproc.convexHull(contour, hull) 78 | val mopHull = MatOfPoint() 79 | mopHull.create(hull.size().height.toInt(), 1, CvType.CV_32SC2) 80 | for (i in 0..hull.size().height.toInt()) { 81 | val index = hull.get(i, 0)[0].toInt() 82 | mopHull.put(i, 0, contour.get(index, 0)[0], contour.get(index, 0)[1]) 83 | } 84 | val solid = 100 * area / Imgproc.contourArea(mopHull) 85 | if (checkBounds(solid, solidity)) continue 86 | if (checkBounds(contour.rows(), vertices)) continue 87 | val ratio = bb.width / bb.height.toDouble() 88 | if (checkBounds(ratio, p.filtering.ratio)) continue 89 | filtered.add(contour) 90 | } 91 | } 92 | 93 | return filtered 94 | } 95 | } -------------------------------------------------------------------------------- /vision/src/main/kotlin/org/team401/vision2018/Extensions.kt: -------------------------------------------------------------------------------- 1 | package org.team401.vision2018 2 | 3 | import org.opencv.core.Point 4 | import org.opencv.core.Rect 5 | 6 | fun Rect.centerpoint() = Point(x + (width / 2.0), y + (height / 2.0)) -------------------------------------------------------------------------------- /vision/src/main/kotlin/org/team401/vision2018/VideoRecorder.kt: -------------------------------------------------------------------------------- 1 | package org.team401.vision2018 2 | 3 | import org.opencv.core.CvType 4 | import org.opencv.core.Size 5 | import org.opencv.videoio.VideoWriter 6 | import org.team401.snakeeyes.service.Service 7 | import org.team401.snakeeyes.view.View 8 | import java.util.concurrent.Executors 9 | import java.util.concurrent.ScheduledFuture 10 | import java.util.concurrent.TimeUnit 11 | 12 | /* 13 | * 2018-Robot-Code - Created on 1/17/18 14 | * Author: Cameron Earle 15 | * 16 | * This code is licensed under the GNU GPL v3 17 | * You can find more info in the LICENSE file at project root 18 | */ 19 | 20 | /** 21 | * @author Cameron Earle 22 | * @version 1/17/18 23 | */ 24 | 25 | class VideoRecorder(val view: View, val filename: String, val width: Int, val height: Int, val framerate: Double = 30.0, 26 | val fourcc: Int = VideoWriter.fourcc('M', 'J', 'P', 'G')): Service { 27 | private val msPerFrame = (1000 / framerate).toLong() 28 | 29 | private val writer = VideoWriter() 30 | 31 | private val executor = Executors.newSingleThreadScheduledExecutor() 32 | private var future: ScheduledFuture<*>? = null 33 | 34 | private fun task() { 35 | if (writer.isOpened) { 36 | val rendered = view.render(width, height, CvType.CV_8UC3) 37 | writer.write(rendered) 38 | rendered.release() 39 | } 40 | } 41 | 42 | override fun start() { 43 | writer.open(filename, fourcc, framerate, Size(width.toDouble(), height.toDouble())) 44 | future = executor.scheduleAtFixedRate(::task, 0L, msPerFrame, TimeUnit.MILLISECONDS) 45 | } 46 | 47 | override fun stop() { 48 | future?.cancel(false) 49 | writer.release() 50 | } 51 | } -------------------------------------------------------------------------------- /vision/src/main/kotlin/org/team401/vision2018/Vision.kt: -------------------------------------------------------------------------------- 1 | package org.team401.vision2018 2 | 3 | import com.google.gson.Gson 4 | import org.opencv.core.Core 5 | import org.team401.snakeeyes.Cameras 6 | import org.team401.snakeeyes.camera.Camera 7 | import org.team401.snakeeyes.service.MjpegServer 8 | import org.team401.snakeeyes.view.CameraView 9 | import org.team401.snakeeyes.view.GridView 10 | import org.team401.snakeeyes.view.PipView 11 | import java.io.File 12 | 13 | /* 14 | * 2018-Robot-Code - Created on 1/5/18 15 | * Author: Cameron Earle 16 | * 17 | * This code is licensed under the GNU GPL v3 18 | * You can find more info in the LICENSE file at project root 19 | */ 20 | 21 | /** 22 | * @author Cameron Earle 23 | * @version 1/5/18 24 | */ 25 | 26 | fun main(args: Array) { 27 | System.loadLibrary(Core.NATIVE_LIBRARY_NAME) 28 | 29 | val top = Camera() 30 | val front = Camera() 31 | 32 | val topView = CameraView(top) 33 | val frontView = CameraView(front) 34 | 35 | top.open("/dev/v4l/by-path/platform-70090000.xusb-usb-0:1:1.0-video-index0") 36 | front.open("/dev/v4l/by-path/platform-70090000.xusb-usb-0:3.1:1.0-video-index0") 37 | 38 | Cameras.add(top) 39 | Cameras.add(front) 40 | Cameras.start() 41 | 42 | val view = PipView(topView, frontView, PipView.Position.BOTTOM_CENTER, .5) 43 | val mjpegServer = MjpegServer(1180, view) 44 | mjpegServer.start() 45 | 46 | val sideBySide = GridView(1, 2) 47 | sideBySide.putView(topView, 0, 0) 48 | sideBySide.putView(frontView, 0, 1) 49 | 50 | val controller = ControllerServer(sideBySide, 640 * sideBySide.cols, 480 * sideBySide.rows, "/media/2018REC") 51 | controller.start() 52 | 53 | 54 | /* 55 | val paramFile = File("parameters.json") 56 | val params = Gson().fromJson(paramFile.readText(), VisionParameters::class.java) 57 | 58 | val dataServer = VisionDataServer(5801) 59 | dataServer.start() 60 | 61 | val cubeFinder = CubeFinderPipeline(top, dataServer, params) 62 | cubeFinder.start() 63 | */ 64 | } -------------------------------------------------------------------------------- /vision/src/main/kotlin/org/team401/vision2018/VisionData.kt: -------------------------------------------------------------------------------- 1 | package org.team401.vision2018 2 | 3 | data class VisionData(val isCubePresent: Boolean, 4 | val cubeX: Double = 0.0, 5 | val cubeY: Double = 0.0) -------------------------------------------------------------------------------- /vision/src/main/kotlin/org/team401/vision2018/VisionDataServer.kt: -------------------------------------------------------------------------------- 1 | package org.team401.vision2018 2 | 3 | import com.google.gson.Gson 4 | import org.team401.snakeeyes.service.Server 5 | import org.zeromq.ZMQ 6 | import java.util.concurrent.ConcurrentLinkedQueue 7 | 8 | /* 9 | * 2018-Robot-Code - Created on 2/5/18 10 | * Author: Cameron Earle 11 | * 12 | * This code is licensed under the GNU GPL v3 13 | * You can find more info in the LICENSE file at project root 14 | */ 15 | 16 | /** 17 | * @author Cameron Earle 18 | * @version 2/5/18 19 | */ 20 | 21 | class VisionDataServer(override val port: Int): Server { 22 | private val queue = ConcurrentLinkedQueue() 23 | private val context = ZMQ.context(1) 24 | private lateinit var socket: ZMQ.Socket 25 | private val thread = Thread(::run) 26 | private val gson = Gson() 27 | 28 | fun send(data: VisionData) = queue.add(data) 29 | 30 | private fun run() { 31 | socket = context.socket(ZMQ.PUB) 32 | socket.bind("tcp://*:$port") 33 | 34 | var latest: VisionData? = queue.poll() //Grab data for the first time 35 | while (!Thread.interrupted()) { //While this thread should run 36 | while (latest != null) { //While there is data 37 | socket.send(gson.toJson(latest)) //Send the data 38 | latest = queue.poll() //Grab new data 39 | } 40 | try { 41 | Thread.sleep(10L) 42 | } catch (e: InterruptedException) { 43 | Thread.currentThread().interrupt() 44 | } 45 | } 46 | } 47 | 48 | override fun start() { 49 | thread.start() 50 | } 51 | 52 | override fun stop() { 53 | thread.interrupt() 54 | thread.join() 55 | } 56 | } -------------------------------------------------------------------------------- /vision/src/main/kotlin/org/team401/vision2018/VisionParameters.kt: -------------------------------------------------------------------------------- 1 | package org.team401.vision2018 2 | 3 | data class VisionParameters(val procRegionStart: List, 4 | val procRegionEnd: List, 5 | val blurRadius: Double, 6 | val hThresh: List, 7 | val sThresh: List, 8 | val vThresh: List, 9 | val externalContoursOnly: Boolean, 10 | val filtering: FilteringParameters, 11 | val topCamera: CameraSettings, 12 | val frontCamera: CameraSettings) { 13 | 14 | data class FilteringParameters(val minArea: Double, 15 | val minPerimeter: Double, 16 | val width: List, 17 | val height: List, 18 | val solidity: List, 19 | val vertices: List, 20 | val ratio: List) 21 | 22 | data class CameraSettings(val brightness: Int) 23 | } --------------------------------------------------------------------------------