Improved targetting and logging

Author: sentientspud

config/commands.yml

@@ -1,6 +1,6 @@
 DriveTank:
-  deadzone: .05
-  leftMultiplier: 120.0
+  deadzone: .1
+  leftMultiplier: 130.0
   rightMultiplier: 120.0
   exponent: 2.0
 

config/subsystems.yml

@@ -10,7 +10,7 @@ Drive:
     F: .01
     minISetpoint: 1.0
     maxIError: 24.0
-    dataLogging: true
+    dataLogging: false
   driveMode: CONTROLLER
   maxSpeed: 120.0
 Picker:
@@ -44,6 +44,7 @@ Turret:
     F: 0.004
     minOutput: 0.0
     target: { within: 1.0 }
+    dataLogging: true
   turretRotationMotor: { channel: 4, inverted: true }
   turretRotationEncoder: { aChannel: 6, bChannel: 7, reverse: true, distancePerPulse: 0.02538 }
   angleController:

config/triggers.yml

@@ -3,7 +3,7 @@ Driver:
   BUMPER_RIGHT: start whenPressed PickerToPick
   TRIGGER_LEFT: toggle whenPressed TurretShootPrep
   TRIGGER_RIGHT: toggle whenPressed PickerPick
-  STICK_LEFT: start whenPressed $SequentialTest
+#  STICK_LEFT: start whenPressed $SequentialTest
   Y: start whenPressed TurretStopWheel
   B: start whenPressed TurretSpinUp
   X: toggle whenPressed TurretShoot

core/src/main/java/org/teamtators/rotator/commands/TurretShoot.java

@@ -7,6 +7,7 @@
 import org.teamtators.rotator.subsystems.AbstractTurret;
 import org.teamtators.rotator.subsystems.AbstractVision;
 import org.teamtators.rotator.subsystems.HoodPosition;
+import org.teamtators.rotator.subsystems.VisionData;
 
 /**
  * Fires a ball from the turret
@@ -57,18 +58,15 @@ protected boolean step() {
         }
         // check if roller isn't ready to start yet
         double wheelSpeed = turret.getWheelSpeed();
-        if (!turret.isAtTargetWheelSpeed()) {
-            logger.trace("Turret wheel speed not at target (speed: {}, target {}), not firing.",
-                    wheelSpeed, turret.getTargetWheelSpeed());
-        } else if (!turret.isAngleOnTarget()) {
-            logger.trace("Turret angle is not on target, not firing");
-        } else {
+        if (turret.isAtTargetWheelSpeed() && turret.isAngleOnTarget()) {
             // start rolling
             rollingStartTime = timestamp;
             double angle = turret.getAngle();
-            double angleOffset = vision.getVisionData().getOffsetAngle();
-            logger.info("Shooting at {} RPS, pointed at {} degrees, vision offset of {}", wheelSpeed, angle,
-                    angleOffset);
+            VisionData visionData = vision.getVisionData();
+            double angleOffset = visionData.getOffsetAngle();
+            double distance = visionData.getDistance();
+            logger.info("Shooting at {} RPS, pointed at {} degrees, vision offset of {}, distance {} in", wheelSpeed,
+                    angle, angleOffset, distance);
             turret.setKingRollerPower(config.kingRollerPower);
         }
         return false;

core/src/main/java/org/teamtators/rotator/control/InputDifferentiator.java

@@ -24,14 +24,14 @@ public void reset() {
     }
 
     @Override
-    public void step(double delta) {
+    public synchronized void step(double delta) {
         double input = inputProvider.getControllerInput();
         lastRate = Double.isNaN(lastInput) ? 0.0 : (input - lastInput) / delta;
         lastInput = input;
     }
 
     @Override
-    public double getControllerInput() {
+    public synchronized double getControllerInput() {
         return lastRate;
     }
 

core/src/main/java/org/teamtators/rotator/subsystems/AbstractTurret.java

@@ -44,7 +44,9 @@ protected void resetPower() {
         setWheelPower(0);
     }
 
-    public abstract double getWheelSpeed();
+    public double getWheelSpeed() {
+        return shooterWheelInputDifferentiator.getControllerInput();
+    }
 
     public abstract double getWheelRotations();