Tune auto paths

SouthEugeneRoboticsTeam · Sep 23, 2018 · 22f4859 · 22f4859
1 parent 5ad9e33
commit 22f4859
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 9 deletions.
diff --git a/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt b/src/main/java/org/sert2521/powerup/autonomous/Autonomous.kt
@@ -35,7 +35,7 @@ private fun CommandBridgeMirror.waitUntil(condition: () -> Boolean) = object : C
 } then this
 
 object Auto : RobotLifecycle {
-    private const val SCALE_TO_SWITCH_TURN = 130.0
+    private const val SCALE_TO_SWITCH_TURN = 125.0
 
     init {
         RobotLifecycle.addListener(this)

diff --git a/src/main/java/org/sert2521/powerup/autonomous/Paths.kt b/src/main/java/org/sert2521/powerup/autonomous/Paths.kt
@@ -178,7 +178,7 @@ object RightToRightSwitchPath : PathBase() {
     override var points = arrayOf(
             0.0 with -3.0 angle 0.0,
             1.3 with -3.2 angle 0.0,
-            3.7 with -1.7 angle -90.0
+            3.7 with -1.8 angle -90.0
     )
 }
 
@@ -250,7 +250,7 @@ object RightToRightScalePath : PathBase() {
     override var points = arrayOf(
             0.0 with -3.0 angle 0.0,
             4.2 with -3.0 angle 0.0,
-            6.4 with -2.6 angle 30.0
+            6.7 with -2.4 angle 30.0
     )
 }
 
@@ -288,7 +288,7 @@ object RightSwitchToRearPath : PathBase() {
     override var points = arrayOf(
             4.0 with 2.0 angle 90.0,
             3.0 with 2.6 angle 0.0,
-            2.3 with 1.6 angle -185.0
+            2.3 with 1.6 angle -188.0
     )
 }
 

diff --git a/src/main/java/org/sert2521/powerup/drivetrain/commands/DriveToCube.kt b/src/main/java/org/sert2521/powerup/drivetrain/commands/DriveToCube.kt
@@ -7,8 +7,9 @@ import org.sert2521.powerup.util.DEGREES_PER_PIXEL
 import org.sert2521.powerup.util.Vision
 import java.util.Date
 
-class DriveToCube : AngleDriver(0.008, 0.0, 0.022) {
+class DriveToCube : AngleDriver(0.008, 0.0, 0.002) {
     private lateinit var visionLastSeen: Date
+    private var hasBeenAtBottom = false
 
     init {
         requires(Drivetrain)
@@ -29,11 +30,15 @@ class DriveToCube : AngleDriver(0.008, 0.0, 0.022) {
             visionLastSeen = Date()
 
             Drivetrain.drive(BASE_SPEED + output, BASE_SPEED - output)
-            updateSetpoint(Vision.xOffset?.toDouble() ?: 0.0 * DEGREES_PER_PIXEL)
+            updateSetpoint((Vision.xOffset?.toDouble() ?: 0.0) * DEGREES_PER_PIXEL)
             return Intake.hasCube && Elevator.atBottom
         }
 
-        return visionLastSeen.time - Date().time > MAX_TIME_WITHOUT_CUBE
+        if (Elevator.atBottom) {
+            hasBeenAtBottom = true
+        }
+
+        return (visionLastSeen.time - Date().time > MAX_TIME_WITHOUT_CUBE) || Intake.hasCube && hasBeenAtBottom
     }
 
     private fun updateSetpoint(offset: Double) {

diff --git a/src/main/java/org/sert2521/powerup/util/Constants.kt b/src/main/java/org/sert2521/powerup/util/Constants.kt
@@ -38,8 +38,8 @@ const val BLUE_LED_PORT = 7
 const val ENCODER_TICKS_PER_REVOLUTION = 8192
 const val WHEEL_DIAMETER = 0.15
 const val WHEELBASE_WIDTH = 0.7
-const val MAX_VELOCITY = 1.0
-const val MAX_ACCELERATION = 0.1
+const val MAX_VELOCITY = 2.5
+const val MAX_ACCELERATION = 0.175
 const val MAX_JERK = 60.0
 
 // Other