diff --git a/AAAScripts/scripts.txt b/AAAScripts/scripts.txt new file mode 100644 index 0000000..b12474c --- /dev/null +++ b/AAAScripts/scripts.txt @@ -0,0 +1,389 @@ +# Naming Conventions +# ------------------- +# 1. Robot's start position. R = right, C = center, L = left. +# 2. Switch information. R = going for the right plate, L = going for the left plate, x = not going for switch +# 3. Scale information. R = going for the right plate, L = going for the left plate, x = not going for scale +# 4. Exchange. E = going for the exchange, x = not going for exchange + +# 3.3(l) x 2.9(w) + +#Cross Auto Line +Rxxx: + moveto (0, 120) #Drive past auto line + +#------------------------------------------------- + +#Switch +RRxx: + moveto (0, 128.4) (-42.6, 128.4) -90 #Drive directly forward to switch and then ram into switch + switch + +RLxx: + moveto (0, 188.4) (-210.6, 188.4) (-210.6, 128.4) (-186.6, 128.4) 90 #Curve around switch and then "ram" into switch + switch + +#------------------------------------------------- + +#Scale +RxRx: + moveto (0, 284.4) (-24.6, 284.4) -90 #Drive directly forward and place at initial horizontal to scale + scale + +RxLx: + moveto (0, 188.4) (-246.6, 188.4) (-246.6, 284.4) (-192.6, 284.4) 90 #Curve around switch and drive to scale + scale + +#------------------------------------------------- + +#Exchange +RxxE + moveto (0, 188.4) (-210.6, 188.4) (-210.6, 44.4) (-123.6, 44.4) (-123.6, -39.6) 180 #Curve around switch for baseline #then go to exchange + exchange + +#------------------------------------------------- + +#Switch and Scale +RRRx: + moveto (0, 128.4) (-41.4, 128.4) 90 #Drive directly forward then turn into switch + switch + moveto (0, 128.4) #Back out from switch + moveto (0, 80.4) (-9, 80.4) #Drive toward cubes + intake + moveto (0, 80.4) #Back out from cubes + moveto (0, 284.4) (-24.6, 284.4) -90 #Drive toward scale + scale + +RRLx: + moveto (0, 128.4) (-41.4, 128.4) 90 #Drive directly forward then turn into switch + switch + moveto (0, 128.4) #Back out from switch + moveto (0, 80.4) (-9, 80.4) -90 #Drive toward cubes + intake + moveto (0, 80.4) #Back out from cubes + moveto (0, 188.4) (-246.6, 188.4) (-246.6, 284.4) (-192.6, 284.4) 90 #Curve around switch + place at init distance to scale + scale + +RLRx: + moveto (0, 284.4) (-24.6, 284.4) -90 #Drive directly forward and place at initial horizontal to scale + scale + moveto (0, 284.4) #Back out from switch + moveto (0, 80.4) (-108, 80.4) -90 #Drive toward cubes + intake + moveto (0, 80.4) #Back out from cubes + moveto (0, 188.4) (-210.6,188.4) (-210.6, 128.4) (-186.6, 128.4) #Drive toward switch + switch + +RLLx: + moveto (0, 188.4) (-210.6,188.4) (-210.6, 128.4) (-186.6, 128.4) -90 #Curve around switch and then "ram" into switch + switch + moveto (-246.6, 128.4) #Back out from switch + moveto (-246.6, 80.4) (-144, 80.4) #Drive toward cubes + intake + moveto (-246.6, 80.4) #Back out from cubes + moveto (-246.6, 284.4) (-192.6, 284.4) 90 #Curve around switch + place at init distance to scale + scale + +#------------------------------------------------- + +#Switch and Exchange +RRxE: + moveto (0, 128.4) (-41.4, 128.4) 90 #Drive directly forward then turn into switch + switch + moveto (-192, 128.4) #Back out from switch + moveto (-192, 80.4) (-144, 80.4) #Drive toward cubes + #intake + moveto (0, 80.4) #Back out from cubes + moveto (0, 188.4) (-210.6, 188.4) (-210.6, 44.4) (-123.6, 44.4) (-123.6, -39.6) 180 #Curve around switch for baseline #then go to + exchange + +RLxE: + moveto (0, 188.4) (-210.6,188.4) (-210.6, 128.4) (-186.6, 128.4) 180 #Curve around switch and then "ram" into switch + switch + moveto (-246.6, 128.4) #Back out from switch + moveto (-246.6, 80.4) (-12, 80.4) #Drive toward cubes + #intake + moveto (-246.6, 80.4) #Back out from cubes + moveto (-246.6, 44.4) (-123.6, 44.4) (-123.6, -39.6) #Drive toward exchange + exchange + +# Naming Conventions +# ------------------- +# 1. Robot's start position. R = right, C = center, L = left. +# 2. Switch information. R = going for the right plate, L = going for the left plate, x = not going for switch +# 3. Scale information. R = going for the right plate, L = going for the left plate, x = not going for scale +# 4. Exchange. E = going for the exchange, x = not going for exchange + +# 3.3(l) x 2.9(w) + +#Cross Auto Line +Lxxx: + moveto (0, 120) #Drive past auto line + +#------------------------------------------------- + +#Switch +LLxx: + moveto (0, 129.8) (42.6, 129.8) -90 #Drive directly forward to switch and then ram into switch + switch + +LRxx: + moveto (0, 188.4) (210.6,188.4) (210.6, 129.8) (186.6, 129.8) 90 #Curve around switch and then "ram" into switch + switch + +#------------------------------------------------- + +#Scale +LxLx: + moveto (0, 284.4) (24.6, 284.4) -90 #Drive directly forward and place at initial horizontal to scale + scale + +LxRx: + moveto (0, 188.4) (246.6, 188.4) (246.6, 284.4) (16.05, 284.4) 90 #Curve around switch + place at init distance to scale + scale + +#------------------------------------------------- + +#Exchange +LxxE + moveto (0, 188.4) (210.6, 188.4) (210.6, 3.7) (124.8, 3.7) (124.8, -3.3) 180 #Curve around switch for baseline then go to exchange + exchange + +#------------------------------------------------- + +#Switch and Scale +LRRx: + moveto (0, 188.4) (210.6,188.4) (210.6, 129.8) (186.6, 129.8) -90 #Curve around switch and then "ram" into switch + switch + moveto (246.6, 129.8) #Back out from switch + moveto (246.6, 80.4) (12, 80.4) #Drive toward cubes + intake + moveto (246.6, 80.4) #Back out from cubes + moveto (246.6, 284.4) (16.05, 284.4) 90 #Curve around switch + place at init distance to scale + scale + +LRLx: + moveto (0, 284.4) (24.6, 284.4) -90 #Drive directly forward and place at initial horizontal to scale + scale + moveto (0, 284.4) #Back out from switch + moveto (0, 80.4) (108, 80.4) -90 #Drive toward cubes + intake + moveto (0, 80.4) #Back out from cubes + moveto (0, 188.4) (210.6,188.4) (210.6, 129.8) (186.6, 129.8) #Drive toward switch + switch + +LLRx: + moveto (0, 129.8) (42.6, 129.8) 90 #Drive directly forward then turn into switch + switch + moveto (0, 129.8) #Back out from switch + moveto (0, 80.4) (108, 80.4) -90 #Drive toward cubes + intake + moveto (0, 80.4) #Back out from cubes + moveto (0, 188.4) (246.6, 188.4) (246.6, 284.4) (192.6, 284.4) 90 #Curve around switch and drive to scale + scale + +LLLx: + moveto (0, 129.8) (42.6, 129.8) 90 #Drive directly forward then turn into switch + switch + moveto (0, 129.8) #Back out from switch + moveto (0, 80.4) (108, 80.4) #Drive toward cubes + intake + moveto (0, 80.4) #Back out from cubes + moveto (0, 284.4) (24.6, 284.4) -90 #Drive toward scale + scale + +#------------------------------------------------- + +#Switch and Exchange +LRxE: + moveto (0, 188.4) (210.6,188.4) (210.6, 129.8) (186.6, 129.8) 180 #Curve around switch and then "ram" into switch + switch + moveto (246.6, 129.8) #Back out from switch + moveto (246.6, 80.4) (12, 80.4) #Drive toward cubes + intake + moveto (246.6, 80.4) #Back out from cubes + moveto (246.6, 3.7) (124.8, 3.7) (124.8, -39.6) #Drive toward exchange + exchange + +LLxE: + moveto (0, 129.8) (42.6, 129.8) 90 #Drive directly forward then turn into switch + switch + moveto (17, 129.8) #Back out from switch + moveto (17, 80.4) (12, 80.4) #Drive toward cubes + intake + moveto (0, 80.4) #Back out from cubes + moveto (0, 188.4) (210.6, 188.4) (210.6, 44.4) (124.8, 44.4) (124.8, -39.6) 180 #Go to exchange + exchange + +# moveto +# turn +# move +# switch +# scale +# exchange +# wait +# intake +# jump +# end + + + +# 1. Robot's start position. R = right, C = center, L = left. +# 2. Switch information. R = going for the right plate, L = going for the left plate, x = not going for # switch +# 3. Scale information. R = going for the right plate, L = going for the left plate, x = not going for # scale +# 4. Exchange. E = going for the exchange, x = not going for exchange + + +# Location of Robot: rightmost next to exchange + +# Cross Auto Line +Cxxx: +moveto (48,80.375) #cross baseline + + +# Switch +CRxx: +moveto (0,60) 90 #5 feet forward, turn 90 +moveto (48,60) -90 #4 feet right,turn -90 +moveto(48,104.375) #7 feet forward until against switch enclosure, compensating for robot length +switch #deploy switch + +CLxx: +moveto (0,60) -90 #5 feet forward, turn -90 +moveto (-48,60) 90 #4 feet left,turn 90 +moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +switch #deploy switch + + +# Scale +CxRx: +moveto (0,60) 90 #5 feet forward, turn 90 +moveto (114,60) -90 #9.5 feet right, turn -90 +move 224.375 #22 feet forward, compensating for robot length +turn -90 #turn to face the switch +scale #deploy scale + + +CxLx: +moveto (0,60) -90 #5 feet forward, turn -90 +moveto (-90,60) 90 #7.5 feet left, turn 90 +move 224.375 #22 feet forward, compensating for robot length +turn 90 #turn to face the switch +scale #deploy scale + + +# Switch and Scale +CRLx: +moveto (0,60) 90 #5 feet forward, turn 90 +moveto (48,60) -90 #4 feet right,turn -90 +moveto(48,104.375) #7 feet forward until against switch enclosure +switch #deploy switch +move -39 #move 39 inches back +turn -90 #turn -90 degrees +move 48 #move 4 feet forward +intake #intake +turn -90 #turn -90 +move 45 # move 45 in. forward +turn -90 #turn 90 +moveto (-90,60) 90 #7.5 feet left, turn 90 +move 264 #22 feet forward +turn 90 #turn to face the switch +scale #deploy scale2 + + + +CRRx: +moveto (0,60) 90 #5 feet forward, turn 90 +moveto (48,60) -90 #4 feet right,turn -90 +moveto(48,104.375) #7 feet forward until against switch enclosure +switch #deploy switch +move -39 #move 39 inches back +turn -90 #turn -90 degrees +move 48 #move 4 feet forward +intake +turn -90 #turn -90nyu +move 45 # move 45 in. forward +turn -90 #turn -90 degrees +moveto (114, 60) -90 #9.5 feet right, turn -90 +move 264 #22 feet forward +turn -90 #turn to face the switch +scale #deploy scale + + + +CLRx: +moveto (0,60) -90 #5 feet forward, turn -90 +moveto (-48,60) 90 #4 feet left,turn 90 +moveto(-48,104.375) #7 feet forward until against switch enclosure +switch #deploy switch +move -39 #move 39 inches back +turn -90 #turn -90 degrees +move 48 #move 4 feet forward +intake #intake +turn 90 #turn 90 +move 45 # move 45 in. forward +turn 90 #turn 90 degrees +moveto (114, 60) -90 #9.5 feet right, turn -90 +move 264 #22 feet forward +turn -90 #turn to face the switch +scale #deploy scale + + +CLLx: +moveto (0,60) -90 #5 feet forward, turn -90 +moveto (-48,60) 90 #4 feet left,turn 90 +moveto(-48,104.375) #7 feet forward until against switch enclosure +switch #deploy switch +move -39 #move 39 inches back +turn 90 #turn 90 degrees +move 48 #move 4 feet forward +intake #intake +turn 90 #turn -90 +move 45 # move 45 in. forward +turn 90 #turn 90 +moveto (-90,60) -90 #7.5 feet right, turn -90 +move 264 #22 feet forward +turn 90 #turn to face the switch +scale #deploy scale2 + + + + +# Switch and Exchange +CRxE: +moveto (0,60) 90 #5 feet forward, turn 90 +moveto (48,60) -90 #4 feet right,turn -90 +moveto(48,104.375) #7 feet forward until against switch enclosure, compensating for robot length +switch #deploy switch +move -24 # 2 feet back +turn -90 +move 12 # 1 ft forward +intake +turn -90 +move 72 #move 6 feet forwards +turn 90 +move 60 #move 5 feet forward +turn -90 #turn to face the exchange +exchange + +CLxE: +moveto (0,60) -90 #5 feet forward, turn -90 +moveto (-48,60) 90 #3 feet left,turn 90 +moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +switch #deploy switch +move -24 +turn 90 +move 24 +intake +turn 90 +move -84 #move 7 feet back +turn 180 #turn 180 degrees to face the exchange +exchange + +# Exchange +CxxE: +moveto (0,60) -90 #5 feet forward, turn -90 +moveto (-48,60) 90 #3 feet left,turn 90 +moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +move -96 #move 8 feet back +turn 180 #turn 180 degrees to face the exchange +exchange \ No newline at end of file diff --git a/Robot2018/lib/.gitignore b/Robot2018/lib/.gitignore new file mode 100644 index 0000000..d8d8bd1 --- /dev/null +++ b/Robot2018/lib/.gitignore @@ -0,0 +1 @@ +/User_Libraries.properties diff --git a/Robot2018/lib/User_Libraries.properties b/Robot2018/lib/User_Libraries.properties new file mode 100644 index 0000000..5bce966 --- /dev/null +++ b/Robot2018/lib/User_Libraries.properties @@ -0,0 +1,2 @@ +#Sat Feb 17 17:04:37 PST 2018 +C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4 diff --git a/Robot2018/lib/navx_frc.jar b/Robot2018/lib/navx_frc.jar index 7f5e059..32d6298 100644 Binary files a/Robot2018/lib/navx_frc.jar and b/Robot2018/lib/navx_frc.jar differ diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 452606c..38aa73f 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -7,8 +7,15 @@ package org.usfirst.frc.team199.Robot2018; +import org.usfirst.frc.team199.Robot2018.commands.CloseIntake; +import org.usfirst.frc.team199.Robot2018.commands.IntakeCube; +import org.usfirst.frc.team199.Robot2018.commands.LowerIntake; +import org.usfirst.frc.team199.Robot2018.commands.OpenIntake; +import org.usfirst.frc.team199.Robot2018.commands.OutakeCube; import org.usfirst.frc.team199.Robot2018.commands.PIDMove; import org.usfirst.frc.team199.Robot2018.commands.PIDTurn; +import org.usfirst.frc.team199.Robot2018.commands.RaiseIntake; +import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders; import org.usfirst.frc.team199.Robot2018.commands.SetDistancePerPulse; import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType; import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear; @@ -31,37 +38,65 @@ public class OI { private JoystickButton shiftDriveTypeButton; private JoystickButton PIDMoveButton; private JoystickButton PIDTurnButton; + private JoystickButton resetEncButton; public Joystick rightJoy; private JoystickButton updatePIDConstantsButton; private JoystickButton updateEncoderDPPButton; public Joystick manipulator; + private JoystickButton closeIntake; + private JoystickButton openIntake; + private JoystickButton raiseIntake; + private JoystickButton lowerIntake; + private JoystickButton intake; + private JoystickButton outake; public int getButton(String key, int def) { if (!SmartDashboard.containsKey("Button/" + key)) { - SmartDashboard.putNumber("Button/" + key, def); + if (!SmartDashboard.putNumber("Button/" + key, def)) { + System.err.println("SmartDashboard Key" + "Button/" + key + "already taken by a different type"); + return def; + } } return (int) SmartDashboard.getNumber("Button/" + key, def); } public OI() { leftJoy = new Joystick(0); - shiftLowGearButton = new JoystickButton(leftJoy, getButton("Shift Low Gear", 3)); - shiftLowGearButton.whenPressed(new ShiftLowGear()); - shiftHighGearButton = new JoystickButton(leftJoy, getButton("Shift High Gear", 5)); - shiftHighGearButton.whenPressed(new ShiftHighGear()); shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2)); shiftDriveTypeButton.whenPressed(new ShiftDriveType()); PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7)); - PIDMoveButton.whenPressed(new PIDMove(40, Robot.dt, RobotMap.distEncAvg)); + PIDMoveButton + .whenPressed(new PIDMove(Robot.sd.getConst("Move Targ", 24), Robot.dt, Robot.sd, RobotMap.distEncAvg)); PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8)); - PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro)); + // PIDTurnButton.whenPressed(new PIDTurn(Robot.getConst("Turn Targ", 90), + // Robot.dt, Robot.sd RobotMap.fancyGyro)); + PIDTurnButton + .whenReleased(new PIDTurn(Robot.getConst("Turn Targ", 90), Robot.dt, Robot.sd, RobotMap.fancyGyro)); + resetEncButton = new JoystickButton(leftJoy, getButton("Reset Dist Enc", 10)); + resetEncButton.whenPressed(new ResetEncoders()); rightJoy = new Joystick(1); + shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 3)); + shiftHighGearButton.whenPressed(new ShiftHighGear()); + shiftLowGearButton = new JoystickButton(rightJoy, getButton("Shift Low Gear", 2)); + shiftLowGearButton.whenPressed(new ShiftLowGear()); updatePIDConstantsButton = new JoystickButton(rightJoy, getButton("Get PID Constants", 8)); updatePIDConstantsButton.whenPressed(new UpdatePIDConstants()); updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9)); updateEncoderDPPButton.whenPressed(new SetDistancePerPulse()); manipulator = new Joystick(2); + closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1)); + closeIntake.whenPressed(new CloseIntake()); + openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2)); + openIntake.whenPressed(new OpenIntake()); + raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake Button", 3)); + raiseIntake.whenPressed(new RaiseIntake()); + lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake Button", 4)); + lowerIntake.whenPressed(new LowerIntake()); + intake = new JoystickButton(manipulator, getButton("Intake Button", 5)); + intake.whenPressed(new IntakeCube()); + outake = new JoystickButton(manipulator, getButton("Outake Button", 6)); + outake.whenPressed(new OutakeCube()); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index e67c8c1..450766d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -15,15 +15,17 @@ import org.usfirst.frc.team199.Robot2018.commands.Autonomous; import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Position; import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Strategy; +import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear; import org.usfirst.frc.team199.Robot2018.subsystems.Climber; import org.usfirst.frc.team199.Robot2018.subsystems.ClimberAssist; import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain; import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject; import org.usfirst.frc.team199.Robot2018.subsystems.Lift; +import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.Preferences; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -36,7 +38,7 @@ * creating this project, you must also update the manifest file in the resource * directory. */ -public class Robot extends TimedRobot { +public class Robot extends IterativeRobot { public static Climber climber; public static ClimberAssist climberAssist; @@ -55,23 +57,46 @@ public class Robot extends TimedRobot { Map> stratChoosers = new HashMap>(); String[] fmsPossibilities = { "LL", "LR", "RL", "RR" }; - public static double getConst(String key, double def) { - if (!SmartDashboard.containsKey("Const/" + key)) { - SmartDashboard.putNumber("Const/" + key, def); + public static SmartDashboardInterface sd = new SmartDashboardInterface() { + public double getConst(String key, double def) { + Preferences pref = Preferences.getInstance(); + if (!pref.containsKey("Const/" + key)) { + pref.putDouble("Const/" + key, def); + if (pref.getDouble("Const/ + key", def) != def) { + System.err.println("pref Key" + "Const/" + key + "already taken by a different type"); + return def; + } + } + return pref.getDouble("Const/" + key, def); } - return SmartDashboard.getNumber("Const/" + key, def); + /* + * if (!SmartDashboard.containsKey("Const/" + key)) { if + * (!SmartDashboard.putNumber("Const/" + key, def)) { + * System.err.println("SmartDashboard Key" + "Const/" + key + + * "already taken by a different type"); return def; } } return + * SmartDashboard.getNumber("Const/" + key, def); + */ + }; + + public static double getConst(String key, double def) { + return sd.getConst(key, def); } public static boolean getBool(String key, boolean def) { - if (!SmartDashboard.containsKey("Bool/" + key)) { - SmartDashboard.putBoolean("Bool/" + key, def); + Preferences pref = Preferences.getInstance(); + if (!pref.containsKey("Bool/" + key)) { + pref.putBoolean("Bool/" + key, def); + if (pref.getBoolean("Bool/" + key, def) == def) { + System.err.println("pref Key" + "Bool/" + key + "already taken by a different type"); + return def; + } } - return SmartDashboard.getBoolean("Bool/" + key, def); + return pref.getBoolean("Bool/" + key, def); } /** - * This function is run when the robot is first started up and should be - * used for any initialization code. + * This function is run when the robot is first started up and should be used + * for any initialization code. */ @Override public void robotInit() { @@ -102,17 +127,18 @@ public void robotInit() { // auto delay chooser SmartDashboard.putNumber("Auto Delay", 0); - // parse scripts from Preferences, which maintains values throughout - // reboots + // parse scripts from Preferences, which maintains values throughout reboots autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", "")); listen = new Listener(); + CameraServer.getInstance().startAutomaticCapture(0); + CameraServer.getInstance().startAutomaticCapture(1); } /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. */ @Override public void disabledInit() { @@ -125,12 +151,13 @@ public void disabledPeriodic() { } /** - * This function is called once during the start of autonomous in order to - * grab values from SmartDashboard and the FMS and call the Autonomous - * command with those values. + * This function is called once during the start of autonomous in order to grab + * values from SmartDashboard and the FMS and call the Autonomous command with + * those values. */ @Override public void autonomousInit() { + Scheduler.getInstance().add(new ShiftLowGear()); String fmsInput = DriverStation.getInstance().getGameSpecificMessage(); Position startPos = posChooser.getSelected(); double autoDelay = SmartDashboard.getNumber("Auto Delay", 0); @@ -162,6 +189,7 @@ public void teleopInit() { // this line or comment it out. if (autonomousCommand != null) autonomousCommand.cancel(); + dt.putVelocityControllersToDashboard(); } /** @@ -170,6 +198,26 @@ public void teleopInit() { @Override public void teleopPeriodic() { Scheduler.getInstance().run(); + + // SmartDashboard.putNumber("Drivetrain/Left VPID Targ", + // Robot.dt.getLeftVPIDSetpoint()); + // SmartDashboard.putNumber("Drivetrain/Right VPID Targ", + // Robot.dt.getRightVPIDSetpoint()); + // SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror()); + // SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror()); + // SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate()); + // SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate()); + // + // SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist()); + // SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist()); + // SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist()); + // + // SmartDashboard.putNumber("Angle", dt.getAHRSAngle()); + } + + boolean firstTime = true; + + public void testInit() { } /** @@ -177,5 +225,30 @@ public void teleopPeriodic() { */ @Override public void testPeriodic() { + // if(firstTime) { + // Robot.dt.enableVelocityPIDs(); + // firstTime = false; + //// } + // Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5)); + + Scheduler.getInstance().run(); + + SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint()); + SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint()); + SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror()); + SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror()); + SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate()); + SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate()); + + SmartDashboard.putNumber("Left Enc Dist", dt.getLeftDist()); + SmartDashboard.putNumber("Right Enc Dist", dt.getRightDist()); + SmartDashboard.putNumber("Avg Enc Dist", dt.getEncAvgDist()); + + // dt.dtLeft.set(0.1); + // dt.dtRight.set(-oi.rightJoy.getY()); + // dt.dtLeft.set(-oi.leftJoy.getY()); + // dt.dtRight.set(-oi.rightJoy.getY()); + + dt.putVelocityControllersToDashboard(); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index 383f894..215888f 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -19,8 +19,10 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.PowerDistributionPanel; +import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -32,10 +34,18 @@ */ public class RobotMap { - public static WPI_TalonSRX intakeMotor; + public static PowerDistributionPanel pdp; + public static WPI_TalonSRX liftMotor; public static WPI_TalonSRX climberMotor; + public static VictorSP leftIntakeMotor; + public static VictorSP rightIntakeMotor; + public static DoubleSolenoid leftIntakeVerticalSolenoid; + public static DoubleSolenoid rightIntakeVerticalSolenoid; + public static DoubleSolenoid leftIntakeHorizontalSolenoid; + public static DoubleSolenoid rightIntakeHorizontalSolenoid; + public static DigitalSource leftEncPort1; public static DigitalSource leftEncPort2; public static Encoder leftEncDist; @@ -60,6 +70,8 @@ public class RobotMap { public static AHRS fancyGyro; public static DoubleSolenoid dtGear; + private final double DIST_PER_PULSE_RATIO = (5.0 * Math.PI) * (17.0 / 25) / (3.0 * 256); + /** * This function takes in a TalonSRX motorController and sets nominal and peak * outputs to the default @@ -79,7 +91,7 @@ private void configSRX(WPI_TalonSRX mc) { mc.configPeakCurrentLimit(0, 0); mc.configPeakCurrentDuration(0, 0); // 40 amps is the amp limit of a CIM. also, the PDP has 40 amp circuit breakers, - // so if we went above 40, the motors would stop completely. + // so if we went above 40, the motors would stop completely mc.configContinuousCurrentLimit(40, 0); mc.enableCurrentLimit(true); } @@ -101,63 +113,91 @@ private void configSPX(WPI_VictorSPX mc) { } public RobotMap() { + pdp = new PowerDistributionPanel(); + + // intakeMotor = new WPI_TalonSRX(getPort("IntakeTalonSRX", 4)); + // configSRX(intakeMotor); + // liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 5)); + // configSRX(liftMotor); + // climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6)); + // configSRX(climberMotor); - intakeMotor = new WPI_TalonSRX(getPort("IntakeTalonSRX", 4)); - configSRX(intakeMotor); - liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 5)); - configSRX(liftMotor); - climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6)); - configSRX(climberMotor); + leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 9)); + rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 8)); + // leftIntakeHorizontalSolenoid = new + // DoubleSolenoid(getPort("IntakeLeftHorizontalSolenoidPort1", 2), + // getPort("IntakeLeftHorizontalSolenoidPort2", 3)); + // rightIntakeHorizontalSolenoid = new + // DoubleSolenoid(getPort("IntakeRightHorizontalSolenoidPort1", 4), + // getPort("IntakeRightHorizontalSolenoidPort2", 5)); + // leftIntakeVerticalSolenoid = new + // DoubleSolenoid(getPort("IntakeLeftVerticalSolenoidPort1", 6), + // getPort("IntakeLeftVerticalSolenoidPort2", 7)); + // rightIntakeVerticalSolenoid = new + // DoubleSolenoid(getPort("IntakeRightVerticalSolenoidPort1", 8), + // getPort("IntakeRightVerticalSolenoidPort2", 9)); - leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 0)); - leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 1)); + leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 2)); + leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 3)); leftEncDist = new Encoder(leftEncPort1, leftEncPort2); leftEncDist.setPIDSourceType(PIDSourceType.kDisplacement); leftEncRate = new Encoder(leftEncPort1, leftEncPort2); leftEncRate.setPIDSourceType(PIDSourceType.kRate); - dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 0)); + leftEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO)); + leftEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO)); + + dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 1)); configSRX(dtLeftMaster); - dtLeftSlave = new WPI_VictorSPX(getPort("LeftTalonSPXSlave", 1)); + dtLeftSlave = new WPI_VictorSPX(getPort("LeftVictorSPXSlave", 2)); configSPX(dtLeftSlave); dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave); + // inverted bc gear boxes invert from input to output + dtLeft.setInverted(true); - leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 1), - Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0), + leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkI", 0), 0, + Robot.getConst("VelocityLeftkD", calcDefkD(Robot.getConst("Max Low Speed", 84))), 1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft); - leftVelocityController.enable(); leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204)); leftVelocityController.setOutputRange(-1.0, 1.0); leftVelocityController.setContinuous(false); leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2)); + SmartDashboard.putData(leftVelocityController); - rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2)); - rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3)); - rightEncDist = new Encoder(leftEncPort1, leftEncPort2); + rightEncPort1 = new DigitalInput(getPort("1RightEnc", 1)); + rightEncPort2 = new DigitalInput(getPort("2RightEnc", 0)); + rightEncDist = new Encoder(rightEncPort1, rightEncPort2); rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement); - rightEncRate = new Encoder(leftEncPort1, leftEncPort2); + rightEncRate = new Encoder(rightEncPort1, rightEncPort2); rightEncRate.setPIDSourceType(PIDSourceType.kRate); - dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 2)); + rightEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO)); + rightEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO)); + rightEncRate.setReverseDirection(true); + + dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 4)); configSRX(dtRightMaster); - dtRightSlave = new WPI_VictorSPX(getPort("RightTalonSPXSlave", 3)); + dtRightSlave = new WPI_VictorSPX(getPort("RightVictorSPXSlave", 3)); configSPX(dtRightSlave); dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave); + // inverted bc gear boxes invert from input to output + dtRight.setInverted(true); - rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 1), - Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0), + rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkI", 0), 0, + Robot.getConst("VelocityRightkD", calcDefkD(Robot.getConst("Max Low Speed", 84))), 1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight); - rightVelocityController.enable(); rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204)); rightVelocityController.setOutputRange(-1.0, 1.0); rightVelocityController.setContinuous(false); rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2)); - robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController); - robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204)); + // robotDrive = new DifferentialDrive(leftVelocityController, + // rightVelocityController); + // robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204)); + robotDrive = new DifferentialDrive(dtLeft, dtRight); distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist); - fancyGyro = new AHRS(SerialPort.Port.kMXP); + fancyGyro = new AHRS(SPI.Port.kMXP); dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1)); } @@ -173,9 +213,50 @@ public RobotMap() { */ public int getPort(String key, int def) { if (!SmartDashboard.containsKey("Port/" + key)) { - SmartDashboard.putNumber("Port/" + key, def); + if (!SmartDashboard.putNumber("Port/" + key, def)) { + System.err.println("SmartDashboard Key" + "Port/" + key + "already taken by a different type"); + return def; + } } return (int) SmartDashboard.getNumber("Port/" + key, def); } + /** + * Uses SmartDashboard and math to calculate a *great* default kD + */ + public double calcDefkD(double maxSpeed) { + /* + * timeConstant is proportional to max speed of the shaft (which is the max + * speed of the cim divided by the gear reduction), half the mass (because the + * half of the drivetrain only has to support half of the robot), and radius of + * the drivetrain wheels squared. It's inversely proportional to the stall + * torque of the shaft, which is found by multiplying the stall torque of the + * motor with the gear reduction by the amount of motors. The omegaMax needs to + * be converted from rpm to radians per second, so divide by 60 and multiply to + * get radians. + */ + double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392) + : Robot.getConst("Low Gear Gear Reduction", 12.255); + double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635); + double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction / 60 * 2 * Math.PI + * convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius + / (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2); + double cycleTime = Robot.getConst("Code cycle time", 0.05); + /* + * The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is + * one. + */ + double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1; + return 1 / denominator / maxSpeed; + } + + private double convertLbsTokG(double lbs) { + // number from google ;) + return lbs * 0.45359237; + } + + private double convertNtokG(double newtons) { + // weight / accel due to grav = kg + return newtons / 9.81; + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java new file mode 100644 index 0000000..18a9dc2 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java @@ -0,0 +1,5 @@ +package org.usfirst.frc.team199.Robot2018; + +public interface SmartDashboardInterface { + public double getConst(String key, double def); +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java index 48d86d1..7f523a3 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java @@ -5,32 +5,34 @@ import java.util.Map; public class AutoUtils { - + + public static Position position = new Position(0, 0, 0); + /** * Parses the inputted script file into a map of scripts * - * @param scriptFile the script file to parse - * @return a map, with the key being the script name, and the argument - * being a list of arrays that are instruction-argument pairs + * @param scriptFile + * the script file to parse + * @return a map, with the key being the script name, and the argument being a + * list of arrays that are instruction-argument pairs */ public static Map> parseScriptFile(String scriptFile) { Map> autoScripts = new HashMap>(); - + String lines[] = scriptFile.split("\\r?\\n"); - + ArrayList currScript = new ArrayList(); - String currScriptName = ""; - + String currScriptName = ""; + int count = 1; for (String line : lines) { // remove comments int commentIndex = line.indexOf("#"); if (commentIndex != -1) line = line.substring(0, commentIndex); - + // trim and remove extra whitespace just to make it neater - line = line.trim().replaceAll("\\s+", " "); - + line = line.trim().replaceAll("\\s+", " "); // make coordinates with spaces also work int parenIndex = line.indexOf("("); while (parenIndex != -1) { @@ -42,23 +44,23 @@ public static Map> parseScriptFile(String scriptFile // finds next parentheses parenIndex = line.indexOf("(", parenIndex + 1); } - // if there's no instruction on this line, skip if (line.equals("")) { continue; } - - // if current line is a label, store the previous script and make a new empty one + + // if current line is a label, store the previous script and make a new empty + // one if (line.endsWith(":")) { autoScripts.put(currScriptName, currScript); currScript = new ArrayList(); currScriptName = line.substring(0, line.length() - 1); } else { - + // first separate the command into instruction and args String instruction; String args; - + int separator = line.indexOf(' '); if (separator == -1) { instruction = line; @@ -67,85 +69,87 @@ public static Map> parseScriptFile(String scriptFile instruction = line.substring(0, separator); args = line.substring(separator + 1); } - + // if it's valid, put it into the script if (isValidCommand(instruction, args, count)) { - String[] command = {instruction, args}; + String[] command = { instruction, args }; currScript.add(command); } } count++; } - + // puts the last script in autoScripts.put(currScriptName, currScript); - + // remove the stray one in the beginning autoScripts.remove(""); - + return autoScripts; } - - + /** * Validates the command inputted to see if it's AAA compliant * - * @param instruction the instruction/command name - * @param args the arguments provided to the instruction. A blank String if none - * @param lineNumber the lineNumber in the script file. used for logging warnings + * @param instruction + * the instruction/command name + * @param args + * the arguments provided to the instruction. A blank String if none + * @param lineNumber + * the lineNumber in the script file. used for logging warnings * @return if the command is valid */ - public static boolean isValidCommand (String instruction, String args, int lineNumber) { + public static boolean isValidCommand(String instruction, String args, int lineNumber) { // moveto takes in a set of points, and the last arg can be a number if (instruction.equals("moveto")) { if (args == "") { logWarning(lineNumber, "The command `moveto` requires at least one argument."); return false; } - + String[] splitArgs = args.split(" "); for (int i = 0; i < splitArgs.length - 1; i++) { if (!isPoint(splitArgs[i])) { - logWarning(lineNumber, "The arguments for command `moveto` should be points formatted like this: " - + "`(x,y)`."); + logWarning(lineNumber, + "The arguments for command `moveto` should be points formatted like this: " + "`(x,y)`."); return false; } } - + if (!isDouble(splitArgs[splitArgs.length - 1]) && !isPoint(splitArgs[splitArgs.length - 1])) { logWarning(lineNumber, "The last argument for command `moveto` should be a number, or a point " + "formatted like this: `(x,y)`."); return false; } - } - + } + // turn can take a number or point else if (instruction.equals("turn")) { if (args.contains(" ")) { logWarning(lineNumber, "Command `turn` only accepts one argument."); return false; } - + if (!isDouble(args) && !isPoint(args)) { logWarning(lineNumber, "The argument for command `turn` should be a number or a point formatted like " + "this: `(x,y)`."); return false; } - } - + } + // move and wait can take only a number else if (instruction.equals("move") || instruction.equals("wait")) { if (args.contains(" ")) { logWarning(lineNumber, "Command `move` only accepts one argument."); return false; } - + if (!isDouble(args)) { logWarning(lineNumber, "The argument for command `move` should be a number."); return false; } } - + // switch, scale, exchange, intake, and end all don't have any args else if (instruction.equals("switch") || instruction.equals("scale") || instruction.equals("exchange") || instruction.equals("intake") || instruction.equals("end")) { @@ -153,76 +157,80 @@ else if (instruction.equals("switch") || instruction.equals("scale") || instruct logWarning(lineNumber, "Command `" + instruction + "` does not accept any arguments."); return false; } - } - + } + // Jump only takes one argument else if (instruction.equals("jump")) { if (args.contains(" ")) { logWarning(lineNumber, "Command `jump` only accepts one argument."); return false; } - } - - // if it's not even a valid instruction + } + + // if it's not even a valid instruction else { logWarning(lineNumber, "`" + instruction + "` is not a valid command."); return false; } - + // if everything is all good return true; } - + /** - * Helper method used by isValidCommand() to log warnings for non-valid commands. + * Helper method used by isValidCommand() to log warnings for non-valid + * commands. * - * @param lineNumber the line number in the script file - * @param message the message to log + * @param lineNumber + * the line number in the script file + * @param message + * the message to log */ + private static void logWarning (int lineNumber, String message) { System.err.println("[ERROR] Line " + lineNumber + ": " + message); } - + /** - * Helper method used by isValidCommand() to check if an argument is a - * point, characterized by parentheses on the left and right, with two - * numbers separated by a comma, with no whitespace in between. + * Helper method used by isValidCommand() to check if an argument is a point, + * characterized by parentheses on the left and right, with two numbers + * separated by a comma, with no whitespace in between. * - * @param s the argument + * @param s + * the argument * @return if the argument is a point */ - private static boolean isPoint (String s) { + public static boolean isPoint(String s) { // checks if it starts and ends with parentheses if (!s.startsWith("(") || !s.endsWith(")")) return false; - + // checks that there's one, and only one comma (like this phrase) int indexOfComma = s.indexOf(','); int count = 0; while (indexOfComma != -1) { - count++; - indexOfComma = s.indexOf(',', indexOfComma + 1); + count++; + indexOfComma = s.indexOf(',', indexOfComma + 1); } if (count != 1) return false; - - + // really ugly, but just checks if the stuff between the parentheses are numbers - if (!isDouble(s.substring(1, s.indexOf(','))) - || !isDouble(s.substring(s.indexOf(',') + 1, s.length() - 1))) + if (!isDouble(s.substring(1, s.indexOf(','))) || !isDouble(s.substring(s.indexOf(',') + 1, s.length() - 1))) return false; - + return true; } - + /** - * Helper method used by isValidCommand() used to check if an argument is - * able to be converted into a double + * Helper method used by isValidCommand() used to check if an argument is able + * to be converted into a double * - * @param s the argument + * @param s + * the argument * @return if the argument is a double */ - private static boolean isDouble (String s) { + public static boolean isDouble(String s) { try { Double.parseDouble(s); } catch (Exception e) { @@ -230,4 +238,22 @@ private static boolean isDouble (String s) { } return true; } + + public static double[] parsePoint(String cmdArgs) { + double[] point = new double[2]; + String parentheseless; + String[] pointparts; + if (AutoUtils.isPoint(cmdArgs)) { + parentheseless = cmdArgs.substring(1, cmdArgs.length() - 1); + pointparts = parentheseless.split(","); + try { + point[0] = Double.parseDouble(pointparts[0]); + point[1] = Double.parseDouble(pointparts[1]); + } catch (Exception e) { + point[0] = 1; + point[1] = 1; + } + } + return point; + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/Position.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/Position.java new file mode 100644 index 0000000..01a1dca --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/Position.java @@ -0,0 +1,52 @@ +package org.usfirst.frc.team199.Robot2018.autonomous; + +public class Position { + private double currX; + private double currY; + private double currRotation; + + public Position(double x, double y, double rot) { + currX = x; + currY = y; + currRotation = rot; + } + + /* + * All of these are getters and setters for the robot's position and orientation + */ + public double getX() { + return currX; + } + + public double getY() { + return currY; + } + + public double getRot() { + return currRotation; + } + + public void setX(double x) { + currX = x; + } + + public void setY(double y) { + currY = y; + } + + public void setRot(double rot) { + currRotation = rot; + } + + public void changeX(double x) { + currX += x; + } + + public void changeY(double y) { + currY += y; + } + + public void changeRot(double rot) { + currRotation += rot; + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java index 416f167..94b5b29 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/VelocityPIDController.java @@ -41,7 +41,7 @@ public VelocityPIDController(double kp, double ki, double kd, double kf, PIDSour */ @Override public void pidWrite(double output) { - setSetpoint(output); + set(output); } /** @@ -64,7 +64,7 @@ public void set(double speed) { */ @Override public double get() { - return getSetpoint(); + return out.get(); } /** diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/requirements.txt b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/requirements.txt new file mode 100644 index 0000000..c17bd6b --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/requirements.txt @@ -0,0 +1 @@ +pynetworktables==2018.0.0 diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py new file mode 100644 index 0000000..bedc85e --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py @@ -0,0 +1,40 @@ +#nt setup +from networktables import NetworkTables +import __future__ +NetworkTables.initialize(server='10.1.99.2') +prefs = NetworkTables.getTable("Preferences") + +go = False #true when file has been successfully read +lines = [] #an array of strings which represent each line of the file +filename = raw_input("File name: ") #the name of the file to read (user input) +oneline = "" #the file as a single string +retry = False #whether or not to retry. + +if not NetworkTables.isConnected(): + print("You aren't connected to the bot.") + quit() +#loops until a file is read into the file array +while not go: + if retry: + filename = raw_input("Not found. Try another name (enter to quit): ")#retry, or quit (in case the file doesn't exist) + if filename == "": + quit() + try: + with open(filename) as script: + lines = script.readlines() + oneline = "".join(lines) #this is python's weird syntax for joining an array into one line, as we can't seem to pull string arrays from the WPILib Preferences class. + break + except: + retry = True +#puts the string +prefs.putString("autoscripts", oneline) +print("Uploading %s as a String[] to key \"autoscripts\"" % filename) + +#checks if the key has been filled +tester = "" #a variable to check if autoscripts is None + +tester = prefs.getValue("autoscripts", None) +if tester != oneline: + print("It doesn't look like key \"autoscripts\" is filled, maybe you aren't connected to networktables?") +else: + print("Success!") diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java index ec1c414..ec04c61 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java @@ -1,36 +1,55 @@ package org.usfirst.frc.team199.Robot2018.commands; -import edu.wpi.first.wpilibj.command.Command; +import org.usfirst.frc.team199.Robot2018.Robot; +import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; +import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; +import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; -/** - * - */ -public class AutoMoveTo extends Command { - - public AutoMoveTo(String[] points) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } +import edu.wpi.first.wpilibj.PIDSource; - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } +//import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain; - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } +import edu.wpi.first.wpilibj.command.CommandGroup; - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } +/** + * The AutoMoveTo command, which makes the robot go through a set of movements + * and rotations. + */ +public class AutoMoveTo extends CommandGroup { + + public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc) { + // requires(Drivetrain); + double rotation; + double[] point = { 0, 0 }; + for (String arg : args) { + if (AutoUtils.isDouble(arg)) { + rotation = Double.valueOf(arg); + double relrotation = rotation - AutoUtils.position.getRot(); + addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc)); + AutoUtils.position.setRot(rotation); + } else if (AutoUtils.isPoint(arg)) { + point = AutoUtils.parsePoint(arg); + double deltaX = point[0] - AutoUtils.position.getX(); + double deltaY = point[1] - AutoUtils.position.getY(); + double atan = Math.toDegrees(Math.atan(deltaX / deltaY)); + double relrotation = atan - AutoUtils.position.getRot(); + addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc)); + double dX2 = deltaX * deltaX; + double dY2 = deltaY * deltaY; + double distance = Math.sqrt(dX2 + dY2); + addSequential(new PIDMove(distance, dt, sd, pidMoveSrc)); + double x = AutoUtils.position.getX(); + double y = AutoUtils.position.getY(); + AutoUtils.position.setX(point[0]); + AutoUtils.position.setY(point[1]); + AutoUtils.position.setRot(Math.toDegrees(Math.atan((point[0] - x) / (point[1] - y)))); + } else { + throw new IllegalArgumentException(); + } + } + } + + public AutoMoveTo(String[] args) { + this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()); + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/CloseIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/CloseIntake.java new file mode 100644 index 0000000..01acc4f --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/CloseIntake.java @@ -0,0 +1,25 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * + */ +public class CloseIntake extends InstantCommand { + + public CloseIntake() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called once when the command executes + protected void initialize() { + Robot.intakeEject.closeIntake(); + SmartDashboard.putBoolean("Intake Open", false); + } + +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java index 2e30a79..5557563 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java @@ -1,5 +1,7 @@ package org.usfirst.frc.team199.Robot2018.commands; +import org.usfirst.frc.team199.Robot2018.Robot; + import edu.wpi.first.wpilibj.command.Command; /** @@ -7,30 +9,31 @@ */ public class IntakeCube extends Command { - public IntakeCube() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } + public IntakeCube() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.intakeEject.runIntake(1); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return Robot.intakeEject.hasCube(); + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LowerIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LowerIntake.java new file mode 100644 index 0000000..0b4a7d2 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LowerIntake.java @@ -0,0 +1,25 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * + */ +public class LowerIntake extends InstantCommand { + + public LowerIntake() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called once when the command executes + protected void initialize() { + Robot.intakeEject.lowerIntake(); + SmartDashboard.putBoolean("Intake Up", false); + } + +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OpenIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OpenIntake.java new file mode 100644 index 0000000..a9d9d87 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OpenIntake.java @@ -0,0 +1,25 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * + */ +public class OpenIntake extends InstantCommand { + + public OpenIntake() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called once when the command executes + protected void initialize() { + Robot.intakeEject.openIntake(); + SmartDashboard.putBoolean("Intake Open", true); + } + +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java new file mode 100644 index 0000000..e68dea4 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java @@ -0,0 +1,43 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class OutakeCube extends Command { + + Timer tim; + + public OutakeCube() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + protected void initialize() { + tim = new Timer(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.intakeEject.runIntake(-1); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return tim.get() > Robot.getConst("Outake Time", 1); + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index e719213..7619b5c 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -1,12 +1,14 @@ package org.usfirst.frc.team199.Robot2018.commands; import org.usfirst.frc.team199.Robot2018.Robot; -import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage; +import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * Drives the robot a certain target distance using PID. Implements PIDOutput in @@ -17,6 +19,7 @@ public class PIDMove extends Command implements PIDOutput { private double target; private DrivetrainInterface dt; private PIDController moveController; + private PIDSource avg; /** * Constructs this command with a new PIDController. Sets all of the @@ -32,14 +35,19 @@ public class PIDMove extends Command implements PIDOutput { * @param avg * the PIDSourceAverage of the DT's two Encoders */ - public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) { + public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) { // Use requires() here to declare subsystem dependencies - requires(Robot.dt); target = targ; this.dt = dt; - double kf = 1 / (dt.getCurrentMaxSpeed() * Robot.getConst("Default PID Update Time", 0.05)); - moveController = new PIDController(Robot.getConst("MovekP", 1), Robot.getConst("MovekI", 0), - Robot.getConst("MovekD", 0), kf, avg, this); + this.avg = avg; + if (Robot.dt != null) { + requires(Robot.dt); + } + moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0), + avg, this); + double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05)); + moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0), + kf, avg, this); } /** @@ -49,14 +57,19 @@ public PIDMove(double targ, DrivetrainInterface dt, PIDSourceAverage avg) { @Override public void initialize() { dt.resetDistEncs(); + moveController.disable(); // input is in inches moveController.setInputRange(-Robot.getConst("Max High Speed", 204), Robot.getConst("Max High Speed", 204)); // output in "motor units" (arcade and tank only accept values [-1, 1] moveController.setOutputRange(-1.0, 1.0); moveController.setContinuous(false); - moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2)); - moveController.setSetpoint(target); + moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1)); + moveController.setSetpoint(Robot.getConst("Move Targ", 24)); + + SmartDashboard.putData("Move PID", moveController); + moveController.enable(); + // dt.enableVelocityPIDs(); } /** @@ -66,6 +79,9 @@ public void initialize() { */ @Override protected void execute() { + System.out.println("Enc Avg Dist: " + avg.pidGet()); + SmartDashboard.putNumber("Move PID Result", moveController.get()); + SmartDashboard.putNumber("Move PID Error", moveController.getError()); } /** @@ -76,7 +92,9 @@ protected void execute() { */ @Override protected boolean isFinished() { - return moveController.onTarget(); + return moveController.onTarget() + && Math.abs(dt.getLeftEncRate()) <= Robot.getConst("Maximum Velocity When Stop", 1) + && Math.abs(dt.getRightEncRate()) <= Robot.getConst("Maximum Velocity When Stop", 1); } /** @@ -87,7 +105,8 @@ protected boolean isFinished() { @Override protected void end() { moveController.disable(); - moveController.free(); + System.out.println("End"); + // moveController.free(); } /** @@ -113,5 +132,6 @@ protected void interrupted() { @Override public void pidWrite(double output) { dt.arcadeDrive(output, 0); + SmartDashboard.putNumber("Move PID Output", output); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index 84aaf3c..9677eb0 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -1,13 +1,15 @@ package org.usfirst.frc.team199.Robot2018.commands; import org.usfirst.frc.team199.Robot2018.Robot; +import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; -import com.kauailabs.navx.frc.AHRS; - import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * Turns the robot to a certain target bearing using PID. Implements PIDOutput @@ -18,6 +20,9 @@ public class PIDTurn extends Command implements PIDOutput { private double target; private DrivetrainInterface dt; private PIDController turnController; + private PIDSource ahrs; + private Timer tim; + private double lastTime; /** * Constructs this command with a new PIDController. Sets all of the @@ -33,19 +38,26 @@ public class PIDTurn extends Command implements PIDOutput { * testing) * @param ahrs * the AHRS (gyro) + * @param sd + * the Smart Dashboard reference, or a SmartDashboardInterface for + * testing */ - public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) { + public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { // Use requires() here to declare subsystem dependencies - requires(Robot.dt); target = targ; this.dt = dt; + this.ahrs = ahrs; + + if (Robot.dt != null) { + requires(Robot.dt); + } // calculates the maximum turning speed in degrees/sec based on the max linear // speed in inches/s and the distance (inches) between sides of the DT - double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 - / (Math.PI * Robot.getConst("Distance Between Wheels", 26.25)); - double kf = 1 / (maxTurnSpeed * Robot.getConst("Default PID Update Time", 0.05)); - turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0), - Robot.getConst("TurnkD", 0), kf, ahrs, this); + double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25)); + double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05)); + turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0), + kf, ahrs, this); + // tim = new Timer(); } /** @@ -54,16 +66,30 @@ public PIDTurn(double targ, DrivetrainInterface dt, AHRS ahrs) { */ @Override protected void initialize() { - dt.resetAHRS(); turnController.disable(); + // dt.enableVelocityPIDs(); + System.out.println("initialize2s"); + dt.resetAHRS(); + System.out.println("after reset"); + System.out.println("after disabling"); // input is in degrees turnController.setInputRange(-180, 180); // output in "motor units" (arcade and tank only accept values [-1, 1] turnController.setOutputRange(-1.0, 1.0); - turnController.setContinuous(); + turnController.setContinuous(true); turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1)); - turnController.setSetpoint(target); + double newSetPoint = Robot.getConst("Turn Targ", 90); + while (Math.abs(newSetPoint) > 180) { + newSetPoint = newSetPoint - Math.signum(newSetPoint) * 360; + } + turnController.setSetpoint(newSetPoint); + + SmartDashboard.putData("Turn PID", turnController); + turnController.enable(); + System.out.println("initialize finished"); + // tim.start(); + // lastTime = tim.get(); } /** @@ -73,6 +99,12 @@ protected void initialize() { */ @Override protected void execute() { + System.out.println("execute"); + System.out.println("Angle: " + dt.getAHRSAngle()); + // if (tim.get() > lastTime + Robot.getConst("Update Time", 1)) { + // SmartDashboard.putNumber("Angle", dt.getAHRSAngle()); + // lastTime = tim.get(); + // } } /** @@ -84,7 +116,13 @@ protected void execute() { */ @Override protected boolean isFinished() { - return turnController.onTarget(); + System.out.println("isFinished"); + return (turnController.onTarget() && Math.abs(dt.getGyroRate()) < 1); + // return turnController.onTarget() + // && Math.abs(dt.getLeftEncRate()) <= Robot.getConst("Maximum Velocity When + // Stop", 1) + // && Math.abs(dt.getRightEncRate()) <= Robot.getConst("Maximum Velocity When + // Stop", 1); } /** @@ -95,7 +133,10 @@ protected boolean isFinished() { @Override protected void end() { turnController.disable(); - turnController.free(); + System.out.println("end"); + SmartDashboard.putNumber("Turn PID Result", turnController.get()); + SmartDashboard.putNumber("Turn PID Error", turnController.getError()); + // turnController.free(); } /** @@ -120,5 +161,6 @@ protected void interrupted() { @Override public void pidWrite(double output) { dt.arcadeDrive(0, output); + SmartDashboard.putNumber("Turn PID Output", output); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RaiseIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RaiseIntake.java new file mode 100644 index 0000000..27da950 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RaiseIntake.java @@ -0,0 +1,25 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * + */ +public class RaiseIntake extends InstantCommand { + + public RaiseIntake() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called once when the command executes + protected void initialize() { + Robot.intakeEject.raiseIntake(); + SmartDashboard.putBoolean("Intake Up", true); + } + +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ResetEncoders.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ResetEncoders.java new file mode 100644 index 0000000..758068d --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ResetEncoders.java @@ -0,0 +1,22 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; + +/** + * + */ +public class ResetEncoders extends InstantCommand { + + public ResetEncoders() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + super(); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.dt.resetDistEncs(); + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java index d5b7208..2c0bc59 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java @@ -3,6 +3,7 @@ import java.util.ArrayList; import org.usfirst.frc.team199.Robot2018.Robot; +import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; import edu.wpi.first.wpilibj.command.CommandGroup; import edu.wpi.first.wpilibj.command.WaitCommand; @@ -12,48 +13,53 @@ */ public class RunScript extends CommandGroup { - public RunScript(String scriptName) { - ArrayList script = Robot.autoScripts.getOrDefault(scriptName, new ArrayList()); - - outerloop: - for(String[] cmd : script) { - String cmdName = cmd[0]; - String cmdArgs = cmd[1]; - - switch (cmdName) { - case "moveto": - addSequential(new AutoMoveTo(cmdArgs.split(" "))); - break; - case "turn": - addSequential(new AutoTurn(Double.parseDouble(cmdArgs))); - break; - case "move": - addSequential(new AutoMove(Double.parseDouble(cmdArgs))); - break; - case "switch": - addSequential(new EjectToSwitch()); - break; - case "scale": - addSequential(new EjectToScale()); - break; - case "exchange": - addSequential(new EjectToExchange()); - break; - case "wait": - addSequential(new WaitCommand(Double.parseDouble(cmdArgs))); - break; - case "intake": - addSequential(new IntakeCube()); - break; - case "jump": - addSequential(new RunScript(cmdArgs)); - break; - case "end": - break outerloop; - default: - // this should never happen since AutoUtils already validates the script. - System.err.println("[ERROR] `" + cmdName + "` is not a valid command name."); - } - } - } + public RunScript(String scriptName) { + ArrayList script = Robot.autoScripts.getOrDefault(scriptName, new ArrayList()); + + outerloop: for (String[] cmd : script) { + String cmdName = cmd[0]; + String cmdArgs = cmd[1]; + + switch (cmdName) { + case "moveto": + addSequential(new AutoMoveTo(cmdArgs.split(" "))); + break; + case "turn": + double rotation = Double.parseDouble(cmdArgs); + addSequential(new PIDTurn(rotation, Robot.dt, Robot.sd, Robot.dt.getGyro())); + AutoUtils.position.setRot(rotation); + break; + case "move": + double distance = Double.parseDouble(cmdArgs); + + addSequential(new PIDMove(distance, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg())); + AutoUtils.position.setX(distance * Math.sin(Math.toRadians(AutoUtils.position.getRot()))); + AutoUtils.position.setY(distance * Math.cos(Math.toRadians(AutoUtils.position.getRot()))); + break; + case "switch": + addSequential(new EjectToSwitch()); + break; + case "scale": + addSequential(new EjectToScale()); + break; + case "exchange": + addSequential(new EjectToExchange()); + break; + case "wait": + addSequential(new WaitCommand(Double.parseDouble(cmdArgs))); + break; + case "intake": + addSequential(new IntakeCube()); + break; + case "jump": + addSequential(new RunScript(cmdArgs)); + break; + case "end": + break outerloop; + default: + // this should never happen since AutoUtils already validates the script. + System.err.println("[ERROR] `" + cmdName + "` is not a valid command name."); + } + } + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TalonDrive.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TalonDrive.java new file mode 100644 index 0000000..e5614a4 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TalonDrive.java @@ -0,0 +1,41 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class TalonDrive extends Command { + + public TalonDrive() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.dt.dtRightPIDDrive(Robot.oi.leftJoy.getY()); + Robot.dt.dtLeftPIDDrive(Robot.oi.rightJoy.getY()); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java index fb35327..1940cde 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java @@ -23,11 +23,14 @@ public TeleopDrive() { // Called just before this Command runs the first time @Override protected void initialize() { + // Robot.dt.enableVelocityPIDs(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + Robot.dt.resetVPIDInputRanges(); + Robot.dt.updatePidConstants(); Robot.dt.teleopDrive(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java index 7855dfe..e9708ca 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java @@ -13,23 +13,34 @@ import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController; import org.usfirst.frc.team199.Robot2018.commands.TeleopDrive; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Drivetrain extends Subsystem implements DrivetrainInterface { // Put methods for controlling this subsystem // here. Call these from Commands. + private final WPI_TalonSRX dtLeftMaster = RobotMap.dtLeftMaster; + private final WPI_VictorSPX dtLeftSlave = RobotMap.dtLeftSlave; + private final WPI_TalonSRX dtRightMaster = RobotMap.dtRightMaster; + private final WPI_VictorSPX dtRightSlave = RobotMap.dtRightSlave; private final Encoder leftEncDist = RobotMap.leftEncDist; private final Encoder rightEncDist = RobotMap.rightEncDist; + private final Encoder leftEncRate = RobotMap.leftEncRate; + private final Encoder rightEncRate = RobotMap.rightEncRate; private final PIDSourceAverage distEncAvg = RobotMap.distEncAvg; - private final SpeedControllerGroup dtLeft = RobotMap.dtLeft; - private final SpeedControllerGroup dtRight = RobotMap.dtRight; + public final SpeedControllerGroup dtLeft = RobotMap.dtLeft; + public final SpeedControllerGroup dtRight = RobotMap.dtRight; private final DifferentialDrive robotDrive = RobotMap.robotDrive; private final VelocityPIDController leftVelocityController = RobotMap.leftVelocityController; private final VelocityPIDController rightVelocityController = RobotMap.rightVelocityController; @@ -42,6 +53,98 @@ public void initDefaultCommand() { setDefaultCommand(new TeleopDrive()); } + public PIDSourceAverage getDistEncAvg() { + return distEncAvg; + } + + public double getEncAvgDist() { + return distEncAvg.pidGet(); + } + + public double getLeftDist() { + return leftEncDist.getDistance(); + } + + public double getRightDist() { + return rightEncDist.getDistance(); + } + + public void setLeft(double spd) { + dtLeft.set(spd); + } + + public void setRight(double spd) { + dtRight.set(spd); + } + + /** + * Use for testing only (i.e. when not going through robotDrive) + */ + public void setVPIDs(double realSpd) { + leftVelocityController.set(realSpd); + rightVelocityController.set(-realSpd); + } + + public double getLeftVPIDerror() { + return leftVelocityController.getError(); + } + + public double getRightVPIDerror() { + return rightVelocityController.getError(); + } + + public double getLeftVPIDSetpoint() { + return leftVelocityController.get(); + } + + public double getRightVPIDSetpoint() { + return rightVelocityController.get(); + } + + /** + * Returns the getRate() of the left encoder + * + * @return the rate of the left encoder + */ + public double getLeftEncRate() { + return leftEncRate.getRate(); + } + + /** + * Returns the getRate() of the right encoder + * + * @return the rate of the right encoder + */ + public double getRightEncRate() { + return rightEncRate.getRate(); + } + + /** + * Sets the left side of the drivetrain to use the talons' pids to run at the + * specified speed. + * + * @param value + * The speed to run at + */ + public void dtLeftPIDDrive(double value) { + double setValue = value * Robot.getConst("Units per 100ms", 3413); + dtLeftMaster.set(ControlMode.Velocity, setValue); + dtLeftSlave.set(ControlMode.Velocity, setValue); + } + + /** + * Sets the right side of the drivetrain to use the talons' pids to run at the + * specified speed. + * + * @param value + * The speed to run at + */ + public void dtRightPIDDrive(double value) { + double setValue = value * Robot.getConst("Units per 100ms", 3413); + dtRightMaster.set(ControlMode.Velocity, setValue); + dtRightSlave.set(ControlMode.Velocity, setValue); + } + /** * Drives based on joystick input and SmartDashboard values */ @@ -49,13 +152,21 @@ public void initDefaultCommand() { public void teleopDrive() { if (Robot.getBool("Arcade Drive", true)) { if (Robot.getBool("Arcade Drive Default Setup", true)) { - Robot.dt.arcadeDrive(Robot.oi.leftJoy.getY(), Robot.oi.rightJoy.getX()); + Robot.dt.arcadeDrive(-Robot.oi.leftJoy.getY(), Robot.oi.rightJoy.getX()); } else { - Robot.dt.arcadeDrive(Robot.oi.rightJoy.getY(), Robot.oi.leftJoy.getX()); + Robot.dt.arcadeDrive(-Robot.oi.rightJoy.getY(), Robot.oi.leftJoy.getX()); } } else { - Robot.dt.tankDrive(Robot.oi.leftJoy.getY(), Robot.oi.rightJoy.getY()); + Robot.dt.tankDrive(-Robot.oi.leftJoy.getY(), -Robot.oi.rightJoy.getY()); } + SmartDashboard.putNumber("Drivetrain/Left VPID Targ", leftVelocityController.getSetpoint()); + SmartDashboard.putNumber("Drivetrain/Right VPID Targ", rightVelocityController.getSetpoint()); + SmartDashboard.putNumber("Drivetrain/Current Max Speed", getCurrentMaxSpeed()); + SmartDashboard.putNumber("Drivetrain/Left Enc Dist", leftEncDist.getDistance()); + SmartDashboard.putNumber("Drivetrain/Left Enc Rate", leftEncRate.getRate()); + SmartDashboard.putNumber("Drivetrain/Right Enc Dist", rightEncDist.getDistance()); + SmartDashboard.putNumber("Drivetrain/Right Enc Rate", rightEncRate.getRate()); + SmartDashboard.putNumber("Drivetrain/Enc Avg Dist", distEncAvg.pidGet()); } /** @@ -111,10 +222,8 @@ public double getDtRightSpeed() { */ @Override public void updatePidConstants() { - leftVelocityController.setPID(Robot.getConst("VelocityLeftkP", 1), Robot.getConst("VelocityLeftkI", 0), - Robot.getConst("VelocityLeftkD", 0)); - rightVelocityController.setPID(Robot.getConst("VelocityRightkP", 1), Robot.getConst("VelocityRightkI", 0), - Robot.getConst("VelocityRightkD", 0)); + leftVelocityController.setPID(Robot.getConst("VelocityLeftkI", 0), 0, calcDefkD(getCurrentMaxSpeed())); + rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), 0, calcDefkD(getCurrentMaxSpeed())); resetVelocityPIDkFConsts(); } @@ -146,6 +255,10 @@ public void resetAHRS() { fancyGyro.reset(); } + public double getGyroRate() { + return fancyGyro.getRate(); + } + /** * Used to get the yaw angle (Z-axis in degrees) that the ahrs currently reads * @@ -153,7 +266,7 @@ public void resetAHRS() { */ @Override public double getAHRSAngle() { - return fancyGyro.getAngle(); + return fancyGyro.getYaw(); } /** @@ -175,6 +288,7 @@ public void resetDistEncs() { @Override public void setDistancePerPulseLeft(double ratio) { leftEncDist.setDistancePerPulse(ratio); + leftEncRate.setDistancePerPulse(ratio); } /** @@ -187,6 +301,7 @@ public void setDistancePerPulseLeft(double ratio) { @Override public void setDistancePerPulseRight(double ratio) { rightEncDist.setDistancePerPulse(ratio); + rightEncRate.setDistancePerPulse(ratio); } /** @@ -213,15 +328,15 @@ public double getRightEncDist() { * Activates the solenoid to push the drivetrain into high or low gear. * * @param highGear - * If the solenoid is to be pushed into high gear (true, kForward) or - * low gear (false, kReverse) + * If the solenoid is to be pushed into high gear (true, kReverse) or + * low gear (false, kForward) */ @Override public void shiftGears(boolean highGear) { if (highGear ^ Robot.getBool("Drivetrain Gear Shift Low", false)) { - dtGear.set(DoubleSolenoid.Value.kForward); - } else { dtGear.set(DoubleSolenoid.Value.kReverse); + } else { + dtGear.set(DoubleSolenoid.Value.kForward); } } @@ -233,6 +348,13 @@ public void shiftGearSolenoidOff() { dtGear.set(DoubleSolenoid.Value.kOff); } + /** + * @return the gyroscope + */ + public PIDSource getGyro() { + return fancyGyro; + } + /** * Reset the kf constants for both VelocityPIDControllers based on current DT * gearing (high or low gear). @@ -247,9 +369,17 @@ public double resetVelocityPIDkFConsts() { double newKF = 1 / getCurrentMaxSpeed(); leftVelocityController.setF(newKF); rightVelocityController.setF(newKF); + SmartDashboard.putNumber("VPID kF", newKF); return newKF; } + public double resetVPIDInputRanges() { + double currentMaxSpd = getCurrentMaxSpeed(); + leftVelocityController.setInputRange(-currentMaxSpd, currentMaxSpd); + rightVelocityController.setInputRange(-currentMaxSpd, currentMaxSpd); + return currentMaxSpd; + } + /** * Gets the current max speed of the DT based on gearing (high or low gear) * @@ -263,4 +393,45 @@ public double getCurrentMaxSpeed() { return Robot.getConst("Max Low Speed", 84); } } + + /** + * Put left and right velocity controllers (PID) on SmartDashboard. + */ + @Override + public void putVelocityControllersToDashboard() { + SmartDashboard.putData("Left PID Controller", leftVelocityController); + SmartDashboard.putData("Right PID Controller", rightVelocityController); + } + + /** + * Uses SmartDashboard and math to calculate a *great* default kD + */ + public double calcDefkD(double maxSpeed) { + /* + * timeConstant is proportional to max speed of the shaft (which is the max + * speed of the cim divided by the gear reduction), half the mass (because the + * half of the drivetrain only has to support half of the robot), and radius of + * the drivetrain wheels squared. It's inversely proportional to the stall + * torque of the shaft, which is found by multiplying the stall torque of the + * motor with the gear reduction by the amount of motors. + */ + double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392) + : Robot.getConst("Low Gear Gear Reduction", 12.255); + double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635); + double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction / 60 * 2 * Math.PI + * convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius + / (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2); + double cycleTime = Robot.getConst("Code cycle time", 0.05); + /* + * The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is + * one. + */ + double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1; + return 1 / denominator / maxSpeed; + } + + private double convertNtokG(double newtons) { + // weight / accel due to grav = kg + return newtons / 9.81; + } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java index 8d3d842..2bab735 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/DrivetrainInterface.java @@ -1,9 +1,25 @@ package org.usfirst.frc.team199.Robot2018.subsystems; +import edu.wpi.first.wpilibj.PIDSource; + public interface DrivetrainInterface { public void initDefaultCommand(); + /** + * Returns the getRate() of the left encoder + * + * @return the rate of the left encoder + */ + public double getLeftEncRate(); + + /** + * Returns the getRate() of the right encoder + * + * @return the rate of the right encoder + */ + public double getRightEncRate(); + /** * Drives based on joystick input and SmartDashboard values */ @@ -67,6 +83,8 @@ public interface DrivetrainInterface { */ public void resetAHRS(); + public double getGyroRate(); + /** * Used to get the yaw angle (Z-axis in degrees) that the ahrs currently reads * @@ -125,6 +143,13 @@ public interface DrivetrainInterface { */ public void shiftGearSolenoidOff(); + /** + * Returns the gyroscope + * + * @return the gyroscope + */ + public PIDSource getGyro(); + /** * Reset the kf constants for both VelocityPIDControllers based on current DT * gearing (high or low gear). @@ -142,4 +167,9 @@ public interface DrivetrainInterface { * @return the current max speed of the DT in inches/second */ public double getCurrentMaxSpeed(); + + /** + * Put left and right velocity controllers (PID) on SmartDashboard. + */ + public void putVelocityControllersToDashboard(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java index e850424..a8b44ff 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java @@ -1,67 +1,130 @@ package org.usfirst.frc.team199.Robot2018.subsystems; +import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.RobotMap; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PowerDistributionPanel; +import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.command.Subsystem; /** * */ public class IntakeEject extends Subsystem implements IntakeEjectInterface { - - private final WPI_TalonSRX intakeMotor = RobotMap.intakeMotor; - - - + private final PowerDistributionPanel pdp = RobotMap.pdp; + private final VictorSP leftIntakeMotor = RobotMap.leftIntakeMotor; + private final VictorSP rightIntakeMotor = RobotMap.rightIntakeMotor; + private final DoubleSolenoid leftVerticalSolenoid = RobotMap.leftIntakeVerticalSolenoid; + private final DoubleSolenoid rightVerticalSolenoid = RobotMap.rightIntakeVerticalSolenoid; + private final DoubleSolenoid leftHorizontalSolenoid = RobotMap.leftIntakeHorizontalSolenoid; + private final DoubleSolenoid rightHorizontalSolenoid = RobotMap.rightIntakeHorizontalSolenoid; + /** * Set the default command for a subsystem here. - * */ - public void initDefaultCommand() { - // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); - } + */ + public void initDefaultCommand() { + } - /** - * returns current motor value + /** + * @return current left motor value */ - public double getIntakeSpeed() { - return intakeMotor.get(); + public double getLeftIntakeSpeed() { + return leftIntakeMotor.get(); } - + /** - * Uses (insert sensor here) to detect - * a cube in front of the robot. + * @return current right motor value */ - public boolean seeCube() { - return false; + public double getRightIntakeSpeed() { + return rightIntakeMotor.get(); } - + /** - * Uses (insert sensor here) to detect if - * the cube is currently inside the robot + * Uses current to check if the wheels are blocked aka the cube is inside the + * robot * */ public boolean hasCube() { - return false; + return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 38) + && pdp.getCurrent(Robot.rmap.getPort("PDP Intake Right Channel", 11)) > Robot.getConst("Max Current", + 38); } - + /** * stops the motors * */ public void stopIntake() { - intakeMotor.stopMotor(); + leftIntakeMotor.stopMotor(); + rightIntakeMotor.stopMotor(); } - + /** * Spins the rollers - * @param speed - positive -> rollers in, negative -> rollers out + * + * @param speed + * - positive -> rollers in, negative -> rollers out */ public void runIntake(double speed) { - + double actualSpeed = speed * Robot.getConst("Intake Motor Speed Multiplier", 1); + leftIntakeMotor.set(actualSpeed); + rightIntakeMotor.set(actualSpeed); } - -} + /** + * Raises the intake + */ + public void raiseIntake() { + DoubleSolenoid.Value leftSet = Robot.getBool("Intake Left Vertical Solenoid Inverted", false) + ? DoubleSolenoid.Value.kReverse + : DoubleSolenoid.Value.kForward; + DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Vertical Solenoid Inverted", false) + ? DoubleSolenoid.Value.kReverse + : DoubleSolenoid.Value.kForward; + leftVerticalSolenoid.set(leftSet); + rightVerticalSolenoid.set(rightSet); + } + + /** + * Lowers the intake + */ + public void lowerIntake() { + DoubleSolenoid.Value leftSet = Robot.getBool("Intake Left Vertical Solenoid Inverted", false) + ? DoubleSolenoid.Value.kForward + : DoubleSolenoid.Value.kReverse; + DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Vertical Solenoid Inverted", false) + ? DoubleSolenoid.Value.kForward + : DoubleSolenoid.Value.kReverse; + leftVerticalSolenoid.set(leftSet); + rightVerticalSolenoid.set(rightSet); + } + + /** + * Opens the intake + */ + public void openIntake() { + DoubleSolenoid.Value leftSet = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) + ? DoubleSolenoid.Value.kReverse + : DoubleSolenoid.Value.kForward; + DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) + ? DoubleSolenoid.Value.kReverse + : DoubleSolenoid.Value.kForward; + leftHorizontalSolenoid.set(leftSet); + rightHorizontalSolenoid.set(rightSet); + } + + /** + * Closes the intake + */ + public void closeIntake() { + DoubleSolenoid.Value leftSet = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) + ? DoubleSolenoid.Value.kForward + : DoubleSolenoid.Value.kReverse; + DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) + ? DoubleSolenoid.Value.kForward + : DoubleSolenoid.Value.kReverse; + leftHorizontalSolenoid.set(leftSet); + rightHorizontalSolenoid.set(rightSet); + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java index 428e1f8..0262727 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java @@ -1,41 +1,60 @@ package org.usfirst.frc.team199.Robot2018.subsystems; public interface IntakeEjectInterface { - + /** * Set the default command for a subsystem here. - * */ + */ public void initDefaultCommand(); - + /** - * returns current motor value + * returns current left motor value */ - public double getIntakeSpeed(); - + public double getLeftIntakeSpeed(); + /** - * Uses (insert sensor here) to detect - * a cube in front of the robot. + * returns current right motor value */ - public boolean seeCube(); - + public double getRightIntakeSpeed(); + /** - * Uses (insert sensor here) to detect if - * the cube is currently inside the robot + * Uses current to check if the wheels are blocked aka the cube is inside the + * robot * */ public boolean hasCube(); - + /** * stops the motors * */ public void stopIntake(); - + /** * Spins the rollers - * @param speed - positive -> rollers in, negative -> rollers out + * + * @param speed + * - positive -> rollers in, negative -> rollers out */ public void runIntake(double speed); - - + + /** + * Raises the intake + */ + public void raiseIntake(); + + /** + * Lowers the intake + */ + public void lowerIntake(); + + /** + * Closes the intake + */ + public void closeIntake(); + + /** + * Opens the intake + */ + public void openIntake(); } diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java new file mode 100644 index 0000000..27d2560 --- /dev/null +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java @@ -0,0 +1,133 @@ +package org.usfirst.frc.team199.Robot2018; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.when; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; +import org.usfirst.frc.team199.Robot2018.commands.AutoMoveTo; +import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; + +import edu.wpi.first.wpilibj.HLUsageReporting; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.internal.HardwareTimer; + +class AutoMoveToTest { + + // May need a BeforeEach to reset AutoUtils + + @BeforeEach + void setUp() { + // Since VelocityPIDController extends PIDController and PIDController calls + // static methods in wpilib that only work on robot, + // we setup these mocks to allow the tests to run off robot. + HardwareTimer tim = mock(HardwareTimer.class); + Timer.Interface timerInstance = mock(Timer.Interface.class); + when(tim.newTimer()).thenReturn(timerInstance); + Timer.SetImplementation(tim); + HLUsageReporting.Interface usageReporter = mock(HLUsageReporting.Interface.class); + HLUsageReporting.SetImplementation(usageReporter); + } + + @Test + void testWPICommand() { + Command command = new Command() { + protected boolean isFinished() { + return false; + } + }; + assertNotNull(command); + } + + @Test + void testWPICommandGroup() { + CommandGroup group = new CommandGroup(); + assertNotNull(group); + + Command command = new Command() { + protected boolean isFinished() { + return false; + } + }; + group.addSequential(command); + } + + @Test + void testPIDController() { + PIDSource source = mock(PIDSource.class); + PIDOutput output = mock(PIDOutput.class); + + PIDController ctrl = new PIDController(0, 0, 0, source, output); + assertNotNull(ctrl); + } + + // @Test + // Problem instantiating Subsystem because of SendableBase using network tables. + void testWPISubsystem() { + // LiveWindow.setEnabled(false); + Subsystem subsystem = new Subsystem() { + protected void initDefaultCommand() { + setDefaultCommand(new Command() { + protected boolean isFinished() { + return false; + } + }); + } + }; + assertNotNull(subsystem); + } + + @Test + void testForwardAndRight() { + String[] args = { "(0,12)", "(12,12)" }; + + AutoUtils.position.setRot(0); + AutoUtils.position.setX(0); + AutoUtils.position.setY(0); + + PIDSource pidGyroSrc = mock(PIDSource.class); + when(pidGyroSrc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement); + DrivetrainInterface dt = mock(DrivetrainInterface.class); + when(dt.getGyro()).thenReturn(pidGyroSrc); + SmartDashboardInterface sd = mock(SmartDashboardInterface.class); + PIDSource pidMoveSrc = mock(PIDSource.class); + + AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc); + + assertEquals(90, AutoUtils.position.getRot()); + assertEquals(12, AutoUtils.position.getX()); + assertEquals(12, AutoUtils.position.getY()); + } + + @Test + void testForward() { + String[] args = { "(0,12)" }; + + AutoUtils.position.setRot(0); + AutoUtils.position.setX(0); + AutoUtils.position.setY(0); + + PIDSource pidGyroSrc = mock(PIDSource.class); + when(pidGyroSrc.getPIDSourceType()).thenReturn(PIDSourceType.kDisplacement); + DrivetrainInterface dt = mock(DrivetrainInterface.class); + when(dt.getGyro()).thenReturn(pidGyroSrc); + SmartDashboardInterface sd = mock(SmartDashboardInterface.class); + PIDSource pidMoveSrc = mock(PIDSource.class); + + AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc); + + assertEquals(0, AutoUtils.position.getRot()); + assertEquals(0, AutoUtils.position.getX()); + assertEquals(12, AutoUtils.position.getY()); + } +} diff --git a/shuffleboard.json b/shuffleboard.json new file mode 100644 index 0000000..5d7bb03 --- /dev/null +++ b/shuffleboard.json @@ -0,0 +1,600 @@ +{ + "dividerPosition": 0.2218100890207715, + "tabPane": [ + { + "title": "PID Testing", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "0,0": { + "size": [ + 1, + 2 + ], + "content": { + "_type": "PID Controller", + "_source0": "network_table:///SmartDashboard/Left PID Controller", + "_title": "SmartDashboard/Left PID Controller" + } + }, + "1,0": { + "size": [ + 1, + 2 + ], + "content": { + "_type": "PID Controller", + "_source0": "network_table:///SmartDashboard/Right PID Controller", + "_title": "SmartDashboard/Right PID Controller" + } + }, + "0,2": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Left Enc Rate", + "_title": "SmartDashboard/Left Enc Rate", + "Visible time": 30.0, + "SmartDashboard/Left Enc Rate visible": true + } + }, + "5,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left VPID Error", + "_title": "SmartDashboard/Left VPID Error" + } + }, + "2,2": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Right Enc Rate", + "_title": "SmartDashboard/Right Enc Rate", + "Visible time": 30.0, + "SmartDashboard/Right Enc Rate visible": true + } + }, + "5,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right VPID Error", + "_title": "SmartDashboard/Right VPID Error" + } + }, + "2,0": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ", + "_title": "SmartDashboard/Drivetrain/Left VPID Targ", + "Visible time": 30.0, + "SmartDashboard/Drivetrain/Left VPID Targ visible": true + } + }, + "4,0": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ", + "_title": "SmartDashboard/Drivetrain/Right VPID Targ", + "Visible time": 30.0, + "SmartDashboard/Drivetrain/Right VPID Targ visible": true + } + }, + "4,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Const/VelocityRightkI", + "_title": "SmartDashboard/Const/VelocityRightkI" + } + }, + "4,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Const/VelocityLeftkI", + "_title": "SmartDashboard/Const/VelocityLeftkI" + } + } + } + } + }, + { + "title": "Drivetrain", + "autoPopulate": true, + "autoPopulatePrefix": "SmartDashboard/Drivetrain", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "0,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ", + "_title": "Left VPID Targ" + } + }, + "1,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ", + "_title": "Right VPID Targ" + } + }, + "2,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Current Max Speed", + "_title": "Current Max Speed" + } + }, + "3,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Left Enc Dist", + "_title": "Left Enc Dist" + } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Left Enc Rate", + "_title": "Left Enc Rate" + } + }, + "5,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Right Enc Dist", + "_title": "Right Enc Dist" + } + }, + "0,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Right Enc Rate", + "_title": "Right Enc Rate" + } + }, + "1,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Enc Avg Dist", + "_title": "Enc Avg Dist" + } + }, + "0,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Bool/High Gear", + "_title": "SmartDashboard/Bool/High Gear", + "colorWhenTrue": "#7CFC00FF", + "colorWhenFalse": "#8B0000FF" + } + }, + "2,1": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Const/DPP", + "_title": "SmartDashboard/Const/DPP" + } + } + } + } + }, + { + "title": "Move PID Testing", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "0,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Const/Move Targ", + "_title": "SmartDashboard/Const/Move Targ" + } + }, + "0,0": { + "size": [ + 1, + 2 + ], + "content": { + "_type": "PID Controller", + "_source0": "network_table:///SmartDashboard/Move PID", + "_title": "SmartDashboard/Move PID" + } + }, + "3,0": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Right Enc Dist", + "_title": "SmartDashboard/Right Enc Dist", + "Visible time": 10.0, + "SmartDashboard/Right Enc Dist visible": true + } + }, + "1,0": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Left Enc Dist", + "_title": "SmartDashboard/Left Enc Dist", + "Visible time": 10.0, + "SmartDashboard/Left Enc Dist visible": true + } + }, + "5,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Network Table Tree", + "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler", + "_title": "LiveWindow/Ungrouped/Scheduler" + } + }, + "0,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Move PID Output", + "_title": "SmartDashboard/Move PID Output" + } + }, + "1,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Move PID Result", + "_title": "SmartDashboard/Move PID Result" + } + }, + "1,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Move PID Error", + "_title": "SmartDashboard/Move PID Error" + } + }, + "2,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right VPID Error", + "_title": "SmartDashboard/Right VPID Error" + } + }, + "2,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left VPID Error", + "_title": "SmartDashboard/Left VPID Error" + } + }, + "3,2": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ", + "_title": "SmartDashboard/Drivetrain/Left VPID Targ", + "Visible time": 10.0, + "SmartDashboard/Drivetrain/Left VPID Targ visible": true + } + }, + "5,2": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ", + "_title": "SmartDashboard/Drivetrain/Right VPID Targ", + "Visible time": 10.0, + "SmartDashboard/Drivetrain/Right VPID Targ visible": true + } + }, + "5,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Const/MoveTolerance", + "_title": "SmartDashboard/Const/MoveTolerance" + } + } + } + } + }, + { + "title": "Turn PID Testing", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "5,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Network Table Tree", + "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler", + "_title": "LiveWindow/Ungrouped/Scheduler" + } + }, + "1,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Turn PID Error", + "_title": "SmartDashboard/Turn PID Error" + } + }, + "0,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Turn PID Output", + "_title": "SmartDashboard/Turn PID Output" + } + }, + "1,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Turn PID Result", + "_title": "SmartDashboard/Turn PID Result" + } + }, + "5,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Angle", + "_title": "SmartDashboard/Angle" + } + }, + "0,0": { + "size": [ + 1, + 2 + ], + "content": { + "_type": "PID Controller", + "_source0": "network_table:///SmartDashboard/Turn PID", + "_title": "SmartDashboard/Turn PID" + } + }, + "2,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left VPID Error", + "_title": "SmartDashboard/Left VPID Error" + } + }, + "2,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right VPID Error", + "_title": "SmartDashboard/Right VPID Error" + } + }, + "3,2": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Left VPID Targ", + "_title": "SmartDashboard/Drivetrain/Left VPID Targ", + "Visible time": 30.0, + "SmartDashboard/Drivetrain/Left VPID Targ visible": true + } + }, + "5,2": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Right VPID Targ", + "_title": "SmartDashboard/Drivetrain/Right VPID Targ", + "Visible time": 30.0, + "SmartDashboard/Drivetrain/Right VPID Targ visible": true + } + }, + "0,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Const/Turn Targ", + "_title": "SmartDashboard/Const/Turn Targ" + } + }, + "6,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Const/TurnTolerance", + "_title": "SmartDashboard/Const/TurnTolerance" + } + }, + "3,0": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Right Enc Dist", + "_title": "SmartDashboard/Drivetrain/Right Enc Dist", + "Visible time": 30.0, + "SmartDashboard/Drivetrain/Right Enc Dist visible": true + } + }, + "1,0": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Left Enc Dist", + "_title": "SmartDashboard/Drivetrain/Left Enc Dist", + "Visible time": 30.0, + "SmartDashboard/Drivetrain/Left Enc Dist visible": true + } + } + } + } + } + ] +} \ No newline at end of file