From 8d974b88ae89c30db69ca07e6adaac6774bb700d Mon Sep 17 00:00:00 2001 From: josehrl Date: Mon, 14 Jan 2019 15:43:06 -0800 Subject: [PATCH 01/30] Code for the chassis --- .../java/org/usfirst/frc4904/robot/Robot.java | 10 +- .../org/usfirst/frc4904/robot/RobotMap.java | 92 +++++++++++++++++-- .../humaninterface/HumanInterfaceConfig.java | 9 ++ .../humaninterface/drivers/NathanGain.java | 4 +- 4 files changed, 101 insertions(+), 14 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4904/robot/humaninterface/HumanInterfaceConfig.java diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index ff74254d..890c9ca2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -14,20 +14,22 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc4904.standard.CommandRobotBase; import org.usfirst.frc4904.standard.commands.chassis.ChassisMove; +import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; +import org.usfirst.frc4904.robot.humaninterface.operators.DefaultOperator; public class Robot extends CommandRobotBase { private RobotMap map = new RobotMap(); @Override public void initialize() { - // driverChooser.addDefault(object); - // operatorChooser.addDefault(); + driverChooser.addDefault(new NathanGain()); + operatorChooser.addDefault(new DefaultOperator()); } @Override public void teleopInitialize() { - // teleopCommand = new ChassisMove(RobotMap.Component.chassis, driverChooser.getSelected()); - // teleopCommand.start(); + teleopCommand = new ChassisMove(RobotMap.Component.chassis, driverChooser.getSelected()); + teleopCommand.start(); } @Override diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index eda501ca..832cc32f 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -1,32 +1,108 @@ package org.usfirst.frc4904.robot; +import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick; import org.usfirst.frc4904.standard.custom.controllers.CustomXbox; +import org.usfirst.frc4904.robot.humaninterface.HumanInterfaceConfig; +import org.usfirst.frc4904.standard.subsystems.chassis.TankDriveShifting; +import org.usfirst.frc4904.standard.subsystems.chassis.SolenoidShifters; +import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.EnableableModifier; +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.AccelerationCap; + +import org.usfirst.frc4904.standard.custom.sensors.PDP; +import org.usfirst.frc4904.standard.custom.PCMPort; +import org.usfirst.frc4904.standard.custom.sensors.CANEncoder; +import org.usfirst.frc4904.standard.custom.sensors.EncoderPair; +import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; +import org.usfirst.frc4904.standard.custom.sensors.NavX; + +import edu.wpi.first.wpilibj.VictorSP; public class RobotMap { public static class Port { public static class HumanInput { + public static final int joystick = 0; public static final int xboxController = 1; } public static class CANMotor {} - public static class PWM {} - public static class CAN {} - public static class Pneumatics {} + public static class PWM { + public static final int leftDriveA = 2; // Not real ports, change these + public static final int leftDriveB = 3; + public static final int rightDriveA = 0; + public static final int rightDriveB = 1; + } + public static class CAN { + public static final int leftEncoder = -1; // Not real ports, change these + public static final int rightEncoder = -1; + } + public static class Pneumatics { + public static final PCMPort shifter = new PCMPort(1, 0, 1); + } } - public static class Metrics{} + public static class Metrics{ + public static class Wheel { + public static final double TICKS_PER_REVOLUTION = 4.904; // Obviously not real metrics + public static final double DIAMETER_INCHES = 4.904; + public static final double CIRCUMFERENCE_INCHES = Metrics.Wheel.DIAMETER_INCHES * Math.PI; + public static final double TICKS_PER_INCH = Metrics.Wheel.TICKS_PER_REVOLUTION + / Metrics.Wheel.CIRCUMFERENCE_INCHES; + public static final double DISTANCE_FRONT_BACK = 4.904; + public static final double DISTANCE_SIDE_SIDE = 4.904; + public static final double INCHES_PER_TICK = Metrics.Wheel.CIRCUMFERENCE_INCHES + / Metrics.Wheel.TICKS_PER_REVOLUTION; + } + } public static class Component { - public static CustomXbox driverXbox; + public static PDP pdp; + + public static TankDriveShifting chassis; + public static Motor leftWheel; + public static Motor rightWheel; + public static SolenoidShifters shifter; + public static EnableableModifier rightWheelAccelerationCap; + public static EnableableModifier leftWheelAccelerationCap; + + public static CANEncoder leftWheelEncoder; + public static CANEncoder rightWheelEncoder; + public static EncoderPair chassisEncoders; + public static CustomPIDController chassisTurnMC; + public static NavX navx; } public static class HumanInput { public static class Driver { public static CustomXbox xbox; } - public static class Operator {} + public static class Operator { + public static CustomJoystick joystick; + } } public RobotMap() { - Component.driverXbox = new CustomXbox(Port.HumanInput.xboxController); - Component.driverXbox.setDeadZone(0.1); + Component.pdp = new PDP(); + + // Wheels + Component.leftWheelEncoder = new CANEncoder("LeftEncoder", Port.CAN.leftEncoder); + Component.rightWheelEncoder = new CANEncoder("RightEncoder", Port.CAN.rightEncoder); + Component.leftWheelEncoder.setDistancePerPulse(Metrics.Wheel.INCHES_PER_TICK); + Component.rightWheelEncoder.setDistancePerPulse(Metrics.Wheel.INCHES_PER_TICK); + Component.leftWheelAccelerationCap = new EnableableModifier(new AccelerationCap(Component.pdp)); + Component.leftWheelAccelerationCap.enable(); + Component.rightWheelAccelerationCap = new EnableableModifier(new AccelerationCap(Component.pdp)); + Component.rightWheelAccelerationCap.enable(); + Component.leftWheel = new Motor("LeftWheel", Component.leftWheelAccelerationCap, + new VictorSP(Port.PWM.leftDriveA), new VictorSP(Port.PWM.leftDriveB)); + Component.rightWheel = new Motor("RightWheel", Component.rightWheelAccelerationCap, + new VictorSP(Port.PWM.rightDriveA), new VictorSP(Port.PWM.rightDriveB)); + // Chassis + Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, + Port.Pneumatics.shifter.reverse); + Component.chassisEncoders = new EncoderPair(Component.leftWheelEncoder, Component.rightWheelEncoder); + Component.chassis = new TankDriveShifting("2018-Chassis", Component.leftWheel, Component.rightWheel, Component.shifter); + HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController); + HumanInput.Driver.xbox.setDeadZone(HumanInterfaceConfig.XBOX_DEADZONE); + HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick); + HumanInput.Operator.joystick.setDeadzone(HumanInterfaceConfig.STICK_LEFT_DEADZONE); } } diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/HumanInterfaceConfig.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/HumanInterfaceConfig.java new file mode 100644 index 00000000..76f4eb9b --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/HumanInterfaceConfig.java @@ -0,0 +1,9 @@ +package org.usfirst.frc4904.robot.humaninterface; + +public class HumanInterfaceConfig { + public static final double XBOX_DEADZONE = 0.1; + public static final double STICK_LEFT_DEADZONE = 0.1; + public static final double STICK_RIGHT_DEADZONE = 0.1; + + private HumanInterfaceConfig(){} +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index 7218ba1a..a84fe49a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -36,7 +36,7 @@ public double getX() { @Override public double getY() { - double rawSpeed = RobotMap.Component.driverXbox.rt.getX() - RobotMap.Component.driverXbox.lt.getX(); + double rawSpeed = RobotMap.HumanInput.Driver.xbox.rt.getX() - RobotMap.HumanInput.Driver.xbox.lt.getX(); double speed = scaleGain(rawSpeed, NathanGain.SPEED_GAIN, NathanGain.SPEED_EXP) * NathanGain.Y_SPEED_SCALE; return speed; @@ -44,7 +44,7 @@ public double getY() { @Override public double getTurnSpeed() { - double rawTurnSpeed = RobotMap.Component.driverXbox.leftStick.getX(); + double rawTurnSpeed = RobotMap.HumanInput.Driver.xbox.leftStick.getX(); double turnSpeed = scaleGain(rawTurnSpeed, NathanGain.TURN_GAIN, NathanGain.TURN_EXP) * NathanGain.TURN_SPEED_SCALE; return turnSpeed; From d75d6a703fc9f9deec9ade4569c67c223fd35771 Mon Sep 17 00:00:00 2001 From: josehrl Date: Mon, 14 Jan 2019 15:53:12 -0800 Subject: [PATCH 02/30] uncommented a thing --- .../frc4904/robot/humaninterface/drivers/NathanGain.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index a84fe49a..dbf86acf 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -24,10 +24,10 @@ protected double scaleGain(double input, double gain, double exp) { @Override public void bindCommands() { - // RobotMap.Component.driverXbox.lb - // .whenPressed(new ChassisShift(RobotMap.Component.chassis.getShifter(), SolenoidShifters.ShiftState.DOWN)); - // RobotMap.Component.driverXbox.rb - // .whenPressed(new ChassisShift(RobotMap.Component.chassis.getShifter(), SolenoidShifters.ShiftState.UP)); + RobotMap.Component.driverXbox.lb + .whenPressed(new ChassisShift(RobotMap.Component.chassis.getShifter(), SolenoidShifters.ShiftState.DOWN)); + RobotMap.Component.driverXbox.rb + .whenPressed(new ChassisShift(RobotMap.Component.chassis.getShifter(), SolenoidShifters.ShiftState.UP)); } @Override public double getX() { From 3973dd3518012812210580a68306219d0d50c6ce Mon Sep 17 00:00:00 2001 From: josehrl Date: Mon, 14 Jan 2019 15:55:14 -0800 Subject: [PATCH 03/30] fixed a thing --- .../frc4904/robot/humaninterface/drivers/NathanGain.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java index dbf86acf..e31ae0cf 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/drivers/NathanGain.java @@ -24,9 +24,9 @@ protected double scaleGain(double input, double gain, double exp) { @Override public void bindCommands() { - RobotMap.Component.driverXbox.lb + RobotMap.HumanInput.Driver.xbox.lb .whenPressed(new ChassisShift(RobotMap.Component.chassis.getShifter(), SolenoidShifters.ShiftState.DOWN)); - RobotMap.Component.driverXbox.rb + RobotMap.HumanInput.Driver.xbox.rb .whenPressed(new ChassisShift(RobotMap.Component.chassis.getShifter(), SolenoidShifters.ShiftState.UP)); } @Override From 1d20676493f327a9c316cbc48e7eee0ef225b21c Mon Sep 17 00:00:00 2001 From: akshar Date: Thu, 17 Jan 2019 17:06:53 -0800 Subject: [PATCH 04/30] Added driving PID --- .../org/usfirst/frc4904/robot/RobotMap.java | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 832cc32f..ad4b2dfe 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -55,6 +55,16 @@ public static class Wheel { / Metrics.Wheel.TICKS_PER_REVOLUTION; } } + public static class PID { + public static class Drive { + public static final double P = 0.04; //Tune PID + public static final double I = 0.0; //Tune PID + public static final double D = -0.006; //Tune PID + public static final double F = 0.01; //Tune PID + public static final double tolerance = 4.5; //Tune PID + public static final double dTolerance = 3.0; //Tune PID + } + } public static class Component { public static PDP pdp; @@ -68,7 +78,8 @@ public static class Component { public static CANEncoder leftWheelEncoder; public static CANEncoder rightWheelEncoder; public static EncoderPair chassisEncoders; - public static CustomPIDController chassisTurnMC; + public static CustomPIDController chassisTurnMC; + public static CustomPIDController drivePID; public static NavX navx; } public static class HumanInput { @@ -99,10 +110,13 @@ public RobotMap() { Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, Port.Pneumatics.shifter.reverse); Component.chassisEncoders = new EncoderPair(Component.leftWheelEncoder, Component.rightWheelEncoder); - Component.chassis = new TankDriveShifting("2018-Chassis", Component.leftWheel, Component.rightWheel, Component.shifter); + Component.chassis = new TankDriveShifting("2019-Chassis", Component.leftWheel, Component.rightWheel, Component.shifter); HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController); HumanInput.Driver.xbox.setDeadZone(HumanInterfaceConfig.XBOX_DEADZONE); HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick); HumanInput.Operator.joystick.setDeadzone(HumanInterfaceConfig.STICK_LEFT_DEADZONE); + Component.drivePID = new CustomPIDController(PID.Drive.P, PID.Drive.I, PID.Drive.D, PID.Drive.F, Component.rightWheelEncoder); + Component.drivePID.setAbsoluteTolerance(PID.Drive.tolerance); + Component.drivePID.setDerivativeTolerance(PID.Drive.dTolerance); } } From 5ea0eded09564deea7d80a0854767155da15a36f Mon Sep 17 00:00:00 2001 From: akshar Date: Sun, 20 Jan 2019 10:10:17 -0800 Subject: [PATCH 05/30] Made PR changes --- .../java/org/usfirst/frc4904/robot/RobotMap.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index ad4b2dfe..f7bf9a0d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -28,17 +28,17 @@ public static class HumanInput { } public static class CANMotor {} public static class PWM { - public static final int leftDriveA = 2; // Not real ports, change these - public static final int leftDriveB = 3; - public static final int rightDriveA = 0; - public static final int rightDriveB = 1; + public static final int leftDriveA = -1; // TODO: Change ports + public static final int leftDriveB = -1; + public static final int rightDriveA = -1; + public static final int rightDriveB = -1; } public static class CAN { - public static final int leftEncoder = -1; // Not real ports, change these + public static final int leftEncoder = -1; // TODO: Change ports public static final int rightEncoder = -1; } public static class Pneumatics { - public static final PCMPort shifter = new PCMPort(1, 0, 1); + public static final PCMPort shifter = new PCMPort(-1, -1, -1); // TODO: Change ports } } From 81e0953ac3c5de78be3bd130110fa88f47eadf37 Mon Sep 17 00:00:00 2001 From: akshar Date: Sun, 20 Jan 2019 10:22:52 -0800 Subject: [PATCH 06/30] Fixed PR Changes --- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index f7bf9a0d..ec9a5eec 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -13,6 +13,7 @@ import org.usfirst.frc4904.standard.custom.PCMPort; import org.usfirst.frc4904.standard.custom.sensors.CANEncoder; import org.usfirst.frc4904.standard.custom.sensors.EncoderPair; +import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX; import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; import org.usfirst.frc4904.standard.custom.sensors.NavX; @@ -103,9 +104,9 @@ public RobotMap() { Component.rightWheelAccelerationCap = new EnableableModifier(new AccelerationCap(Component.pdp)); Component.rightWheelAccelerationCap.enable(); Component.leftWheel = new Motor("LeftWheel", Component.leftWheelAccelerationCap, - new VictorSP(Port.PWM.leftDriveA), new VictorSP(Port.PWM.leftDriveB)); + new CANTalonSRX(Port.PWM.leftDriveA), new CANTalonSRX(Port.PWM.leftDriveB)); Component.rightWheel = new Motor("RightWheel", Component.rightWheelAccelerationCap, - new VictorSP(Port.PWM.rightDriveA), new VictorSP(Port.PWM.rightDriveB)); + new CANTalonSRX(Port.PWM.rightDriveA), new CANTalonSRX(Port.PWM.rightDriveB)); // Chassis Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, Port.Pneumatics.shifter.reverse); From e4c2e6eac3c54d1a6fee9adfd02d777c76fbf331 Mon Sep 17 00:00:00 2001 From: akshar Date: Sun, 20 Jan 2019 14:21:43 -0800 Subject: [PATCH 07/30] Fixed PR Comment --- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 2 +- .../frc4904/robot/humaninterface/HumanInterfaceConfig.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index ec9a5eec..43e5b02c 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -115,7 +115,7 @@ public RobotMap() { HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController); HumanInput.Driver.xbox.setDeadZone(HumanInterfaceConfig.XBOX_DEADZONE); HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick); - HumanInput.Operator.joystick.setDeadzone(HumanInterfaceConfig.STICK_LEFT_DEADZONE); + HumanInput.Operator.joystick.setDeadzone(HumanInterfaceConfig.JOYSTICK_DEADZONE); Component.drivePID = new CustomPIDController(PID.Drive.P, PID.Drive.I, PID.Drive.D, PID.Drive.F, Component.rightWheelEncoder); Component.drivePID.setAbsoluteTolerance(PID.Drive.tolerance); Component.drivePID.setDerivativeTolerance(PID.Drive.dTolerance); diff --git a/src/main/java/org/usfirst/frc4904/robot/humaninterface/HumanInterfaceConfig.java b/src/main/java/org/usfirst/frc4904/robot/humaninterface/HumanInterfaceConfig.java index 76f4eb9b..10a7a848 100644 --- a/src/main/java/org/usfirst/frc4904/robot/humaninterface/HumanInterfaceConfig.java +++ b/src/main/java/org/usfirst/frc4904/robot/humaninterface/HumanInterfaceConfig.java @@ -2,8 +2,7 @@ public class HumanInterfaceConfig { public static final double XBOX_DEADZONE = 0.1; - public static final double STICK_LEFT_DEADZONE = 0.1; - public static final double STICK_RIGHT_DEADZONE = 0.1; + public static final double JOYSTICK_DEADZONE = 0.1; private HumanInterfaceConfig(){} } \ No newline at end of file From e9a104f2e4cefcbdc3c40f9809be8c01ca63e4ae Mon Sep 17 00:00:00 2001 From: akshar Date: Sun, 20 Jan 2019 14:23:05 -0800 Subject: [PATCH 08/30] Fixed PR Comment --- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 43e5b02c..c830f684 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -17,7 +17,6 @@ import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; import org.usfirst.frc4904.standard.custom.sensors.NavX; -import edu.wpi.first.wpilibj.VictorSP; public class RobotMap { From 28665f6b506abb5cd3069b25fb80cdd27e16abae Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Mon, 28 Jan 2019 16:06:31 -0800 Subject: [PATCH 09/30] =?UTF-8?q?made=20the=20class=20NotSplines,=20which?= =?UTF-8?q?=20is=20a=20base=20for=20having=20the=20robot=20drive=20to=20th?= =?UTF-8?q?e=20hatch=20given=20a=20distance=20and=20angle=20=E2=80=93=20wi?= =?UTF-8?q?ll=20need=20another=20angle=20though,=20or=20absolute=20robot?= =?UTF-8?q?=20position.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../org/usfirst/frc4904/robot/RobotMap.java | 26 +++++++++---------- .../frc4904/robot/commands/NotSplines.java | 19 ++++++++++++++ src/main/java/org/usfirst/frc4904/standard | 2 +- 3 files changed, 33 insertions(+), 14 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index c830f684..ff9764ea 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -11,8 +11,8 @@ import org.usfirst.frc4904.standard.custom.sensors.PDP; import org.usfirst.frc4904.standard.custom.PCMPort; -import org.usfirst.frc4904.standard.custom.sensors.CANEncoder; -import org.usfirst.frc4904.standard.custom.sensors.EncoderPair; +//import org.usfirst.frc4904.standard.custom.sensors.CANEncoder; +//import org.usfirst.frc4904.standard.custom.sensors.EncoderPair; import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX; import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; import org.usfirst.frc4904.standard.custom.sensors.NavX; @@ -34,8 +34,8 @@ public static class PWM { public static final int rightDriveB = -1; } public static class CAN { - public static final int leftEncoder = -1; // TODO: Change ports - public static final int rightEncoder = -1; + //public static final int leftEncoder = -1; // TODO: Change ports + //public static final int rightEncoder = -1; } public static class Pneumatics { public static final PCMPort shifter = new PCMPort(-1, -1, -1); // TODO: Change ports @@ -75,9 +75,9 @@ public static class Component { public static EnableableModifier rightWheelAccelerationCap; public static EnableableModifier leftWheelAccelerationCap; - public static CANEncoder leftWheelEncoder; - public static CANEncoder rightWheelEncoder; - public static EncoderPair chassisEncoders; + //public static CANEncoder leftWheelEncoder; + //public static CANEncoder rightWheelEncoder; + //public static EncoderPair chassisEncoders; public static CustomPIDController chassisTurnMC; public static CustomPIDController drivePID; public static NavX navx; @@ -94,10 +94,10 @@ public RobotMap() { Component.pdp = new PDP(); // Wheels - Component.leftWheelEncoder = new CANEncoder("LeftEncoder", Port.CAN.leftEncoder); - Component.rightWheelEncoder = new CANEncoder("RightEncoder", Port.CAN.rightEncoder); - Component.leftWheelEncoder.setDistancePerPulse(Metrics.Wheel.INCHES_PER_TICK); - Component.rightWheelEncoder.setDistancePerPulse(Metrics.Wheel.INCHES_PER_TICK); + // Component.leftWheelEncoder = new CANEncoder("LeftEncoder", Port.CAN.leftEncoder); + // Component.rightWheelEncoder = new CANEncoder("RightEncoder", Port.CAN.rightEncoder); + // Component.leftWheelEncoder.setDistancePerPulse(Metrics.Wheel.INCHES_PER_TICK); + // Component.rightWheelEncoder.setDistancePerPulse(Metrics.Wheel.INCHES_PER_TICK); Component.leftWheelAccelerationCap = new EnableableModifier(new AccelerationCap(Component.pdp)); Component.leftWheelAccelerationCap.enable(); Component.rightWheelAccelerationCap = new EnableableModifier(new AccelerationCap(Component.pdp)); @@ -109,13 +109,13 @@ public RobotMap() { // Chassis Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, Port.Pneumatics.shifter.reverse); - Component.chassisEncoders = new EncoderPair(Component.leftWheelEncoder, Component.rightWheelEncoder); + //Component.chassisEncoders = new EncoderPair(Component.leftWheelEncoder, Component.rightWheelEncoder); Component.chassis = new TankDriveShifting("2019-Chassis", Component.leftWheel, Component.rightWheel, Component.shifter); HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController); HumanInput.Driver.xbox.setDeadZone(HumanInterfaceConfig.XBOX_DEADZONE); HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick); HumanInput.Operator.joystick.setDeadzone(HumanInterfaceConfig.JOYSTICK_DEADZONE); - Component.drivePID = new CustomPIDController(PID.Drive.P, PID.Drive.I, PID.Drive.D, PID.Drive.F, Component.rightWheelEncoder); + //Component.drivePID = new CustomPIDController(PID.Drive.P, PID.Drive.I, PID.Drive.D, PID.Drive.F, Component.rightWheelEncoder); Component.drivePID.setAbsoluteTolerance(PID.Drive.tolerance); Component.drivePID.setDerivativeTolerance(PID.Drive.dTolerance); } diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java new file mode 100644 index 00000000..63d912f5 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java @@ -0,0 +1,19 @@ +package org.usfirst.frc4904.robot.commands; + +import java.lang.Math; +import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; +import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class NotSplines extends CommandGroup { + public static class Values{ + public static final double distance = 1.0; + public static final double angle = 45.0; + public static final double distance_x = distance*Math.sin(angle); + public static final double distance_y = distance*Math.cos(angle); + } + public NotSplines() { + super("NotSplines"); + addParallel(new ChassisTurn(RobotMap.Component.chassis, Values.angle, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index c70cd8a7..4e2dce2b 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit c70cd8a7ee0a785641f33c5d6456de5774560321 +Subproject commit 4e2dce2baba2b11a9c7731b4b440081e4267c229 From 7b6b6cee298b2c43641c464c9d71c85476369c46 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 3 Feb 2019 11:58:43 -0800 Subject: [PATCH 10/30] i think i finished, the robot should turn the specified angle and then move the specified distance. --- .../java/org/usfirst/frc4904/robot/commands/NotSplines.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java index 63d912f5..d28333e5 100644 --- a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java +++ b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java @@ -2,6 +2,7 @@ import java.lang.Math; import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; +import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; import org.usfirst.frc4904.robot.RobotMap; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -14,6 +15,7 @@ public static class Values{ } public NotSplines() { super("NotSplines"); - addParallel(new ChassisTurn(RobotMap.Component.chassis, Values.angle, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, Values.angle, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addParallel(new ChassisMoveDistance(RobotMap.Component.chassis, Values.distance, RobotMap.Component.drivePID)); } } \ No newline at end of file From f430ddcc3bb47650b42851b6ab45b51bf6ee8c2b Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 3 Feb 2019 13:38:57 -0800 Subject: [PATCH 11/30] networktables stuff --- .../org/usfirst/frc4904/robot/RobotMap.java | 30 +++++++++++++++++-- .../frc4904/robot/commands/NotSplines.java | 7 +++-- 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index ff9764ea..7085d3a0 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -8,6 +8,9 @@ import org.usfirst.frc4904.standard.subsystems.motor.Motor; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.EnableableModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.AccelerationCap; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; import org.usfirst.frc4904.standard.custom.sensors.PDP; import org.usfirst.frc4904.standard.custom.PCMPort; @@ -81,7 +84,24 @@ public static class Component { public static CustomPIDController chassisTurnMC; public static CustomPIDController drivePID; public static NavX navx; - } + } + + public static class NetworkTables { + public static NetworkTableInstance inst; + public static NetworkTable table; + + public static class Sensors { + public static NetworkTable table; + } + + public static class PID { + public static NetworkTable table; + public static NetworkTableEntry distance; + public static NetworkTableEntry angle; + + } + } + public static class HumanInput { public static class Driver { public static CustomXbox xbox; @@ -105,7 +125,13 @@ public RobotMap() { Component.leftWheel = new Motor("LeftWheel", Component.leftWheelAccelerationCap, new CANTalonSRX(Port.PWM.leftDriveA), new CANTalonSRX(Port.PWM.leftDriveB)); Component.rightWheel = new Motor("RightWheel", Component.rightWheelAccelerationCap, - new CANTalonSRX(Port.PWM.rightDriveA), new CANTalonSRX(Port.PWM.rightDriveB)); + new CANTalonSRX(Port.PWM.rightDriveA), new CANTalonSRX(Port.PWM.rightDriveB)); + + NetworkTables.inst = NetworkTableInstance.getDefault(); + NetworkTables.PID.table = NetworkTables.inst.getTable("PID"); + NetworkTables.PID.distance = NetworkTables.PID.table.getEntry("Distance"); + NetworkTables.PID.angle = NetworkTables.PID.table.getEntry("Angle"); + // Chassis Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, Port.Pneumatics.shifter.reverse); diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java index d28333e5..bf92b836 100644 --- a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java +++ b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java @@ -5,11 +5,14 @@ import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; import org.usfirst.frc4904.robot.RobotMap; import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.networktables.NetworkTable; public class NotSplines extends CommandGroup { public static class Values{ - public static final double distance = 1.0; - public static final double angle = 45.0; + public static final double distance = 1.0; + // public static final double distance = RobotMap.NetworkTables.PID.distance; + public static final double angle = 45.0; + // public static final double angle = RobotMap.NetworkTables.PID.angle; public static final double distance_x = distance*Math.sin(angle); public static final double distance_y = distance*Math.cos(angle); } From 6c0811209e93ac1b41076b1b59e7d86dd0204b0c Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Wed, 13 Feb 2019 17:24:57 -0800 Subject: [PATCH 12/30] added an autonly folder and some new code stuff in it --- .../frc4904/robot/autonly/BoringAuton.java | 25 +++++++++++++ .../robot/autonly/InterestingAuton.java | 35 +++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java create mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java new file mode 100644 index 00000000..d98d2eb3 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java @@ -0,0 +1,25 @@ +package org.usfirst.frc4904.robot.autonly; + +import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; +import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; +import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class BoringAuton extends CommandGroup { + + public BoringAuton(double beta, double x, double y) { + super("BoringAuton"); + if (x >= 0) { + addSequential(new ChassisTurn(RobotMap.Component.chassis, 90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, x, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + } + else { + addSequential(new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java new file mode 100644 index 00000000..f1e56b0f --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java @@ -0,0 +1,35 @@ +package org.usfirst.frc4904.robot.autonly; + +import java.lang.Math; +import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; +import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; +import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class InterestingAuton extends CommandGroup { + + public InterestingAuton(double beta, double x, double y) { + super("InterestingAuton"); + double firstDrive = x/Math.sin(beta); + double newY = y - x/Math.tan(beta); + if (x >= 0) { + if (newY <= 0 || newY >= y) { + addSequential(new ChassisTurn(RobotMap.Component.chassis, 90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, x, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + } + else { + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, -beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, newY, RobotMap.Component.drivePID)); + } + } + else { + addSequential(new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + } + } +} \ No newline at end of file From 3cfb65d2612ff10e19af771f555f8cf276d0074e Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Wed, 13 Feb 2019 21:50:38 -0800 Subject: [PATCH 13/30] fixed up/added to the math --- .../robot/autonly/InterestingAuton.java | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java index f1e56b0f..f7df5f51 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java @@ -10,9 +10,9 @@ public class InterestingAuton extends CommandGroup { public InterestingAuton(double beta, double x, double y) { super("InterestingAuton"); - double firstDrive = x/Math.sin(beta); - double newY = y - x/Math.tan(beta); if (x >= 0) { + double firstDrive = x/Math.sin(-beta); + double newY = y - x/Math.tan(-beta); if (newY <= 0 || newY >= y) { addSequential(new ChassisTurn(RobotMap.Component.chassis, 90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, x, RobotMap.Component.drivePID)); @@ -26,10 +26,19 @@ public InterestingAuton(double beta, double x, double y) { } } else { - addSequential(new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + double firstDrive = x/Math.sin(beta); + double newY = y - x/Math.tan(beta); + if (newY <= 0 || newY >= y) { + addSequential(new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + } + else { + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, newY, RobotMap.Component.drivePID)); + } } } } \ No newline at end of file From bfe275c05d853a44d1e030779f4718116de31a6c Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Thu, 14 Feb 2019 19:06:52 -0800 Subject: [PATCH 14/30] networktables stuff including a new class that uses them. --- .../java/org/usfirst/frc4904/robot/Robot.java | 3 +- .../org/usfirst/frc4904/robot/RobotMap.java | 12 ++-- .../robot/autonly/NetworkTablesAuton.java | 56 +++++++++++++++++++ 3 files changed, 65 insertions(+), 6 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index 0e723285..c9a25e35 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -63,5 +63,6 @@ public void testInitialize() {} public void testExecute() {} @Override - public void alwaysExecute() {} + public void alwaysExecute() { + } } diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 7085d3a0..51c66a76 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -92,12 +92,13 @@ public static class NetworkTables { public static class Sensors { public static NetworkTable table; + public static NetworkTableEntry beta; + public static NetworkTableEntry x; + public static NetworkTableEntry y; } public static class PID { public static NetworkTable table; - public static NetworkTableEntry distance; - public static NetworkTableEntry angle; } } @@ -128,9 +129,10 @@ public RobotMap() { new CANTalonSRX(Port.PWM.rightDriveA), new CANTalonSRX(Port.PWM.rightDriveB)); NetworkTables.inst = NetworkTableInstance.getDefault(); - NetworkTables.PID.table = NetworkTables.inst.getTable("PID"); - NetworkTables.PID.distance = NetworkTables.PID.table.getEntry("Distance"); - NetworkTables.PID.angle = NetworkTables.PID.table.getEntry("Angle"); + NetworkTables.Sensors.table = NetworkTables.inst.getTable("Sensors"); + NetworkTables.Sensors.beta = NetworkTables.Sensors.table.getEntry("beta"); + NetworkTables.Sensors.x = NetworkTables.Sensors.table.getEntry("x"); + NetworkTables.Sensors.y = NetworkTables.Sensors.table.getEntry("y"); // Chassis Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java new file mode 100644 index 00000000..f8984b2d --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java @@ -0,0 +1,56 @@ +package org.usfirst.frc4904.robot.autonly; + +import java.lang.Math; +import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; +import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; +import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.networktables.NetworkTable; +import org.usfirst.frc4904.standard.commands.RunIfElse; +import org.usfirst.frc4904.standard.commands.RunIf; + +public class NetworkTablesAuton extends CommandGroup { + + public NetworkTablesAuton() { + super("NetworkTablesAuton"); + double beta = RobotMap.NetworkTables.Sensors.beta.getDouble(-1.0); + double x = RobotMap.NetworkTables.Sensors.x.getDouble(-1.0); + double y = RobotMap.NetworkTables.Sensors.y.getDouble(-1.0); + double firstTurn = Math.atan2(y/2, x); + double newY = y - x/Math.tan(beta); + double firstDrive = x/Math.sin(90-beta); + addSequential(new RunIf( + new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), + () -> {return newY <= 0;} //TODO: change 0 to robotwidth/2 + )); + beta = RobotMap.NetworkTables.Sensors.beta.getDouble(-1.0); + x = RobotMap.NetworkTables.Sensors.x.getDouble(-1.0); + y = RobotMap.NetworkTables.Sensors.y.getDouble(-1.0); + if (x >= 0) { + if (newY <= 0 || newY >= y) { + addSequential(new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + } + else { + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, -beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, newY, RobotMap.Component.drivePID)); + } + } + else { + if (newY <= 0 || newY >= y) { + addSequential(new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + } + else { + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, newY, RobotMap.Component.drivePID)); + } + } + } +} \ No newline at end of file From ffc4405780bc42690c58dd3e7a5c3cedca46ed88 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Fri, 15 Feb 2019 19:01:09 -0800 Subject: [PATCH 15/30] massive changes mostly to networktablesauton, boringauton also updated (still need to do interestingauton). --- .../org/usfirst/frc4904/robot/RobotMap.java | 3 + .../frc4904/robot/autonly/BoringAuton.java | 54 ++++++--- .../robot/autonly/NetworkTablesAuton.java | 104 +++++++++++++----- src/main/java/org/usfirst/frc4904/standard | 2 +- 4 files changed, 119 insertions(+), 44 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 51c66a76..706f5d08 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -57,6 +57,9 @@ public static class Wheel { public static final double INCHES_PER_TICK = Metrics.Wheel.CIRCUMFERENCE_INCHES / Metrics.Wheel.TICKS_PER_REVOLUTION; } + public static class Robot { + public static final double ROBOT_WIDTH = 4.904; + } } public static class PID { public static class Drive { diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java index d98d2eb3..515d185e 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java @@ -2,24 +2,50 @@ import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; +import java.util.function.DoubleSupplier; import org.usfirst.frc4904.robot.RobotMap; import edu.wpi.first.wpilibj.command.CommandGroup; +import org.usfirst.frc4904.standard.commands.RunIf; public class BoringAuton extends CommandGroup { - public BoringAuton(double beta, double x, double y) { + public BoringAuton(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { super("BoringAuton"); - if (x >= 0) { - addSequential(new ChassisTurn(RobotMap.Component.chassis, 90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, x, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + double beta = betaSupplier.getAsDouble(); + double x = xSupplier.getAsDouble(); + double y = ySupplier.getAsDouble(); + + addSequential(new RunIf( + new ChassisTurn(RobotMap.Component.chassis, 90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), + () -> {return x >= 0;} + )); + addSequential(new RunIf( + new ChassisMoveDistance(RobotMap.Component.chassis, x, RobotMap.Component.drivePID), + () -> {return x >= 0;} + )); + addSequential(new RunIf( + new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), + () -> {return x >= 0;} + )); + addSequential(new RunIf( + new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID), + () -> {return x >= 0;} + )); + addSequential(new RunIf( + new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), + () -> {return x < 0;} + )); + addSequential(new RunIf( + new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID), + () -> {return x < 0;} + )); + addSequential(new RunIf( + new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), + () -> {return x < 0;} + )); + addSequential(new RunIf( + new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID), + () -> {return x < 0;} + )); } - else { - addSequential(new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); - } - } -} \ No newline at end of file + } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java index f8984b2d..0405e4be 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java @@ -1,56 +1,102 @@ package org.usfirst.frc4904.robot.autonly; + import java.lang.Math; import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.networktables.NetworkTable; import org.usfirst.frc4904.standard.commands.RunIfElse; import org.usfirst.frc4904.standard.commands.RunIf; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + public class NetworkTablesAuton extends CommandGroup { + public static double fallbackAngle = -1; public NetworkTablesAuton() { super("NetworkTablesAuton"); - double beta = RobotMap.NetworkTables.Sensors.beta.getDouble(-1.0); - double x = RobotMap.NetworkTables.Sensors.x.getDouble(-1.0); - double y = RobotMap.NetworkTables.Sensors.y.getDouble(-1.0); - double firstTurn = Math.atan2(y/2, x); - double newY = y - x/Math.tan(beta); - double firstDrive = x/Math.sin(90-beta); + double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; + Data getData = new Data(); + addParallel(getData); addSequential(new RunIf( - new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), - () -> {return newY <= 0;} //TODO: change 0 to robotwidth/2 + new ChassisTurn(RobotMap.Component.chassis, getData.firstTurnIfBad(), RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC), + () -> {return getData.getNewY().getAsDouble() >= driveTolerance;} + )); + addSequential(new RunIfElse( + new ChassisMoveDistance(RobotMap.Component.chassis, getData.firstDriveIfBad(), RobotMap.Component.drivePID), + new ChassisMoveDistance(RobotMap.Component.chassis, getData.firstDriveIfGood(), RobotMap.Component.drivePID), + () -> {return getData.getNewY().getAsDouble() >= driveTolerance;} )); - beta = RobotMap.NetworkTables.Sensors.beta.getDouble(-1.0); - x = RobotMap.NetworkTables.Sensors.x.getDouble(-1.0); - y = RobotMap.NetworkTables.Sensors.y.getDouble(-1.0); - if (x >= 0) { - if (newY <= 0 || newY >= y) { - addSequential(new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, getData.getBeta(), RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, getData.getY(), RobotMap.Component.drivePID)); + } + public class Data extends Command { + private double beta; + private double x; + private double y; + private double newY; + private double firstDriveIfBad; + private double firstDriveIfGood; + + public void execute() { + beta = RobotMap.NetworkTables.Sensors.beta.getDouble(fallbackAngle); + x = RobotMap.NetworkTables.Sensors.x.getDouble(-1.0); + y = RobotMap.NetworkTables.Sensors.y.getDouble(-1.0); + } + + private DoubleSupplier getBeta() { + return () -> beta; + } + + private DoubleSupplier getX() { + return () -> x; + } + + private DoubleSupplier getY() { + return () -> y; + } + + private DoubleSupplier getNewY() { + if (x >= 0) { + newY = y - x * Math.tan(beta); } else { - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, -beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, newY, RobotMap.Component.drivePID)); + newY = y - x * Math.tan(-beta); } + return () -> newY; + } + + private DoubleSupplier firstTurnIfBad() { + return () -> Math.atan2(y / 2, x); } - else { - if (newY <= 0 || newY >= y) { - addSequential(new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); + + private DoubleSupplier firstDriveIfBad() { + if (x >= 0) { + firstDriveIfBad = x / Math.sin(Math.PI / 2 - beta); } else { - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, newY, RobotMap.Component.drivePID)); + firstDriveIfBad = x / Math.sin(Math.PI / 2 + beta); } + return () -> firstDriveIfBad; + } + + private DoubleSupplier firstDriveIfGood() { + if (x >= 0) { + firstDriveIfGood = x / Math.sin(beta); + } + else { + firstDriveIfGood = x / Math.sin(-beta); + } + return () -> firstDriveIfGood; + } + + public boolean isFinished() { + return false; } } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 4e2dce2b..ea05d806 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 4e2dce2baba2b11a9c7731b4b440081e4267c229 +Subproject commit ea05d8064494ea41570d7f6bdcd38f9c388ce049 From 5fd6a9c3754c16669cd39c8bd8fbd4160cbfb683 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Fri, 15 Feb 2019 19:01:35 -0800 Subject: [PATCH 16/30] standard stuff --- src/main/java/org/usfirst/frc4904/standard | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index ea05d806..86f0cbf9 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit ea05d8064494ea41570d7f6bdcd38f9c388ce049 +Subproject commit 86f0cbf971e4e864ba1024461a2ad6afb494e634 From 3ed7f08cc52c710bb44b35879205a33ea2a5cff1 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sat, 16 Feb 2019 10:23:03 -0800 Subject: [PATCH 17/30] leo cucked the code --- .../robot/autonly/NetworkTablesAuton.java | 83 ++++++++----------- 1 file changed, 34 insertions(+), 49 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java index 0405e4be..0c66e2f5 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java @@ -13,35 +13,31 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; - public class NetworkTablesAuton extends CommandGroup { public static double fallbackAngle = -1; + public static double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; public NetworkTablesAuton() { super("NetworkTablesAuton"); - double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; Data getData = new Data(); addParallel(getData); addSequential(new RunIf( - new ChassisTurn(RobotMap.Component.chassis, getData.firstTurnIfBad(), RobotMap.Component.navx, + new ChassisTurn(RobotMap.Component.chassis, getData.firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> {return getData.getNewY().getAsDouble() >= driveTolerance;} - )); - addSequential(new RunIfElse( - new ChassisMoveDistance(RobotMap.Component.chassis, getData.firstDriveIfBad(), RobotMap.Component.drivePID), - new ChassisMoveDistance(RobotMap.Component.chassis, getData.firstDriveIfGood(), RobotMap.Component.drivePID), - () -> {return getData.getNewY().getAsDouble() >= driveTolerance;} - )); - addSequential(new ChassisTurn(RobotMap.Component.chassis, getData.getBeta(), RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, getData.getY(), RobotMap.Component.drivePID)); + () -> { + return getData.getNewY.getAsDouble() >= driveTolerance; + })); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, getData.firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, getData.getBeta, RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, getData.getY, RobotMap.Component.drivePID)); } + public class Data extends Command { private double beta; private double x; private double y; private double newY; - private double firstDriveIfBad; - private double firstDriveIfGood; public void execute() { beta = RobotMap.NetworkTables.Sensors.beta.getDouble(fallbackAngle); @@ -49,51 +45,40 @@ public void execute() { y = RobotMap.NetworkTables.Sensors.y.getDouble(-1.0); } - private DoubleSupplier getBeta() { - return () -> beta; - } + private final DoubleSupplier getBeta = () -> beta; - private DoubleSupplier getX() { - return () -> x; - } + private final DoubleSupplier getX = () -> x; - private DoubleSupplier getY() { - return () -> y; - } + private final DoubleSupplier getY = () -> y; - private DoubleSupplier getNewY() { + private final DoubleSupplier getNewY = () -> { if (x >= 0) { newY = y - x * Math.tan(beta); - } - else { + } else { newY = y - x * Math.tan(-beta); } - return () -> newY; - } - - private DoubleSupplier firstTurnIfBad() { - return () -> Math.atan2(y / 2, x); - } + return y; + }; - private DoubleSupplier firstDriveIfBad() { - if (x >= 0) { - firstDriveIfBad = x / Math.sin(Math.PI / 2 - beta); - } - else { - firstDriveIfBad = x / Math.sin(Math.PI / 2 + beta); - } - return () -> firstDriveIfBad; - } + private final DoubleSupplier firstTurn = () -> Math.atan2(y / 2, x); - private DoubleSupplier firstDriveIfGood() { - if (x >= 0) { - firstDriveIfGood = x / Math.sin(beta); + private final DoubleSupplier firstDrive = () -> { + double first; + if (newY <= driveTolerance || newY >= y) { + if (x >= 0) { + first = x / Math.sin(Math.PI / 2 - beta); + } else { + first = x / Math.sin(Math.PI / 2 + beta); + } + } else { + if (x >= 0) { + first = x / Math.sin(beta); + } else { + first = x / Math.sin(-beta); + } } - else { - firstDriveIfGood = x / Math.sin(-beta); - } - return () -> firstDriveIfGood; - } + return first; + }; public boolean isFinished() { return false; From be0940d118abd6ebaa3f233e55a1625faeb1e4b7 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sat, 16 Feb 2019 12:48:07 -0800 Subject: [PATCH 18/30] changed stuff in the classes and deleted interestingauton.java since nikhil said it was unneeded. --- .../frc4904/robot/autonly/BoringAuton.java | 62 ++++++++----------- .../robot/autonly/InterestingAuton.java | 44 ------------- .../robot/autonly/NetworkTablesAuton.java | 5 +- 3 files changed, 27 insertions(+), 84 deletions(-) delete mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java index 515d185e..0f164797 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java @@ -2,50 +2,38 @@ import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.usfirst.frc4904.robot.RobotMap; import edu.wpi.first.wpilibj.command.CommandGroup; import org.usfirst.frc4904.standard.commands.RunIf; +import org.usfirst.frc4904.standard.commands.RunIfElse; public class BoringAuton extends CommandGroup { public BoringAuton(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { super("BoringAuton"); - double beta = betaSupplier.getAsDouble(); - double x = xSupplier.getAsDouble(); - double y = ySupplier.getAsDouble(); - - addSequential(new RunIf( - new ChassisTurn(RobotMap.Component.chassis, 90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), - () -> {return x >= 0;} - )); - addSequential(new RunIf( - new ChassisMoveDistance(RobotMap.Component.chassis, x, RobotMap.Component.drivePID), - () -> {return x >= 0;} - )); - addSequential(new RunIf( - new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), - () -> {return x >= 0;} - )); - addSequential(new RunIf( - new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID), - () -> {return x >= 0;} - )); - addSequential(new RunIf( - new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), - () -> {return x < 0;} - )); - addSequential(new RunIf( - new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID), - () -> {return x < 0;} - )); - addSequential(new RunIf( - new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC), - () -> {return x < 0;} - )); - addSequential(new RunIf( - new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID), - () -> {return x < 0;} - )); + addSequential(new RunIfElse( + new BoringAutonPosX(betaSupplier, xSupplier, ySupplier), //the left of the robot is towards the positive x direction + new BoringAutonNegX(betaSupplier, xSupplier, ySupplier), + () -> xSupplier.getAsDouble() >= 0 + )); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, () -> ySupplier.getAsDouble(), RobotMap.Component.drivePID)); + } + public class BoringAutonPosX extends CommandGroup { + public BoringAutonPosX(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + super("BoringAutonPosX"); //chassisturn and beta are positive to the right + addSequential(new ChassisTurn(RobotMap.Component.chassis, () -> 90 + betaSupplier.getAsDouble(), RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, () -> xSupplier.getAsDouble(), RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + } + } + public class BoringAutonNegX extends CommandGroup { + public BoringAutonNegX(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + super("BoringAutonNegX"); + addSequential(new ChassisTurn(RobotMap.Component.chassis, () -> -90 + betaSupplier.getAsDouble(), RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, () -> -xSupplier.getAsDouble(), RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); } - } \ No newline at end of file + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java deleted file mode 100644 index f7df5f51..00000000 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/InterestingAuton.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.usfirst.frc4904.robot.autonly; - -import java.lang.Math; -import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; -import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; -import org.usfirst.frc4904.robot.RobotMap; -import edu.wpi.first.wpilibj.command.CommandGroup; - -public class InterestingAuton extends CommandGroup { - - public InterestingAuton(double beta, double x, double y) { - super("InterestingAuton"); - if (x >= 0) { - double firstDrive = x/Math.sin(-beta); - double newY = y - x/Math.tan(-beta); - if (newY <= 0 || newY >= y) { - addSequential(new ChassisTurn(RobotMap.Component.chassis, 90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, x, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); - } - else { - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, -beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, newY, RobotMap.Component.drivePID)); - } - } - else { - double firstDrive = x/Math.sin(beta); - double newY = y - x/Math.tan(beta); - if (newY <= 0 || newY >= y) { - addSequential(new ChassisTurn(RobotMap.Component.chassis, -90 + beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, -x, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, y, RobotMap.Component.drivePID)); - } - else { - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, beta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, newY, RobotMap.Component.drivePID)); - } - } - } -} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java index 0c66e2f5..226df219 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java @@ -24,9 +24,8 @@ public NetworkTablesAuton() { addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, getData.firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> { - return getData.getNewY.getAsDouble() >= driveTolerance; - })); + () -> getData.getNewY.getAsDouble() >= driveTolerance + )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, getData.firstDrive, RobotMap.Component.drivePID)); addSequential(new ChassisTurn(RobotMap.Component.chassis, getData.getBeta, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); From 8eda0a0c1ae02b37c7bfafa83a867b518cc9a23c Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sat, 16 Feb 2019 16:14:39 -0800 Subject: [PATCH 19/30] moved networktables stuff to robotmap so we can choose when to update values --- .../org/usfirst/frc4904/robot/RobotMap.java | 27 +++++- .../frc4904/robot/autonly/BoringAuton.java | 1 + .../robot/autonly/NetworkTablesAuton.java | 86 ++++++------------- 3 files changed, 53 insertions(+), 61 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 706f5d08..dda5121b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -11,6 +11,8 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc4904.standard.custom.sensors.PDP; import org.usfirst.frc4904.standard.custom.PCMPort; @@ -113,7 +115,30 @@ public static class Driver { public static class Operator { public static CustomJoystick joystick; } - } + } + + public static class UpdateableData { + public static double beta; + public static double x; + public static double y; + public static class Update extends Command { + public Update() {} + + @Override + protected void initialize() { + beta = RobotMap.NetworkTables.Sensors.beta.getDouble(0); + x = RobotMap.NetworkTables.Sensors.x.getDouble(0); + y = RobotMap.NetworkTables.Sensors.y.getDouble(0); + } + protected boolean isFinished() { + return true; + } + } + public static final DoubleSupplier getBeta = () -> beta; + public static final DoubleSupplier getX = () -> x; + public static final DoubleSupplier getY = () -> y; + } + public RobotMap() { Component.pdp = new PDP(); diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java index 0f164797..3bb104c0 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java @@ -5,6 +5,7 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; import org.usfirst.frc4904.standard.commands.RunIf; import org.usfirst.frc4904.standard.commands.RunIfElse; diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java index 226df219..9f8b6365 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java @@ -14,73 +14,39 @@ import java.util.function.DoubleSupplier; public class NetworkTablesAuton extends CommandGroup { - public static double fallbackAngle = -1; + public static double fallbackValue = -1; public static double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; + public static double minimumVisionDistance = 18; + + private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.beta); + + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y/2,minimumVisionDistance), RobotMap.UpdateableData.x); + + private final DoubleSupplier firstDrive = () -> { + double first; + if (RobotMap.UpdateableData.x >= 0) { + first = RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.beta); + } else { + first = RobotMap.UpdateableData.x / Math.sin(-RobotMap.UpdateableData.beta); + } + return first; + }; public NetworkTablesAuton() { super("NetworkTablesAuton"); - Data getData = new Data(); - addParallel(getData); + addSequential(new RobotMap.UpdateableData.Update()); addSequential(new RunIf( - new ChassisTurn(RobotMap.Component.chassis, getData.firstTurn, RobotMap.Component.navx, + new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> getData.getNewY.getAsDouble() >= driveTolerance + () -> getNewY.getAsDouble() <= driveTolerance || getNewY.getAsDouble() >= RobotMap.UpdateableData.y )); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, getData.firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, getData.getBeta, RobotMap.Component.navx, + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.beta, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, getData.getY, RobotMap.Component.drivePID)); - } - - public class Data extends Command { - private double beta; - private double x; - private double y; - private double newY; - - public void execute() { - beta = RobotMap.NetworkTables.Sensors.beta.getDouble(fallbackAngle); - x = RobotMap.NetworkTables.Sensors.x.getDouble(-1.0); - y = RobotMap.NetworkTables.Sensors.y.getDouble(-1.0); - } - - private final DoubleSupplier getBeta = () -> beta; - - private final DoubleSupplier getX = () -> x; - - private final DoubleSupplier getY = () -> y; - - private final DoubleSupplier getNewY = () -> { - if (x >= 0) { - newY = y - x * Math.tan(beta); - } else { - newY = y - x * Math.tan(-beta); - } - return y; - }; - - private final DoubleSupplier firstTurn = () -> Math.atan2(y / 2, x); - - private final DoubleSupplier firstDrive = () -> { - double first; - if (newY <= driveTolerance || newY >= y) { - if (x >= 0) { - first = x / Math.sin(Math.PI / 2 - beta); - } else { - first = x / Math.sin(Math.PI / 2 + beta); - } - } else { - if (x >= 0) { - first = x / Math.sin(beta); - } else { - first = x / Math.sin(-beta); - } - } - return first; - }; - - public boolean isFinished() { - return false; - } + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBeta, RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC)); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.y, RobotMap.Component.drivePID)); } } \ No newline at end of file From bbd0b48a2f556f5027fdcf12d813455029c139c4 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 17 Feb 2019 10:55:51 -0800 Subject: [PATCH 20/30] edited code so robot should always be able to see tapes when it needs to --- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 4 +++- .../org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index dda5121b..2505c197 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -60,7 +60,7 @@ public static class Wheel { / Metrics.Wheel.TICKS_PER_REVOLUTION; } public static class Robot { - public static final double ROBOT_WIDTH = 4.904; + public static final double ROBOT_WIDTH = 28; } } public static class PID { @@ -100,6 +100,7 @@ public static class Sensors { public static NetworkTableEntry beta; public static NetworkTableEntry x; public static NetworkTableEntry y; + public static NetworkTableEntry isTapeVisible; } public static class PID { @@ -161,6 +162,7 @@ public RobotMap() { NetworkTables.Sensors.beta = NetworkTables.Sensors.table.getEntry("beta"); NetworkTables.Sensors.x = NetworkTables.Sensors.table.getEntry("x"); NetworkTables.Sensors.y = NetworkTables.Sensors.table.getEntry("y"); + NetworkTables.Sensors.isTapeVisible = NetworkTables.Sensors.table.getEntry("Is Tape Visible?"); // Chassis Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java index 9f8b6365..0eaf5d43 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java @@ -38,7 +38,7 @@ public NetworkTablesAuton() { addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> getNewY.getAsDouble() <= driveTolerance || getNewY.getAsDouble() >= RobotMap.UpdateableData.y + () -> getNewY.getAsDouble() <= Math.max(driveTolerance,minimumVisionDistance) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.beta, RobotMap.Component.navx, From 03fbfbd941c876c66aaaa3627231334c93f7f8d4 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 17 Feb 2019 12:00:02 -0800 Subject: [PATCH 21/30] complete overhaul including new classes --- .../org/usfirst/frc4904/robot/RobotMap.java | 27 +++++++--- .../robot/autonly/FloorTapeAutoAlign.java | 54 +++++++++++++++++++ ...ton.java => VisionFloorTapeAutoAlign.java} | 18 +++---- .../robot/autonly/VisionTapeAutoAlign.java | 54 +++++++++++++++++++ 4 files changed, 137 insertions(+), 16 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java rename src/main/java/org/usfirst/frc4904/robot/autonly/{NetworkTablesAuton.java => VisionFloorTapeAutoAlign.java} (83%) create mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 2505c197..1b16ab1b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -97,7 +97,9 @@ public static class NetworkTables { public static class Sensors { public static NetworkTable table; - public static NetworkTableEntry beta; + public static NetworkTableEntry betaRetroreflective; + public static NetworkTableEntry betaFloor; + public static NetworkTableEntry theta; public static NetworkTableEntry x; public static NetworkTableEntry y; public static NetworkTableEntry isTapeVisible; @@ -119,25 +121,34 @@ public static class Operator { } public static class UpdateableData { - public static double beta; + public static double betaRetroreflective; + public static double betaFloor; + public static double theta; public static double x; public static double y; + public static double isTapeVisible; public static class Update extends Command { public Update() {} @Override protected void initialize() { - beta = RobotMap.NetworkTables.Sensors.beta.getDouble(0); + betaRetroreflective = RobotMap.NetworkTables.Sensors.betaRetroreflective.getDouble(0); + betaFloor = RobotMap.NetworkTables.Sensors.betaFloor.getDouble(0); + theta = RobotMap.NetworkTables.Sensors.theta.getDouble(0); x = RobotMap.NetworkTables.Sensors.x.getDouble(0); y = RobotMap.NetworkTables.Sensors.y.getDouble(0); + isTapeVisible = RobotMap.NetworkTables.Sensors.isTapeVisible.getDouble(0); } protected boolean isFinished() { return true; } } - public static final DoubleSupplier getBeta = () -> beta; + public static final DoubleSupplier getBetaRetroreflective = () -> betaRetroreflective; + public static final DoubleSupplier getBetaFloor = () -> betaFloor; + public static final DoubleSupplier getTheta = () -> theta; public static final DoubleSupplier getX = () -> x; public static final DoubleSupplier getY = () -> y; + public static final DoubleSupplier getIsTapeVisible = () -> isTapeVisible; } public RobotMap() { @@ -159,9 +170,11 @@ public RobotMap() { NetworkTables.inst = NetworkTableInstance.getDefault(); NetworkTables.Sensors.table = NetworkTables.inst.getTable("Sensors"); - NetworkTables.Sensors.beta = NetworkTables.Sensors.table.getEntry("beta"); - NetworkTables.Sensors.x = NetworkTables.Sensors.table.getEntry("x"); - NetworkTables.Sensors.y = NetworkTables.Sensors.table.getEntry("y"); + NetworkTables.Sensors.betaRetroreflective = NetworkTables.Sensors.table.getEntry("Retroreflective Beta"); + NetworkTables.Sensors.betaFloor = NetworkTables.Sensors.table.getEntry("Floor Beta"); + NetworkTables.Sensors.theta = NetworkTables.Sensors.table.getEntry("Theta"); + NetworkTables.Sensors.x = NetworkTables.Sensors.table.getEntry("X"); + NetworkTables.Sensors.y = NetworkTables.Sensors.table.getEntry("Y"); NetworkTables.Sensors.isTapeVisible = NetworkTables.Sensors.table.getEntry("Is Tape Visible?"); // Chassis diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java new file mode 100644 index 00000000..8431e5aa --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java @@ -0,0 +1,54 @@ +package org.usfirst.frc4904.robot.autonly; + + +import java.lang.Math; +import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; +import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; +import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.networktables.NetworkTable; +import org.usfirst.frc4904.standard.commands.RunIfElse; +import org.usfirst.frc4904.standard.commands.RunIf; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class FloorTapeAutoAlign extends CommandGroup { + public static double fallbackValue = -1; + public static double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; + public static double minimumVisionDistance = 18; + + RobotMap.UpdateableData.Update update = new RobotMap.UpdateableData.Update(); + + private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRetroreflective); + + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, minimumVisionDistance), RobotMap.UpdateableData.x); + + private final DoubleSupplier firstDrive = () -> { + double first; + if (RobotMap.UpdateableData.x >= 0) { + first = RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRetroreflective); + } else { + first = RobotMap.UpdateableData.x / Math.sin(-RobotMap.UpdateableData.betaRetroreflective); + } + return first; + }; + + public FloorTapeAutoAlign() { + super("FloorTapeAutoAlign"); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new RunIf( + new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC), + () -> getNewY.getAsDouble() <= Math.max(driveTolerance,minimumVisionDistance) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y + )); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.betaRetroreflective, RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC)); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRetroreflective, RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC)); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.y, RobotMap.Component.drivePID)); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java similarity index 83% rename from src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java rename to src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java index 0eaf5d43..bbb6401a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/NetworkTablesAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java @@ -13,27 +13,27 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -public class NetworkTablesAuton extends CommandGroup { +public class VisionFloorTapeAutoAlign extends CommandGroup { public static double fallbackValue = -1; public static double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; public static double minimumVisionDistance = 18; - private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.beta); + private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRetroreflective); - private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y/2,minimumVisionDistance), RobotMap.UpdateableData.x); + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, minimumVisionDistance), RobotMap.UpdateableData.x); private final DoubleSupplier firstDrive = () -> { double first; if (RobotMap.UpdateableData.x >= 0) { - first = RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.beta); + first = RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRetroreflective); } else { - first = RobotMap.UpdateableData.x / Math.sin(-RobotMap.UpdateableData.beta); + first = RobotMap.UpdateableData.x / Math.sin(-RobotMap.UpdateableData.betaRetroreflective); } return first; }; - public NetworkTablesAuton() { - super("NetworkTablesAuton"); + public VisionFloorTapeAutoAlign() { + super("VisionFloorTapeAutoAlign"); addSequential(new RobotMap.UpdateableData.Update()); addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, @@ -41,10 +41,10 @@ public NetworkTablesAuton() { () -> getNewY.getAsDouble() <= Math.max(driveTolerance,minimumVisionDistance) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.beta, RobotMap.Component.navx, + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.betaFloor, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBeta, RobotMap.Component.navx, + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaFloor, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); addSequential(new RobotMap.UpdateableData.Update()); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.y, RobotMap.Component.drivePID)); diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java new file mode 100644 index 00000000..af5b2fb0 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java @@ -0,0 +1,54 @@ +package org.usfirst.frc4904.robot.autonly; + + +import java.lang.Math; +import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; +import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; +import org.usfirst.frc4904.robot.RobotMap; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.networktables.NetworkTable; +import org.usfirst.frc4904.standard.commands.RunIfElse; +import org.usfirst.frc4904.standard.commands.RunIf; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class VisionTapeAutoAlign extends CommandGroup { + public static double fallbackValue = -1; + public static double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; + public static double minimumVisionDistance = 18; + + RobotMap.UpdateableData.Update update = new RobotMap.UpdateableData.Update(); + + private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRetroreflective); + + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, minimumVisionDistance), RobotMap.UpdateableData.x); + + private final DoubleSupplier firstDrive = () -> { + double first; + if (RobotMap.UpdateableData.x >= 0) { + first = RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRetroreflective); + } else { + first = RobotMap.UpdateableData.x / Math.sin(-RobotMap.UpdateableData.betaRetroreflective); + } + return first; + }; + + public VisionTapeAutoAlign() { + super("VisionTapeAutoAlign"); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new RunIf( + new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC), + () -> getNewY.getAsDouble() <= Math.max(driveTolerance,minimumVisionDistance) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y + )); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.betaRetroreflective, RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC)); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRetroreflective, RobotMap.Component.navx, + RobotMap.Component.chassisTurnMC)); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.y, RobotMap.Component.drivePID)); + } +} \ No newline at end of file From 0d5f5c96195a0acdb0446813157dd11ac6e2a44a Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 17 Feb 2019 13:36:49 -0800 Subject: [PATCH 22/30] made a config class, finished up 3 routines (hopefully) --- .../org/usfirst/frc4904/robot/RobotMap.java | 25 ++++++---- .../frc4904/robot/autonly/AutonConfig.java | 8 ++++ .../robot/autonly/FloorTapeAutoAlign.java | 48 +++++++------------ .../autonly/VisionFloorTapeAutoAlign.java | 28 +++-------- .../robot/autonly/VisionTapeAutoAlign.java | 32 ++++--------- 5 files changed, 54 insertions(+), 87 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 1b16ab1b..92344f11 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -97,12 +97,13 @@ public static class NetworkTables { public static class Sensors { public static NetworkTable table; - public static NetworkTableEntry betaRetroreflective; + public static NetworkTableEntry betaRF; public static NetworkTableEntry betaFloor; public static NetworkTableEntry theta; public static NetworkTableEntry x; public static NetworkTableEntry y; - public static NetworkTableEntry isTapeVisible; + public static NetworkTableEntry isRFTapeVisible; + public static NetworkTableEntry isFloorTapeVisible; } public static class PID { @@ -121,34 +122,37 @@ public static class Operator { } public static class UpdateableData { - public static double betaRetroreflective; + public static double betaRF; public static double betaFloor; public static double theta; public static double x; public static double y; - public static double isTapeVisible; + public static double isRFTapeVisible; + public static double isFloorTapeVisible; public static class Update extends Command { public Update() {} @Override protected void initialize() { - betaRetroreflective = RobotMap.NetworkTables.Sensors.betaRetroreflective.getDouble(0); + betaRF = RobotMap.NetworkTables.Sensors.betaRF.getDouble(0); betaFloor = RobotMap.NetworkTables.Sensors.betaFloor.getDouble(0); theta = RobotMap.NetworkTables.Sensors.theta.getDouble(0); x = RobotMap.NetworkTables.Sensors.x.getDouble(0); y = RobotMap.NetworkTables.Sensors.y.getDouble(0); - isTapeVisible = RobotMap.NetworkTables.Sensors.isTapeVisible.getDouble(0); + isRFTapeVisible = RobotMap.NetworkTables.Sensors.isRFTapeVisible.getDouble(0); + isFloorTapeVisible = RobotMap.NetworkTables.Sensors.isRFTapeVisible.getDouble(0); } protected boolean isFinished() { return true; } } - public static final DoubleSupplier getBetaRetroreflective = () -> betaRetroreflective; + public static final DoubleSupplier getBetaRF = () -> betaRF; public static final DoubleSupplier getBetaFloor = () -> betaFloor; public static final DoubleSupplier getTheta = () -> theta; public static final DoubleSupplier getX = () -> x; public static final DoubleSupplier getY = () -> y; - public static final DoubleSupplier getIsTapeVisible = () -> isTapeVisible; + public static final DoubleSupplier getIsRFTapeVisible = () -> isRFTapeVisible; + public static final DoubleSupplier getIsFloorTapeVisible = () -> isFloorTapeVisible; } public RobotMap() { @@ -170,12 +174,13 @@ public RobotMap() { NetworkTables.inst = NetworkTableInstance.getDefault(); NetworkTables.Sensors.table = NetworkTables.inst.getTable("Sensors"); - NetworkTables.Sensors.betaRetroreflective = NetworkTables.Sensors.table.getEntry("Retroreflective Beta"); + NetworkTables.Sensors.betaRF = NetworkTables.Sensors.table.getEntry("Retroreflective Beta"); NetworkTables.Sensors.betaFloor = NetworkTables.Sensors.table.getEntry("Floor Beta"); NetworkTables.Sensors.theta = NetworkTables.Sensors.table.getEntry("Theta"); NetworkTables.Sensors.x = NetworkTables.Sensors.table.getEntry("X"); NetworkTables.Sensors.y = NetworkTables.Sensors.table.getEntry("Y"); - NetworkTables.Sensors.isTapeVisible = NetworkTables.Sensors.table.getEntry("Is Tape Visible?"); + NetworkTables.Sensors.isRFTapeVisible = NetworkTables.Sensors.table.getEntry("Is RF Tape Visible?"); + NetworkTables.Sensors.isFloorTapeVisible = NetworkTables.Sensors.table.getEntry("Is Floor Tape Visible?"); // Chassis Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java b/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java new file mode 100644 index 00000000..f3a5a8c7 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java @@ -0,0 +1,8 @@ +package org.usfirst.frc4904.robot.autonly; + +import org.usfirst.frc4904.robot.RobotMap; + +public class AutonConfig { + public static final double DRIVE_TOLERANCE = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; + public static final double MINIMUM_VISION_DISTANCE = 18.0; +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java index 8431e5aa..0be706a4 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java @@ -4,6 +4,7 @@ import java.lang.Math; import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; +import org.usfirst.frc4904.standard.commands.chassis.ChassisConstant; import org.usfirst.frc4904.robot.RobotMap; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -14,41 +15,24 @@ import java.util.function.DoubleSupplier; public class FloorTapeAutoAlign extends CommandGroup { - public static double fallbackValue = -1; - public static double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; - public static double minimumVisionDistance = 18; - - RobotMap.UpdateableData.Update update = new RobotMap.UpdateableData.Update(); - - private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRetroreflective); - - private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, minimumVisionDistance), RobotMap.UpdateableData.x); - - private final DoubleSupplier firstDrive = () -> { - double first; - if (RobotMap.UpdateableData.x >= 0) { - first = RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRetroreflective); - } else { - first = RobotMap.UpdateableData.x / Math.sin(-RobotMap.UpdateableData.betaRetroreflective); - } - return first; - }; + public double X = 0; + public double Y = 1; + public double TURN = 0; + public double TIMEOUT = 0; public FloorTapeAutoAlign() { super("FloorTapeAutoAlign"); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new RunIf( - new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, - RobotMap.Component.chassisTurnMC), - () -> getNewY.getAsDouble() <= Math.max(driveTolerance,minimumVisionDistance) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y - )); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.betaRetroreflective, RobotMap.Component.navx, - RobotMap.Component.chassisTurnMC)); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRetroreflective, RobotMap.Component.navx, + addSequential(new DriveUntilTape()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaFloor.getAsDouble(), RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.y, RobotMap.Component.drivePID)); + } + public class DriveUntilTape extends ChassisConstant { + public DriveUntilTape() { + super(RobotMap.Component.chassis, X, Y, TURN, TIMEOUT); + } + + protected boolean isFinished() { + return RobotMap.UpdateableData.getIsFloorTapeVisible.getAsDouble() == 1; + } } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java index bbb6401a..38286b53 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java @@ -5,32 +5,18 @@ import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; import org.usfirst.frc4904.robot.RobotMap; -import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.networktables.NetworkTable; -import org.usfirst.frc4904.standard.commands.RunIfElse; import org.usfirst.frc4904.standard.commands.RunIf; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.usfirst.frc4904.robot.autonly.AutonConfig; public class VisionFloorTapeAutoAlign extends CommandGroup { - public static double fallbackValue = -1; - public static double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; - public static double minimumVisionDistance = 18; - - private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRetroreflective); + private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRF); - private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, minimumVisionDistance), RobotMap.UpdateableData.x); + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, AutonConfig.MINIMUM_VISION_DISTANCE), RobotMap.UpdateableData.x); - private final DoubleSupplier firstDrive = () -> { - double first; - if (RobotMap.UpdateableData.x >= 0) { - first = RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRetroreflective); - } else { - first = RobotMap.UpdateableData.x / Math.sin(-RobotMap.UpdateableData.betaRetroreflective); - } - return first; - }; + private final DoubleSupplier firstDrive = () -> Math.abs(RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRF)); public VisionFloorTapeAutoAlign() { super("VisionFloorTapeAutoAlign"); @@ -38,15 +24,15 @@ public VisionFloorTapeAutoAlign() { addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> getNewY.getAsDouble() <= Math.max(driveTolerance,minimumVisionDistance) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y + () -> getNewY.getAsDouble() <= Math.max(AutonConfig.DRIVE_TOLERANCE,AutonConfig.MINIMUM_VISION_DISTANCE) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.betaFloor, RobotMap.Component.navx, + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaFloor, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); addSequential(new RobotMap.UpdateableData.Update()); addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaFloor, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.y, RobotMap.Component.drivePID)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.getY, RobotMap.Component.drivePID)); } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java index af5b2fb0..b3fd6542 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java @@ -5,34 +5,18 @@ import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; import org.usfirst.frc4904.robot.RobotMap; -import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.networktables.NetworkTable; -import org.usfirst.frc4904.standard.commands.RunIfElse; import org.usfirst.frc4904.standard.commands.RunIf; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.usfirst.frc4904.robot.autonly.AutonConfig; public class VisionTapeAutoAlign extends CommandGroup { - public static double fallbackValue = -1; - public static double driveTolerance = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; - public static double minimumVisionDistance = 18; + private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRF); - RobotMap.UpdateableData.Update update = new RobotMap.UpdateableData.Update(); - - private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRetroreflective); + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, AutonConfig.MINIMUM_VISION_DISTANCE), RobotMap.UpdateableData.x); - private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, minimumVisionDistance), RobotMap.UpdateableData.x); - - private final DoubleSupplier firstDrive = () -> { - double first; - if (RobotMap.UpdateableData.x >= 0) { - first = RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRetroreflective); - } else { - first = RobotMap.UpdateableData.x / Math.sin(-RobotMap.UpdateableData.betaRetroreflective); - } - return first; - }; + private final DoubleSupplier firstDrive = () -> Math.abs(RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRF)); public VisionTapeAutoAlign() { super("VisionTapeAutoAlign"); @@ -40,15 +24,15 @@ public VisionTapeAutoAlign() { addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> getNewY.getAsDouble() <= Math.max(driveTolerance,minimumVisionDistance) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y + () -> getNewY.getAsDouble() <= Math.max(AutonConfig.DRIVE_TOLERANCE,AutonConfig.MINIMUM_VISION_DISTANCE) || getNewY.getAsDouble() >= RobotMap.UpdateableData.getY.getAsDouble() )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.betaRetroreflective, RobotMap.Component.navx, + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRF, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRetroreflective, RobotMap.Component.navx, + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRF, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.y, RobotMap.Component.drivePID)); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.getY, RobotMap.Component.drivePID)); } } \ No newline at end of file From f5646594fa0a2a814a9e406e744c16ef679d3b04 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Mon, 18 Feb 2019 10:35:56 -0800 Subject: [PATCH 23/30] fixed some minor syntax stuff and made notsplines.java work correctly. --- .../org/usfirst/frc4904/robot/RobotMap.java | 7 ++++++- .../frc4904/robot/autonly/AutonConfig.java | 1 + .../robot/autonly/FloorTapeAutoAlign.java | 9 +++++---- .../autonly/VisionFloorTapeAutoAlign.java | 11 +++++++++-- .../robot/autonly/VisionTapeAutoAlign.java | 11 +++++++++-- .../frc4904/robot/commands/NotSplines.java | 18 +++++------------- 6 files changed, 35 insertions(+), 22 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 92344f11..6a3059d3 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -102,6 +102,7 @@ public static class Sensors { public static NetworkTableEntry theta; public static NetworkTableEntry x; public static NetworkTableEntry y; + public static NetworkTableEntry distance; public static NetworkTableEntry isRFTapeVisible; public static NetworkTableEntry isFloorTapeVisible; } @@ -127,6 +128,7 @@ public static class UpdateableData { public static double theta; public static double x; public static double y; + public static double distance; public static double isRFTapeVisible; public static double isFloorTapeVisible; public static class Update extends Command { @@ -139,6 +141,7 @@ protected void initialize() { theta = RobotMap.NetworkTables.Sensors.theta.getDouble(0); x = RobotMap.NetworkTables.Sensors.x.getDouble(0); y = RobotMap.NetworkTables.Sensors.y.getDouble(0); + distance = RobotMap.NetworkTables.Sensors.distance.getDouble(0); isRFTapeVisible = RobotMap.NetworkTables.Sensors.isRFTapeVisible.getDouble(0); isFloorTapeVisible = RobotMap.NetworkTables.Sensors.isRFTapeVisible.getDouble(0); } @@ -151,6 +154,7 @@ protected boolean isFinished() { public static final DoubleSupplier getTheta = () -> theta; public static final DoubleSupplier getX = () -> x; public static final DoubleSupplier getY = () -> y; + public static final DoubleSupplier getDistance = () -> distance; public static final DoubleSupplier getIsRFTapeVisible = () -> isRFTapeVisible; public static final DoubleSupplier getIsFloorTapeVisible = () -> isFloorTapeVisible; } @@ -174,11 +178,12 @@ public RobotMap() { NetworkTables.inst = NetworkTableInstance.getDefault(); NetworkTables.Sensors.table = NetworkTables.inst.getTable("Sensors"); - NetworkTables.Sensors.betaRF = NetworkTables.Sensors.table.getEntry("Retroreflective Beta"); + NetworkTables.Sensors.betaRF = NetworkTables.Sensors.table.getEntry("RF Beta"); NetworkTables.Sensors.betaFloor = NetworkTables.Sensors.table.getEntry("Floor Beta"); NetworkTables.Sensors.theta = NetworkTables.Sensors.table.getEntry("Theta"); NetworkTables.Sensors.x = NetworkTables.Sensors.table.getEntry("X"); NetworkTables.Sensors.y = NetworkTables.Sensors.table.getEntry("Y"); + NetworkTables.Sensors.distance = NetworkTables.Sensors.table.getEntry("Distance"); NetworkTables.Sensors.isRFTapeVisible = NetworkTables.Sensors.table.getEntry("Is RF Tape Visible?"); NetworkTables.Sensors.isFloorTapeVisible = NetworkTables.Sensors.table.getEntry("Is Floor Tape Visible?"); diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java b/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java index f3a5a8c7..b7a23170 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java @@ -5,4 +5,5 @@ public class AutonConfig { public static final double DRIVE_TOLERANCE = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; public static final double MINIMUM_VISION_DISTANCE = 18.0; + public static final double NO_CLOSER = Math.max(DRIVE_TOLERANCE, MINIMUM_VISION_DISTANCE); } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java index 0be706a4..7dd7dc52 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java @@ -3,17 +3,18 @@ import java.lang.Math; import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; -import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; import org.usfirst.frc4904.standard.commands.chassis.ChassisConstant; import org.usfirst.frc4904.robot.RobotMap; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.networktables.NetworkTable; -import org.usfirst.frc4904.standard.commands.RunIfElse; -import org.usfirst.frc4904.standard.commands.RunIf; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +/* +** The robot will drive forwards until it can see the white tape. +** Once the robot can see the white tape, it will stop and turn parallel to it. +*/ + public class FloorTapeAutoAlign extends CommandGroup { public double X = 0; public double Y = 1; diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java index 38286b53..d2af5c6c 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java @@ -11,10 +11,17 @@ import java.util.function.DoubleSupplier; import org.usfirst.frc4904.robot.autonly.AutonConfig; +/* +** If the current path of robot will intersect the tape in a specific area, then it will drive forwards. +** Otherwise, it will turn to be facing that area then drive forwards. +** After driving forwards, the robot will turn to be facing the hatch. +** The robot will then drive forwards to the hatch. +*/ + public class VisionFloorTapeAutoAlign extends CommandGroup { private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRF); - private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, AutonConfig.MINIMUM_VISION_DISTANCE), RobotMap.UpdateableData.x); + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, AutonConfig.NO_CLOSER), RobotMap.UpdateableData.x); private final DoubleSupplier firstDrive = () -> Math.abs(RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRF)); @@ -24,7 +31,7 @@ public VisionFloorTapeAutoAlign() { addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> getNewY.getAsDouble() <= Math.max(AutonConfig.DRIVE_TOLERANCE,AutonConfig.MINIMUM_VISION_DISTANCE) || getNewY.getAsDouble() >= RobotMap.UpdateableData.y + () -> getNewY.getAsDouble() <= AutonConfig.NO_CLOSER || getNewY.getAsDouble() >= RobotMap.UpdateableData.getY.getAsDouble() )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaFloor, RobotMap.Component.navx, diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java index b3fd6542..7d622dac 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java @@ -11,10 +11,17 @@ import java.util.function.DoubleSupplier; import org.usfirst.frc4904.robot.autonly.AutonConfig; +/* +** If the current path of robot will intersect the tape in a specific area, then it will drive forwards. +** Otherwise, it will turn to be facing that area then drive forwards. +** After driving forwards, the robot will turn to be facing the hatch. +** The robot will then drive forwards to the hatch. +*/ + public class VisionTapeAutoAlign extends CommandGroup { private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRF); - private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, AutonConfig.MINIMUM_VISION_DISTANCE), RobotMap.UpdateableData.x); + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, AutonConfig.NO_CLOSER), RobotMap.UpdateableData.x); private final DoubleSupplier firstDrive = () -> Math.abs(RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRF)); @@ -24,7 +31,7 @@ public VisionTapeAutoAlign() { addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> getNewY.getAsDouble() <= Math.max(AutonConfig.DRIVE_TOLERANCE,AutonConfig.MINIMUM_VISION_DISTANCE) || getNewY.getAsDouble() >= RobotMap.UpdateableData.getY.getAsDouble() + () -> getNewY.getAsDouble() <= AutonConfig.NO_CLOSER || getNewY.getAsDouble() >= RobotMap.UpdateableData.getY.getAsDouble() )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRF, RobotMap.Component.navx, diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java index bf92b836..55b063d2 100644 --- a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java +++ b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java @@ -1,24 +1,16 @@ package org.usfirst.frc4904.robot.commands; -import java.lang.Math; import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; import org.usfirst.frc4904.robot.RobotMap; import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.networktables.NetworkTable; public class NotSplines extends CommandGroup { - public static class Values{ - public static final double distance = 1.0; - // public static final double distance = RobotMap.NetworkTables.PID.distance; - public static final double angle = 45.0; - // public static final double angle = RobotMap.NetworkTables.PID.angle; - public static final double distance_x = distance*Math.sin(angle); - public static final double distance_y = distance*Math.cos(angle); - } public NotSplines() { - super("NotSplines"); - addSequential(new ChassisTurn(RobotMap.Component.chassis, Values.angle, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addParallel(new ChassisMoveDistance(RobotMap.Component.chassis, Values.distance, RobotMap.Component.drivePID)); + super("NotSplines"); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getTheta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(new RobotMap.UpdateableData.Update()); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.getDistance, RobotMap.Component.drivePID)); } } \ No newline at end of file From 8f58d531f86339bb88f2f7d170f46fb38a780181 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Mon, 18 Feb 2019 13:48:39 -0800 Subject: [PATCH 24/30] made the UpdateableData.java file and worked with rohan to add CAN support. --- .../org/usfirst/frc4904/robot/RobotMap.java | 39 ------ .../frc4904/robot/autonly/AutonConfig.java | 6 + .../robot/autonly/FloorTapeAutoAlign.java | 6 +- .../frc4904/robot/autonly/UpdateableData.java | 128 ++++++++++++++++++ .../autonly/VisionFloorTapeAutoAlign.java | 20 +-- .../robot/autonly/VisionTapeAutoAlign.java | 21 +-- .../frc4904/robot/commands/NotSplines.java | 9 +- 7 files changed, 164 insertions(+), 65 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4904/robot/autonly/UpdateableData.java diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 6a3059d3..7a42a8ff 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -11,8 +11,6 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import java.util.function.DoubleSupplier; -import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc4904.standard.custom.sensors.PDP; import org.usfirst.frc4904.standard.custom.PCMPort; @@ -121,43 +119,6 @@ public static class Operator { public static CustomJoystick joystick; } } - - public static class UpdateableData { - public static double betaRF; - public static double betaFloor; - public static double theta; - public static double x; - public static double y; - public static double distance; - public static double isRFTapeVisible; - public static double isFloorTapeVisible; - public static class Update extends Command { - public Update() {} - - @Override - protected void initialize() { - betaRF = RobotMap.NetworkTables.Sensors.betaRF.getDouble(0); - betaFloor = RobotMap.NetworkTables.Sensors.betaFloor.getDouble(0); - theta = RobotMap.NetworkTables.Sensors.theta.getDouble(0); - x = RobotMap.NetworkTables.Sensors.x.getDouble(0); - y = RobotMap.NetworkTables.Sensors.y.getDouble(0); - distance = RobotMap.NetworkTables.Sensors.distance.getDouble(0); - isRFTapeVisible = RobotMap.NetworkTables.Sensors.isRFTapeVisible.getDouble(0); - isFloorTapeVisible = RobotMap.NetworkTables.Sensors.isRFTapeVisible.getDouble(0); - } - protected boolean isFinished() { - return true; - } - } - public static final DoubleSupplier getBetaRF = () -> betaRF; - public static final DoubleSupplier getBetaFloor = () -> betaFloor; - public static final DoubleSupplier getTheta = () -> theta; - public static final DoubleSupplier getX = () -> x; - public static final DoubleSupplier getY = () -> y; - public static final DoubleSupplier getDistance = () -> distance; - public static final DoubleSupplier getIsRFTapeVisible = () -> isRFTapeVisible; - public static final DoubleSupplier getIsFloorTapeVisible = () -> isFloorTapeVisible; - } public RobotMap() { Component.pdp = new PDP(); diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java b/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java index b7a23170..7547f985 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/AutonConfig.java @@ -3,7 +3,13 @@ import org.usfirst.frc4904.robot.RobotMap; public class AutonConfig { + public enum VisionInterfaceType { + NETWORK_TABLES, + CAN + } + public static final double DRIVE_TOLERANCE = RobotMap.Metrics.Robot.ROBOT_WIDTH / 2; public static final double MINIMUM_VISION_DISTANCE = 18.0; public static final double NO_CLOSER = Math.max(DRIVE_TOLERANCE, MINIMUM_VISION_DISTANCE); + public static final VisionInterfaceType VISION_INTERFACE_TYPE = VisionInterfaceType.NETWORK_TABLES; } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java index 7dd7dc52..9b973b5c 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/FloorTapeAutoAlign.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.command.CommandGroup; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import org.usfirst.frc4904.robot.autonly.UpdateableData; /* ** The robot will drive forwards until it can see the white tape. @@ -24,7 +25,8 @@ public class FloorTapeAutoAlign extends CommandGroup { public FloorTapeAutoAlign() { super("FloorTapeAutoAlign"); addSequential(new DriveUntilTape()); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaFloor.getAsDouble(), RobotMap.Component.navx, + addSequential(UpdateableData.update()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, UpdateableData.getBetaFloor.getAsDouble(), RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); } public class DriveUntilTape extends ChassisConstant { @@ -33,7 +35,7 @@ public DriveUntilTape() { } protected boolean isFinished() { - return RobotMap.UpdateableData.getIsFloorTapeVisible.getAsDouble() == 1; + return UpdateableData.getIsFloorTapeVisible.getAsBoolean(); } } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/UpdateableData.java b/src/main/java/org/usfirst/frc4904/robot/autonly/UpdateableData.java new file mode 100644 index 00000000..abb8c65e --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/UpdateableData.java @@ -0,0 +1,128 @@ +package org.usfirst.frc4904.robot.autonly; + +import java.nio.ByteBuffer; +import java.util.function.DoubleSupplier; +import java.util.function.BooleanSupplier; +import java.util.Optional; +import edu.wpi.first.wpilibj.command.Command; +import org.usfirst.frc4904.robot.RobotMap; +import org.usfirst.frc4904.standard.custom.CustomCAN; + +public class UpdateableData { + public static double betaRF; + public static double betaFloor; + public static double theta; + public static double x; + public static double y; + public static double distance; + public static boolean isRFTapeVisible; + public static boolean isFloorTapeVisible; + + public static Command update() throws UnsupportedOperationException { + switch (AutonConfig.VISION_INTERFACE_TYPE) { + case NETWORK_TABLES: + return new NTUpdate(); + case CAN: + return new CANUpdate(); + default: + throw new UnsupportedOperationException(); + } + } + + public static class NTUpdate extends Command { + @Override + protected void initialize() { + betaRF = RobotMap.NetworkTables.Sensors.betaRF.getDouble(0); + betaFloor = RobotMap.NetworkTables.Sensors.betaFloor.getDouble(0); + theta = RobotMap.NetworkTables.Sensors.theta.getDouble(0); + x = RobotMap.NetworkTables.Sensors.x.getDouble(0); + y = RobotMap.NetworkTables.Sensors.y.getDouble(0); + distance = RobotMap.NetworkTables.Sensors.distance.getDouble(0); + isRFTapeVisible = RobotMap.NetworkTables.Sensors.isRFTapeVisible.getBoolean(false); + isFloorTapeVisible = RobotMap.NetworkTables.Sensors.isFloorTapeVisible.getBoolean(false); + } + protected boolean isFinished() { + return true; + } + } + + private static class VisionCAN { + private final CustomCAN xChannel; + private final CustomCAN yChannel; + private final CustomCAN thetaChannel; + private final CustomCAN betaChannel; + private final CustomCAN distanceChannel; + private final CustomCAN isTrustableChannel; + + private VisionCAN() { + xChannel = new CustomCAN("Vision::x", 0x11000); + yChannel = new CustomCAN("Vision::y", 0x11001); + thetaChannel = new CustomCAN("Vision::theta", 0x11010); + betaChannel = new CustomCAN("Vision::beta", 0x11011); + distanceChannel = new CustomCAN("Vision::distance", 0x11100); + isTrustableChannel = new CustomCAN("Vision::isTrustable", 0x11101); + } + + protected ByteBuffer bytesToBuffer(byte[] bytes) { + final ByteBuffer buffer = ByteBuffer.allocateDirect(bytes.length); + buffer.put(bytes); + return buffer; + } + + protected Optional getX() { + return xChannel.read() + .map(bytes -> bytesToBuffer(bytes).getDouble()); + } + + protected Optional getY() { + return yChannel.read() + .map(bytes -> bytesToBuffer(bytes).getDouble()); + } + + protected Optional getTheta() { + return thetaChannel.read() + .map(bytes -> bytesToBuffer(bytes).getDouble()); + } + + protected Optional getBeta() { + return betaChannel.read() + .map(bytes -> bytesToBuffer(bytes).getDouble()); + } + + protected Optional getDistance() { + return distanceChannel.read() + .map(bytes -> bytesToBuffer(bytes).getDouble()); + } + + protected Optional getIsTrustable() { + return isTrustableChannel.read() + .map(bytes -> bytes[0] == 0x01); + } + } + + public static class CANUpdate extends Command { + @Override + protected void initialize() { + final VisionCAN visionCAN = new VisionCAN(); + + betaRF = visionCAN.getBeta().orElse(betaRF); + theta = visionCAN.getTheta().orElse(theta); + x = visionCAN.getX().orElse(x); + y = visionCAN.getY().orElse(y); + distance = visionCAN.getDistance().orElse(distance); + isRFTapeVisible = visionCAN.getIsTrustable().orElse(isRFTapeVisible); + } + protected boolean isFinished() { + return true; + } + } + + public static final DoubleSupplier getBetaRF = () -> betaRF; + public static final DoubleSupplier getBetaFloor = () -> betaFloor; + public static final DoubleSupplier getTheta = () -> theta; + public static final DoubleSupplier getX = () -> x; + public static final DoubleSupplier getY = () -> y; + public static final DoubleSupplier getDistance = () -> distance; + public static final BooleanSupplier getIsRFTapeVisible = () -> isRFTapeVisible; + public static final BooleanSupplier getIsFloorTapeVisible = () -> isFloorTapeVisible; +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java index d2af5c6c..b18073e4 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionFloorTapeAutoAlign.java @@ -19,27 +19,27 @@ */ public class VisionFloorTapeAutoAlign extends CommandGroup { - private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRF); + private final DoubleSupplier getNewY = () -> UpdateableData.y - UpdateableData.x / Math.tan(UpdateableData.betaRF); - private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, AutonConfig.NO_CLOSER), RobotMap.UpdateableData.x); + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(UpdateableData.y / 2, AutonConfig.NO_CLOSER), UpdateableData.x); - private final DoubleSupplier firstDrive = () -> Math.abs(RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRF)); + private final DoubleSupplier firstDrive = () -> Math.abs(UpdateableData.x / Math.sin(UpdateableData.betaRF)); public VisionFloorTapeAutoAlign() { super("VisionFloorTapeAutoAlign"); - addSequential(new RobotMap.UpdateableData.Update()); + addSequential(UpdateableData.update()); addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> getNewY.getAsDouble() <= AutonConfig.NO_CLOSER || getNewY.getAsDouble() >= RobotMap.UpdateableData.getY.getAsDouble() + () -> getNewY.getAsDouble() <= AutonConfig.NO_CLOSER || getNewY.getAsDouble() >= UpdateableData.getY.getAsDouble() )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaFloor, RobotMap.Component.navx, + addSequential(new ChassisTurn(RobotMap.Component.chassis, UpdateableData.getBetaFloor, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaFloor, RobotMap.Component.navx, + addSequential(UpdateableData.update()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, UpdateableData.getBetaFloor, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.getY, RobotMap.Component.drivePID)); + addSequential(UpdateableData.update()); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, UpdateableData.getY, RobotMap.Component.drivePID)); } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java index 7d622dac..84bc6b66 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/VisionTapeAutoAlign.java @@ -10,6 +10,7 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.usfirst.frc4904.robot.autonly.AutonConfig; +import org.usfirst.frc4904.robot.autonly.UpdateableData; /* ** If the current path of robot will intersect the tape in a specific area, then it will drive forwards. @@ -19,27 +20,27 @@ */ public class VisionTapeAutoAlign extends CommandGroup { - private final DoubleSupplier getNewY = () -> RobotMap.UpdateableData.y - RobotMap.UpdateableData.x / Math.tan(RobotMap.UpdateableData.betaRF); + private final DoubleSupplier getNewY = () -> UpdateableData.y - UpdateableData.x / Math.tan(UpdateableData.betaRF); - private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(RobotMap.UpdateableData.y / 2, AutonConfig.NO_CLOSER), RobotMap.UpdateableData.x); + private final DoubleSupplier firstTurn = () -> Math.atan2(Math.max(UpdateableData.y / 2, AutonConfig.NO_CLOSER), UpdateableData.x); - private final DoubleSupplier firstDrive = () -> Math.abs(RobotMap.UpdateableData.x / Math.sin(RobotMap.UpdateableData.betaRF)); + private final DoubleSupplier firstDrive = () -> Math.abs(UpdateableData.x / Math.sin(UpdateableData.betaRF)); public VisionTapeAutoAlign() { super("VisionTapeAutoAlign"); - addSequential(new RobotMap.UpdateableData.Update()); + addSequential(UpdateableData.update()); addSequential(new RunIf( new ChassisTurn(RobotMap.Component.chassis, firstTurn, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC), - () -> getNewY.getAsDouble() <= AutonConfig.NO_CLOSER || getNewY.getAsDouble() >= RobotMap.UpdateableData.getY.getAsDouble() + () -> getNewY.getAsDouble() <= AutonConfig.NO_CLOSER || getNewY.getAsDouble() >= UpdateableData.getY.getAsDouble() )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, firstDrive, RobotMap.Component.drivePID)); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRF, RobotMap.Component.navx, + addSequential(new ChassisTurn(RobotMap.Component.chassis, UpdateableData.getBetaRF, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getBetaRF, RobotMap.Component.navx, + addSequential(UpdateableData.update()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, UpdateableData.getBetaRF, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC)); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.getY, RobotMap.Component.drivePID)); + addSequential(UpdateableData.update()); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, UpdateableData.getY, RobotMap.Component.drivePID)); } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java index 55b063d2..4baeaf65 100644 --- a/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java +++ b/src/main/java/org/usfirst/frc4904/robot/commands/NotSplines.java @@ -4,13 +4,14 @@ import org.usfirst.frc4904.standard.commands.chassis.ChassisMoveDistance; import org.usfirst.frc4904.robot.RobotMap; import edu.wpi.first.wpilibj.command.CommandGroup; +import org.usfirst.frc4904.robot.autonly.UpdateableData; public class NotSplines extends CommandGroup { public NotSplines() { super("NotSplines"); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisTurn(RobotMap.Component.chassis, RobotMap.UpdateableData.getTheta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); - addSequential(new RobotMap.UpdateableData.Update()); - addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, RobotMap.UpdateableData.getDistance, RobotMap.Component.drivePID)); + addSequential(UpdateableData.update()); + addSequential(new ChassisTurn(RobotMap.Component.chassis, UpdateableData.getTheta, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); + addSequential(UpdateableData.update()); + addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, UpdateableData.getDistance, RobotMap.Component.drivePID)); } } \ No newline at end of file From f905d82781060a2ad8593bbfb27e2e5293ce9c6a Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Tue, 19 Feb 2019 11:19:51 -0800 Subject: [PATCH 25/30] fixed some requested changes --- .../java/org/usfirst/frc4904/robot/Robot.java | 22 ++--- .../org/usfirst/frc4904/robot/RobotMap.java | 14 ++-- .../frc4904/sovereignty/AligningCamera.java | 81 +++++++++++++++++++ .../frc4904/sovereignty/AligningSystem.java | 2 +- 4 files changed, 93 insertions(+), 26 deletions(-) create mode 100644 src/main/java/org/usfirst/frc4904/sovereignty/AligningCamera.java diff --git a/src/main/java/org/usfirst/frc4904/robot/Robot.java b/src/main/java/org/usfirst/frc4904/robot/Robot.java index c9a25e35..ff74254d 100644 --- a/src/main/java/org/usfirst/frc4904/robot/Robot.java +++ b/src/main/java/org/usfirst/frc4904/robot/Robot.java @@ -14,31 +14,20 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc4904.standard.CommandRobotBase; import org.usfirst.frc4904.standard.commands.chassis.ChassisMove; -import org.usfirst.frc4904.robot.humaninterface.drivers.NathanGain; -import org.usfirst.frc4904.robot.humaninterface.operators.DefaultOperator; -import edu.wpi.first.cameraserver.CameraServer;; - public class Robot extends CommandRobotBase { private RobotMap map = new RobotMap(); @Override public void initialize() { - driverChooser.addDefault(new NathanGain()); - operatorChooser.addDefault(new DefaultOperator()); - /* - works in shuffleboard with Logitech C270 - any amount of compression, 30 FPS, 160 x 120 - < 4 Mbps, often < 1 - */ - CameraServer.getInstance().startAutomaticCapture(); - + // driverChooser.addDefault(object); + // operatorChooser.addDefault(); } @Override public void teleopInitialize() { - teleopCommand = new ChassisMove(RobotMap.Component.chassis, driverChooser.getSelected()); - teleopCommand.start(); + // teleopCommand = new ChassisMove(RobotMap.Component.chassis, driverChooser.getSelected()); + // teleopCommand.start(); } @Override @@ -63,6 +52,5 @@ public void testInitialize() {} public void testExecute() {} @Override - public void alwaysExecute() { - } + public void alwaysExecute() {} } diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 7a42a8ff..f10cdf2b 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -14,8 +14,6 @@ import org.usfirst.frc4904.standard.custom.sensors.PDP; import org.usfirst.frc4904.standard.custom.PCMPort; -//import org.usfirst.frc4904.standard.custom.sensors.CANEncoder; -//import org.usfirst.frc4904.standard.custom.sensors.EncoderPair; import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX; import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; import org.usfirst.frc4904.standard.custom.sensors.NavX; @@ -63,12 +61,12 @@ public static class Robot { } public static class PID { public static class Drive { - public static final double P = 0.04; //Tune PID - public static final double I = 0.0; //Tune PID - public static final double D = -0.006; //Tune PID - public static final double F = 0.01; //Tune PID - public static final double tolerance = 4.5; //Tune PID - public static final double dTolerance = 3.0; //Tune PID + public static final double P = -1; //Tune PID + public static final double I = -1; //Tune PID + public static final double D = -1; //Tune PID + public static final double F = -1; //Tune PID + public static final double tolerance = -1; //Tune PID + public static final double dTolerance = -1; //Tune PID } } public static class Component { diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/AligningCamera.java b/src/main/java/org/usfirst/frc4904/sovereignty/AligningCamera.java new file mode 100644 index 00000000..c875c2e9 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/sovereignty/AligningCamera.java @@ -0,0 +1,81 @@ +package org.usfirst.frc4904.sovereignty; + + +import org.usfirst.frc4904.sovereignty.Fusible; +import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; +import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.networktables.NetworkTable; + +public class AligningCamera implements PIDSensor, Fusible { + public static final String TABLE_NAME = "Vision"; + public static final String FIELD_DEGREES = "degrees"; + public static final String FIELD_DISTANCE = "distance"; + public static final String FIELD_VISIBLE = "visible"; + public static final String FIELD_TRUSTABLE = "trustable"; + protected NetworkTable cameraTable; + private PIDSourceType sourceType; + + public AligningCamera(PIDSourceType sourceType, String cameraTableName) { + this.sourceType = sourceType; + cameraTable = NetworkTable.getTable(cameraTableName); + } + + public AligningCamera(PIDSourceType sourceType) { + this(sourceType, AligningCamera.TABLE_NAME); + } + + public AligningCamera() { + this(PIDSourceType.kDisplacement, AligningCamera.TABLE_NAME); + } + + public double getDegrees() { + return cameraTable.getNumber(AligningCamera.FIELD_DEGREES, Double.NaN); + } + + public double getDistance() { + return cameraTable.getNumber(AligningCamera.FIELD_DISTANCE, Double.NaN); + } + + public boolean isVisible() { + return cameraTable.getBoolean(AligningCamera.FIELD_VISIBLE, + !Double.isNaN(getDegrees()) && !Double.isNaN(getDistance())); + } + + @Override + public PIDSourceType getPIDSourceType() { + return sourceType; + } + + @Override + public double pidGet() { + try { + return pidGetSafely(); + } + catch (InvalidSensorException e) { + // This will never happen (famous last words) + return Double.NaN; + } + } + + @Override + public void setPIDSourceType(PIDSourceType sourceType) { + this.sourceType = sourceType; + } + + @Override + public Double getValue() { + return getDegrees(); + } + + @Override + public boolean trustable() { + return cameraTable.getBoolean(AligningCamera.FIELD_TRUSTABLE, + !Double.isNaN(getDegrees()) && !Double.isNaN(getDistance())); + } + + @Override + public double pidGetSafely() throws InvalidSensorException { + return getDegrees(); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/AligningSystem.java b/src/main/java/org/usfirst/frc4904/sovereignty/AligningSystem.java index 8ea9d6fe..c9a3848e 100755 --- a/src/main/java/org/usfirst/frc4904/sovereignty/AligningSystem.java +++ b/src/main/java/org/usfirst/frc4904/sovereignty/AligningSystem.java @@ -1,7 +1,7 @@ package org.usfirst.frc4904.sovereignty; -import org.usfirst.frc4904.robot.vision.AligningCamera; +import org.usfirst.frc4904.sovereignty.AligningCamera; import org.usfirst.frc4904.sovereignty.FusibleNavX.NavxMode; import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSourceType; From 069dca3f35869bdc18a56fddbdaf4aa1f973999c Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Fri, 1 Mar 2019 15:49:13 -0800 Subject: [PATCH 26/30] i guess some robotmap import stuff --- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index f10cdf2b..356a0bf4 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -3,6 +3,7 @@ import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick; import org.usfirst.frc4904.standard.custom.controllers.CustomXbox; import org.usfirst.frc4904.robot.humaninterface.HumanInterfaceConfig; +import org.usfirst.frc4904.sovereignty.FusibleNavX; import org.usfirst.frc4904.standard.subsystems.chassis.TankDriveShifting; import org.usfirst.frc4904.standard.subsystems.chassis.SolenoidShifters; import org.usfirst.frc4904.standard.subsystems.motor.Motor; @@ -16,7 +17,6 @@ import org.usfirst.frc4904.standard.custom.PCMPort; import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX; import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; -import org.usfirst.frc4904.standard.custom.sensors.NavX; @@ -84,7 +84,7 @@ public static class Component { //public static EncoderPair chassisEncoders; public static CustomPIDController chassisTurnMC; public static CustomPIDController drivePID; - public static NavX navx; + public static FusibleNavX navx; } public static class NetworkTables { From fc45e5726bff5b7f84690e410312c09ee1fcadd6 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 3 Mar 2019 15:06:22 -0800 Subject: [PATCH 27/30] standard stuff --- src/main/java/org/usfirst/frc4904/standard | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index e3b7f285..274e744d 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit e3b7f2856ff09d9af4b4fbfb703c6c6bfcfc00c3 +Subproject commit 274e744da022846ed04bde85ccd0191966d0efc4 From 33489648f6f42fafb03a20b6e0f5146c583e7185 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 3 Mar 2019 15:08:20 -0800 Subject: [PATCH 28/30] standard stuff --- src/main/java/org/usfirst/frc4904/standard | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/usfirst/frc4904/standard b/src/main/java/org/usfirst/frc4904/standard index 274e744d..e3b7f285 160000 --- a/src/main/java/org/usfirst/frc4904/standard +++ b/src/main/java/org/usfirst/frc4904/standard @@ -1 +1 @@ -Subproject commit 274e744da022846ed04bde85ccd0191966d0efc4 +Subproject commit e3b7f2856ff09d9af4b4fbfb703c6c6bfcfc00c3 From 3373ca49b2e97f9f3ed443d2a81eb2dfc57cd9c5 Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 3 Mar 2019 15:10:41 -0800 Subject: [PATCH 29/30] removed sovereignty --- .../org/usfirst/frc4904/robot/RobotMap.java | 5 +- .../frc4904/sovereignty/AligningCamera.java | 81 --------------- .../frc4904/sovereignty/AligningSystem.java | 75 -------------- .../frc4904/sovereignty/FusedSensor.java | 98 ------------------- .../usfirst/frc4904/sovereignty/Fusible.java | 10 -- .../frc4904/sovereignty/FusibleNavX.java | 72 -------------- .../frc4904/sovereignty/TrimCommand.java | 40 -------- .../frc4904/sovereignty/Trimmable.java | 20 ---- .../sovereignty/TrimmablePIDController.java | 63 ------------ .../TrimmablePIDSensorWrapper.java | 61 ------------ .../sovereignty/strategies/GearAlign.java | 23 ----- .../sovereignty/strategies/VisionTurn.java | 21 ---- 12 files changed, 2 insertions(+), 567 deletions(-) delete mode 100644 src/main/java/org/usfirst/frc4904/sovereignty/AligningCamera.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/AligningSystem.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/FusedSensor.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/Fusible.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/FusibleNavX.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/TrimCommand.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/Trimmable.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/TrimmablePIDController.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/TrimmablePIDSensorWrapper.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/strategies/GearAlign.java delete mode 100755 src/main/java/org/usfirst/frc4904/sovereignty/strategies/VisionTurn.java diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 356a0bf4..4506ce24 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -3,7 +3,6 @@ import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick; import org.usfirst.frc4904.standard.custom.controllers.CustomXbox; import org.usfirst.frc4904.robot.humaninterface.HumanInterfaceConfig; -import org.usfirst.frc4904.sovereignty.FusibleNavX; import org.usfirst.frc4904.standard.subsystems.chassis.TankDriveShifting; import org.usfirst.frc4904.standard.subsystems.chassis.SolenoidShifters; import org.usfirst.frc4904.standard.subsystems.motor.Motor; @@ -12,7 +11,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; - +import org.usfirst.frc4904.standard.custom.sensors.NavX; import org.usfirst.frc4904.standard.custom.sensors.PDP; import org.usfirst.frc4904.standard.custom.PCMPort; import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX; @@ -84,7 +83,7 @@ public static class Component { //public static EncoderPair chassisEncoders; public static CustomPIDController chassisTurnMC; public static CustomPIDController drivePID; - public static FusibleNavX navx; + public static NavX navx; } public static class NetworkTables { diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/AligningCamera.java b/src/main/java/org/usfirst/frc4904/sovereignty/AligningCamera.java deleted file mode 100644 index c875c2e9..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/AligningCamera.java +++ /dev/null @@ -1,81 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -import org.usfirst.frc4904.sovereignty.Fusible; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; -import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.networktables.NetworkTable; - -public class AligningCamera implements PIDSensor, Fusible { - public static final String TABLE_NAME = "Vision"; - public static final String FIELD_DEGREES = "degrees"; - public static final String FIELD_DISTANCE = "distance"; - public static final String FIELD_VISIBLE = "visible"; - public static final String FIELD_TRUSTABLE = "trustable"; - protected NetworkTable cameraTable; - private PIDSourceType sourceType; - - public AligningCamera(PIDSourceType sourceType, String cameraTableName) { - this.sourceType = sourceType; - cameraTable = NetworkTable.getTable(cameraTableName); - } - - public AligningCamera(PIDSourceType sourceType) { - this(sourceType, AligningCamera.TABLE_NAME); - } - - public AligningCamera() { - this(PIDSourceType.kDisplacement, AligningCamera.TABLE_NAME); - } - - public double getDegrees() { - return cameraTable.getNumber(AligningCamera.FIELD_DEGREES, Double.NaN); - } - - public double getDistance() { - return cameraTable.getNumber(AligningCamera.FIELD_DISTANCE, Double.NaN); - } - - public boolean isVisible() { - return cameraTable.getBoolean(AligningCamera.FIELD_VISIBLE, - !Double.isNaN(getDegrees()) && !Double.isNaN(getDistance())); - } - - @Override - public PIDSourceType getPIDSourceType() { - return sourceType; - } - - @Override - public double pidGet() { - try { - return pidGetSafely(); - } - catch (InvalidSensorException e) { - // This will never happen (famous last words) - return Double.NaN; - } - } - - @Override - public void setPIDSourceType(PIDSourceType sourceType) { - this.sourceType = sourceType; - } - - @Override - public Double getValue() { - return getDegrees(); - } - - @Override - public boolean trustable() { - return cameraTable.getBoolean(AligningCamera.FIELD_TRUSTABLE, - !Double.isNaN(getDegrees()) && !Double.isNaN(getDistance())); - } - - @Override - public double pidGetSafely() throws InvalidSensorException { - return getDegrees(); - } -} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/AligningSystem.java b/src/main/java/org/usfirst/frc4904/sovereignty/AligningSystem.java deleted file mode 100755 index c9a3848e..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/AligningSystem.java +++ /dev/null @@ -1,75 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -import org.usfirst.frc4904.sovereignty.AligningCamera; -import org.usfirst.frc4904.sovereignty.FusibleNavX.NavxMode; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; - -public class AligningSystem implements PIDSource, Fusible { - public static enum AligningState { - IDLE, ACTIVE; - } - public static final double ANGLE_TOLERANCE = 2; - protected final FusedSensor sensorSystem; - protected final AligningCamera camera; - protected final FusibleNavX navX; - protected AligningState state; - protected PIDSourceType pidSourceType; - - public AligningSystem(AligningCamera camera, FusibleNavX navX) { - sensorSystem = new FusedSensor(camera, navX); - this.camera = camera; - this.navX = navX; - state = AligningState.IDLE; - } - - public void start() { - state = AligningState.ACTIVE; - navX.setMode(NavxMode.ALIGNMENT); - navX.setTargetAngle(camera.getDegrees()); - } - - public void cancel() { - state = AligningState.IDLE; - navX.setMode(NavxMode.STANDARD); - } - - public double getDegrees() { - Fusible sensor = sensorSystem.getActiveSensor(); - Double value = sensor.getValue(); - if (value == null) { - return 0.0d; - } - if (sensor instanceof AligningCamera) { - navX.setTargetAngle(value); - navX.zeroRelativeOffset(); - } - return value; - } - - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - pidSourceType = pidSource; - } - - @Override - public PIDSourceType getPIDSourceType() { - return pidSourceType; - } - - @Override - public double pidGet() { - return getDegrees(); - } - - @Override - public Double getValue() { - return sensorSystem.getValue(); - } - - @Override - public boolean trustable() { - return sensorSystem.trustable(); - } -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/FusedSensor.java b/src/main/java/org/usfirst/frc4904/sovereignty/FusedSensor.java deleted file mode 100755 index a611b1aa..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/FusedSensor.java +++ /dev/null @@ -1,98 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; - -public class FusedSensor implements Fusible { - protected final Fusible primarySensor; - protected final Fusible fallbackSensor; - protected final Fusible lastResortSensor; - - @SafeVarargs - public FusedSensor(T defaultValue, Fusible primarySensor, Fusible secondarySensor, Fusible... sensors) { - Fusible lastSensor = sensors[sensors.length - 1]; - for (int i = sensors.length - 2; i >= 0; i--) { - lastSensor = new FusedSensor<>(sensors[i], lastSensor); - } - lastSensor = new FusedSensor<>(secondarySensor, lastSensor); - this.primarySensor = primarySensor; - this.fallbackSensor = lastSensor; - this.lastResortSensor = new Fusible() { - @Override - public T getValue() { - return defaultValue; - } - - @Override - public boolean trustable() { - return false; - } - - @Override - public String getName() { - return "LastResortSensor"; - } - }; - } - - public FusedSensor(T defaultValue, Fusible primarySensor, Fusible fallbackSensor) { - this.primarySensor = primarySensor; - this.fallbackSensor = fallbackSensor; - this.lastResortSensor = new Fusible() { - @Override - public T getValue() { - return defaultValue; - } - - @Override - public boolean trustable() { - return false; - } - - @Override - public String getName() { - return "LastResortSensor"; - } - }; - } - - public FusedSensor(Fusible primarySensor, Fusible fallbackSensor) { - this(null, primarySensor, fallbackSensor); - } - - protected Fusible getActiveSensor() { - if (primarySensor.trustable()) { - return primarySensor; - } else if (fallbackSensor.trustable()) { - return fallbackSensor; - } - return lastResortSensor; - } - - public T getValueDangerously() throws InvalidSensorException { - Fusible activeSensor = getActiveSensor(); - T value = activeSensor.getValue(); - if (value == null) { - throw new InvalidSensorException(activeSensor.getName() + " returned null."); - } - return value; - } - - @Override - public T getValue() { - T value = null; - try { - value = getValueDangerously(); - } - catch (InvalidSensorException ex) { - LogKitten.ex(ex); - } - return value; - } - - @Override - public boolean trustable() { - return primarySensor.trustable() || fallbackSensor.trustable(); - } -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/Fusible.java b/src/main/java/org/usfirst/frc4904/sovereignty/Fusible.java deleted file mode 100755 index 9edcbebc..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/Fusible.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -import org.usfirst.frc4904.standard.custom.Nameable; - -public interface Fusible extends Nameable { - T getValue(); - - boolean trustable(); -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/FusibleNavX.java b/src/main/java/org/usfirst/frc4904/sovereignty/FusibleNavX.java deleted file mode 100755 index 44d67047..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/FusibleNavX.java +++ /dev/null @@ -1,72 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -import org.usfirst.frc4904.standard.custom.sensors.NavX; -import edu.wpi.first.wpilibj.SerialPort.Port; - -public class FusibleNavX extends NavX implements Fusible { - public static enum NavxMode { - STANDARD, ALIGNMENT; - } - protected double relativeOffset = 0; - protected double targetAngle = 0; - protected NavxMode mode; - - public FusibleNavX(Port port) { - super(port); - mode = NavxMode.STANDARD; - } - - @Override - public Double getValue() { - switch (mode) { - default: - throw new IllegalStateException("NavX must have a NavXMode state"); - case STANDARD: - return getAngle(); - case ALIGNMENT: - return targetAngle - (getAngle() - relativeOffset); - } - } - - @Override - public boolean trustable() { - return isConnected(); - } - - public void setMode(NavxMode mode) { - this.mode = mode; - } - - public void setModeAlignment() { - setMode(NavxMode.ALIGNMENT); - zeroRelativeOffset(); - resetTargetAngle(); - } - - public void setModeStandard() { - setMode(NavxMode.STANDARD); - resetRelativeOffset(); - resetTargetAngle(); - } - - public void zeroRelativeOffset() { - setRelativeOffset(getAngle()); - } - - public void setRelativeOffset(double relativeOffset) { - this.relativeOffset = relativeOffset; - } - - public void resetRelativeOffset() { - relativeOffset = 0; - } - - public void setTargetAngle(double targetAngle) { - this.targetAngle = targetAngle; - } - - public void resetTargetAngle() { - targetAngle = 0; - } -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/TrimCommand.java b/src/main/java/org/usfirst/frc4904/sovereignty/TrimCommand.java deleted file mode 100755 index 6add0e20..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/TrimCommand.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -import edu.wpi.first.wpilibj.command.Command; - -public class TrimCommand extends Command { - public static enum TrimDirection { - UP(true), DOWN(false), LEFT(false), RIGHT(true); - private boolean positive; - - private TrimDirection(boolean positive) { - this.positive = positive; - } - - public boolean getPositive() { - return positive; - } - } - private final Trimmable trimmable; - private final boolean positive; - - public TrimCommand(Trimmable trimmable, TrimDirection direction) { - this(trimmable, direction.getPositive()); - } - - public TrimCommand(Trimmable trimmable, boolean positive) { - this.trimmable = trimmable; - this.positive = positive; - } - - @Override - public void initialize() { - trimmable.adjustTrim(positive); - } - - @Override - protected boolean isFinished() { - return true; - } -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/Trimmable.java b/src/main/java/org/usfirst/frc4904/sovereignty/Trimmable.java deleted file mode 100755 index 310abab2..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/Trimmable.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -public interface Trimmable { - void setTrimIncrement(double increment); - - double getTrimIncrement(); - - void setTrim(double trim); - - double getTrim(); - - default void adjustTrim(boolean positive) { - adjustTrim(positive ? getTrimIncrement() : -getTrimIncrement()); - }; - - default void adjustTrim(double change) { - setTrim(getTrim() + change); - }; -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/TrimmablePIDController.java b/src/main/java/org/usfirst/frc4904/sovereignty/TrimmablePIDController.java deleted file mode 100755 index 96c6d0dd..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/TrimmablePIDController.java +++ /dev/null @@ -1,63 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; -import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; -import edu.wpi.first.wpilibj.PIDSource; - -public class TrimmablePIDController extends CustomPIDController implements Trimmable { - private double trimValue = 0.0; - private double trimIncrement = 0.0; - protected double trueSetpoint = 0.0; - - public TrimmablePIDController(double P, double I, double D, double F, PIDSensor sensor) { - super(P, I, D, F, sensor); - } - - public TrimmablePIDController(double P, double I, double D, double F, PIDSource source) { - super(P, I, D, F, source); - } - - public TrimmablePIDController(double P, double I, double D, PIDSensor sensor) { - super(P, I, D, sensor); - } - - public TrimmablePIDController(double P, double I, double D, PIDSource source) { - super(P, I, D, source); - } - - public TrimmablePIDController(PIDSensor sensor) { - super(sensor); - } - - public TrimmablePIDController(PIDSource source) { - super(source); - } - - @Override - public void setSetpoint(double setpoint) { - trueSetpoint = setpoint; - super.setSetpoint(trueSetpoint + trimValue); - } - - @Override - public void setTrimIncrement(double trimIncrement) { - this.trimIncrement = trimIncrement; - } - - @Override - public double getTrimIncrement() { - return trimIncrement; - } - - @Override - public void setTrim(double trimValue) { - this.trimValue = trimValue; - super.setSetpoint(trueSetpoint + trimValue); - } - - @Override - public double getTrim() { - return trimValue; - } -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/TrimmablePIDSensorWrapper.java b/src/main/java/org/usfirst/frc4904/sovereignty/TrimmablePIDSensorWrapper.java deleted file mode 100755 index 0aaf4383..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/TrimmablePIDSensorWrapper.java +++ /dev/null @@ -1,61 +0,0 @@ -package org.usfirst.frc4904.sovereignty; - - -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; - -public class TrimmablePIDSensorWrapper implements PIDSensor, Trimmable { - private double trimIncrement = 0.0; - private double trimValue = 0.0; - private final PIDSensor sensor; - - public TrimmablePIDSensorWrapper(PIDSensor sensor) { - this.sensor = sensor; - } - - public TrimmablePIDSensorWrapper(PIDSource source) { - this(new PIDSensor.PIDSourceWrapper(source)); - } - - @Override - public void setPIDSourceType(PIDSourceType pidSource) { - sensor.setPIDSourceType(pidSource); - } - - @Override - public PIDSourceType getPIDSourceType() { - return sensor.getPIDSourceType(); - } - - @Override - public double pidGet() { - return sensor.pidGet() + trimValue; - } - - @Override - public double pidGetSafely() throws InvalidSensorException { - return sensor.pidGetSafely() + trimValue; - } - - @Override - public void setTrimIncrement(double trimIncrement) { - this.trimIncrement = trimIncrement; - } - - @Override - public double getTrimIncrement() { - return trimIncrement; - } - - @Override - public void setTrim(double trimValue) { - this.trimValue = trimValue; - } - - @Override - public double getTrim() { - return trimValue; - } -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/strategies/GearAlign.java b/src/main/java/org/usfirst/frc4904/sovereignty/strategies/GearAlign.java deleted file mode 100755 index a2b1e928..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/strategies/GearAlign.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.usfirst.frc4904.sovereignty.strategies; - - -import org.usfirst.frc4904.robot.RobotMap; -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.commands.KittenCommand; -import org.usfirst.frc4904.standard.commands.RunFor; -import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.command.WaitCommand; - -public class GearAlign extends CommandGroup { - protected static final double PAUSE_BETWEEN_ALIGNMENT_CYCLES = 0.4; - protected static final int ALIGNMENT_CYCLES = 2; - protected static final int MAX_CYCLE_TIME_SECONDS = 1; - - public GearAlign() { - for (int i = 0; i < GearAlign.ALIGNMENT_CYCLES; i++) { - addSequential(new WaitCommand(GearAlign.PAUSE_BETWEEN_ALIGNMENT_CYCLES)); - addSequential(new RunFor(new VisionTurn(RobotMap.Component.gearAlignCamera), GearAlign.MAX_CYCLE_TIME_SECONDS)); - } - addSequential(new KittenCommand("Done gear aligning (I did my best)", LogKitten.LEVEL_VERBOSE)); - } -} diff --git a/src/main/java/org/usfirst/frc4904/sovereignty/strategies/VisionTurn.java b/src/main/java/org/usfirst/frc4904/sovereignty/strategies/VisionTurn.java deleted file mode 100755 index 55d84389..00000000 --- a/src/main/java/org/usfirst/frc4904/sovereignty/strategies/VisionTurn.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.usfirst.frc4904.sovereignty.strategies; - - -import org.usfirst.frc4904.robot.RobotMap; -import org.usfirst.frc4904.robot.vision.AligningCamera; -import org.usfirst.frc4904.standard.commands.chassis.ChassisTurn; - -public class VisionTurn extends ChassisTurn { - AligningCamera camera; - - public VisionTurn(AligningCamera camera) { - super(RobotMap.Component.chassis, 0.0, RobotMap.Component.navx, RobotMap.Component.chassisTurnMC); - this.camera = camera; - } - - @Override - protected void initialize() { - super.initialize(); - initialAngle -= camera.getDegrees(); - } -} From 2c9636ca1a7e690e0574cdb722f855ae344e590f Mon Sep 17 00:00:00 2001 From: Evan Steirman Date: Sun, 3 Mar 2019 16:09:20 -0800 Subject: [PATCH 30/30] renamed boringauton to autoalign --- .../{BoringAuton.java => AutoAlign.java} | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) rename src/main/java/org/usfirst/frc4904/robot/autonly/{BoringAuton.java => AutoAlign.java} (67%) diff --git a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java b/src/main/java/org/usfirst/frc4904/robot/autonly/AutoAlign.java similarity index 67% rename from src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java rename to src/main/java/org/usfirst/frc4904/robot/autonly/AutoAlign.java index 3bb104c0..92060655 100644 --- a/src/main/java/org/usfirst/frc4904/robot/autonly/BoringAuton.java +++ b/src/main/java/org/usfirst/frc4904/robot/autonly/AutoAlign.java @@ -10,28 +10,28 @@ import org.usfirst.frc4904.standard.commands.RunIf; import org.usfirst.frc4904.standard.commands.RunIfElse; -public class BoringAuton extends CommandGroup { +public class AutoAlign extends CommandGroup { - public BoringAuton(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { - super("BoringAuton"); + public AutoAlign(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + super("AutoAlign"); addSequential(new RunIfElse( - new BoringAutonPosX(betaSupplier, xSupplier, ySupplier), //the left of the robot is towards the positive x direction - new BoringAutonNegX(betaSupplier, xSupplier, ySupplier), + new AutoAlignPosX(betaSupplier, xSupplier, ySupplier), //the left of the robot is towards the positive x direction + new AutoAlignNegX(betaSupplier, xSupplier, ySupplier), () -> xSupplier.getAsDouble() >= 0 )); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, () -> ySupplier.getAsDouble(), RobotMap.Component.drivePID)); } - public class BoringAutonPosX extends CommandGroup { - public BoringAutonPosX(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { - super("BoringAutonPosX"); //chassisturn and beta are positive to the right + public class AutoAlignPosX extends CommandGroup { + public AutoAlignPosX(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + super("AutoAlignPosX"); //chassisturn and beta are positive to the right addSequential(new ChassisTurn(RobotMap.Component.chassis, () -> 90 + betaSupplier.getAsDouble(), RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, () -> xSupplier.getAsDouble(), RobotMap.Component.drivePID)); addSequential(new ChassisTurn(RobotMap.Component.chassis, -90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); } } - public class BoringAutonNegX extends CommandGroup { - public BoringAutonNegX(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { - super("BoringAutonNegX"); + public class AutoAlignNegX extends CommandGroup { + public AutoAlignNegX(DoubleSupplier betaSupplier, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + super("AutoAlignNegX"); addSequential(new ChassisTurn(RobotMap.Component.chassis, () -> -90 + betaSupplier.getAsDouble(), RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC)); addSequential(new ChassisMoveDistance(RobotMap.Component.chassis, () -> -xSupplier.getAsDouble(), RobotMap.Component.drivePID)); addSequential(new ChassisTurn(RobotMap.Component.chassis, 90, RobotMap.Component.navx, null, RobotMap.Component.chassisTurnMC));