Set sensitivity of the gyro. Removed auto shifting. Added CAMERA_ERRO…

…R to easily adjust any offset of the camera.

src/robot/Constants.java

@@ -20,6 +20,7 @@
     public static ParsableDouble AUTO_SHIFT_UP_THRESHOLD = new ParsableDouble("auto_shift_up_threshold", 0.9);
     public static ParsableDouble AUTO_SHIFT_DOWN_THRESHOLD = new ParsableDouble("auto_shift_down_threshold", 0.5);
     //Aim command
+    public static ParsableDouble CAMERA_ERROR = new ParsableDouble("camera_error", 2.0);
     public static ParsableDouble AUTO_AIM_TARGET_ANGLE = new ParsableDouble("auto_aim_target_angle", 0.0);
     public static ParsableDouble AUTO_AIM_ON_TARGET_TIME = new ParsableDouble("auto_aim_on_target_time", 1.0);
     //Chassis subsystem

src/robot/OI.java

@@ -21,7 +21,6 @@
         //Driver Buttons
         public static ParsableInt SHIFT_LOW_GEAR_BUTTON = new ParsableInt("driver_shift_low_gear_button", 7);
         public static ParsableInt SHIFT_HIGH_GEAR_BUTTON = new ParsableInt("driver_shift_high_gear_button", 8);
-        public static ParsableInt TOGGLE_AUTO_SHIFT_BUTTON = new ParsableInt("driver_toggle_auto_shift_button", 1);
         public static ParsableInt TOGGLE_ENABLE_CHASSIS_PID_BUTTON = new ParsableInt("driver_toggle_enable_chassis_pid_button", 2);
         public static ParsableInt PICKUP_LOWER_BUTTON = new ParsableInt("driver_pickup_lower_button", 6);
         public static ParsableInt PICKUP_LOWER_OVERRIDE_BUTTON = new ParsableInt("driver_pickup_lower_override_button", 5);
@@ -49,14 +48,9 @@
     public static final ParsableDouble SHOOTER_MINIMUM_SPEED = new ParsableDouble("shooter_minimum_speed", 0.75);
     Joystick stickDriver = new Joystick(Driver.PORT);
     Joystick stickOperator = new Joystick(Operator.PORT);
-    Toggle autoShift = new Toggle(false);
     Toggle enableChassisPID = new Toggle(true);
 
     //DRIVER
-    public boolean getAutoShift() {
-        return autoShift.update(stickDriver.getRawButton(Driver.TOGGLE_AUTO_SHIFT_BUTTON.get()));
-    }
-
     public boolean getEnableChassisPID() {
         return enableChassisPID.update(stickDriver.getRawButton(Driver.TOGGLE_ENABLE_CHASSIS_PID_BUTTON.get()));
     }

src/robot/commands/AimCommand.java

@@ -24,7 +24,9 @@ protected void initialize() {
     }
 
     protected void execute() {
-        double targetAngle = Constants.AUTO_AIM_TARGET_ANGLE.get();
+        //This will make the camera shift over to the right by CAMERA_ERROR, or it will make the camera run away
+        //from the target. It all depends on the sign of CAMERA_ERROR, must tweak
+        double targetAngle = Constants.AUTO_AIM_TARGET_ANGLE.get() - Constants.CAMERA_ERROR.get();
 
         //Only set the setpoint if it's changed from the last setpoint we saw
         if (lastTargetAngle != targetAngle) {

src/robot/commands/TeleopDriveCommand.java

@@ -20,28 +20,8 @@ protected void execute() {
         if (oi.getAutoAim() && AimCommand.lastTargetAngle != Constants.AUTO_AIM_TARGET_ANGLE.get()) {
             Scheduler.getInstance().add(new AimCommand());
         } else {
-            if (oi.getAutoShift()) {
-                //Do autoshift
-                double rate = chassisSubsystem.getAverageRate();
-
-                //Allow manual overrides for holding low gear and holding high gear
-                if (oi.getShiftLowGear()) {
-                    chassisSubsystem.shift(false);
-                } else if (oi.getShiftHighGear()) {
-                    chassisSubsystem.shift(true);
-                } else {
-                    //If lowGear && above threshold { shiftUp; }
-                    //If highGear && below threshold { shiftDown; }
-                    //Currently set to chassisSubsystem.getShiftState() = false when in low gear
-                    if (!chassisSubsystem.getHighGear() && Math.abs(rate) > Constants.AUTO_SHIFT_UP_THRESHOLD.get() * Constants.CHASSIS_MAX_LOW_ENCODER_RATE.get()) {
-                        chassisSubsystem.shift(true);
-                    } else if (chassisSubsystem.getHighGear() && Math.abs(rate) < Constants.AUTO_SHIFT_DOWN_THRESHOLD.get() * Constants.CHASSIS_MAX_LOW_ENCODER_RATE.get()) {
-                        chassisSubsystem.shift(false);
-                    }
-                }
-            } else {
             chassisSubsystem.shift(oi.getShiftHighGear());
-            }
+
 
             if (oi.getEnableChassisPID()) {
                 chassisSubsystem.enablePID();

src/robot/subsystems/PositioningSubsystem.java

@@ -21,6 +21,9 @@
     double lastDistance = 0;
 
     public PositioningSubsystem() {
+        //Found in http://pdf1.alldatasheet.com/datasheet-pdf/view/347604/AD/ADXRS652BBGZ-RL.html
+        //This is the same gyro that is found in 2012 and 2013 KoP
+        positionGyro.setSensitivity(0.007);
     }
 
     protected void initDefaultCommand() {