From 96f1ee2ef639ffb34d386e05187904e22fb1ab3e Mon Sep 17 00:00:00 2001 From: Antonio Dias Date: Sun, 6 Oct 2019 20:11:04 -0700 Subject: [PATCH 1/5] timed loop waking up based on schedule --- MiniLib/src/edu/wpi/first/wpilibj/TimedRobot.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/MiniLib/src/edu/wpi/first/wpilibj/TimedRobot.java b/MiniLib/src/edu/wpi/first/wpilibj/TimedRobot.java index bd3b49d..ddd5691 100755 --- a/MiniLib/src/edu/wpi/first/wpilibj/TimedRobot.java +++ b/MiniLib/src/edu/wpi/first/wpilibj/TimedRobot.java @@ -69,6 +69,10 @@ public void startCompetition() throws Exception { // m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period; // updateAlarm(); // + + // using currentTime/nextTime to replace the NotifierJNI - we could probably get NotifierJNI to work if it ever mattered + long millisToNextTime = (long)(1000.0 * m_period); + long nextTime = System.currentTimeMillis() + millisToNextTime; // Loop forever, calling the appropriate mode-dependent function while (true) { // long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier); @@ -79,7 +83,13 @@ public void startCompetition() throws Exception { // m_expirationTime += m_period; // updateAlarm(); + long currentTime = System.currentTimeMillis(); + while (currentTime < nextTime) { + Thread.sleep(nextTime - currentTime); + currentTime = System.currentTimeMillis(); + } loopFunc(); + nextTime = currentTime + millisToNextTime; } } From 79ffb9daed541c610480bcf9c570ab25f67e60a9 Mon Sep 17 00:00:00 2001 From: Antonio Dias Date: Sun, 6 Oct 2019 20:11:58 -0700 Subject: [PATCH 2/5] cleaned up Subsystem --- MiniLib/src/edu/wpi/first/wpilibj/Timer.java | 4 ++-- .../wpi/first/wpilibj/command/Command.java | 18 +++++++-------- .../wpi/first/wpilibj/command/Subsystem.java | 22 +++++++++---------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/MiniLib/src/edu/wpi/first/wpilibj/Timer.java b/MiniLib/src/edu/wpi/first/wpilibj/Timer.java index 889e735..423bb0e 100644 --- a/MiniLib/src/edu/wpi/first/wpilibj/Timer.java +++ b/MiniLib/src/edu/wpi/first/wpilibj/Timer.java @@ -17,7 +17,7 @@ public class Timer { @SuppressWarnings("AbbreviationAsWordInName") public static double getFPGATimestamp() { //return RobotController.getFPGATime() / 1000000.0; - return System.currentTimeMillis(); + return System.currentTimeMillis() / 1000.0; } /** @@ -59,7 +59,7 @@ public Timer() { } private double getMsClock() { - return System.currentTimeMillis(); // .getFPGATime() / 1000.0; + return System.currentTimeMillis(); } /** diff --git a/MiniLib/src/edu/wpi/first/wpilibj/command/Command.java b/MiniLib/src/edu/wpi/first/wpilibj/command/Command.java index 16dbeb5..3375cef 100755 --- a/MiniLib/src/edu/wpi/first/wpilibj/command/Command.java +++ b/MiniLib/src/edu/wpi/first/wpilibj/command/Command.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj.RobotState; //import edu.wpi.first.wpilibj.SendableBase; -//import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Timer; //import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** @@ -145,7 +145,7 @@ public Command(double timeout) { */ public Command(Subsystem subsystem) { this(); - //requires(subsystem); + requires(subsystem); } /** @@ -157,7 +157,7 @@ public Command(Subsystem subsystem) { */ public Command(String name, Subsystem subsystem) { this(name); - //requires(subsystem); + requires(subsystem); } /** @@ -171,7 +171,7 @@ public Command(String name, Subsystem subsystem) { */ public Command(double timeout, Subsystem subsystem) { this(timeout); - //requires(subsystem); + requires(subsystem); } /** @@ -202,7 +202,7 @@ public Command(String name, double timeout) { */ public Command(String name, double timeout, Subsystem subsystem) { this(name, timeout); - //requires(subsystem); + requires(subsystem); } /** @@ -226,7 +226,7 @@ protected final synchronized void setTimeout(double seconds) { * @return the time since this command was initialized (in seconds). */ public final synchronized double timeSinceInitialized() { - return m_startTime < 0 ? 0 : System.currentTimeMillis() - m_startTime; // TODO System.time + return m_startTime < 0 ? 0 : (System.currentTimeMillis() - m_startTime) / 1000.0; } /** @@ -242,7 +242,7 @@ public final synchronized double timeSinceInitialized() { * to a {@link CommandGroup} * @see Subsystem */ - protected synchronized void requires(Subsystem subsystem) throws Exception { + protected synchronized void requires(Subsystem subsystem) { if(!m_locked) { if (subsystem != null) { m_requirements.add(subsystem); @@ -251,7 +251,7 @@ protected synchronized void requires(Subsystem subsystem) throws Exception { } } else { - throw new Exception("Command should not be lock"); + throw new IllegalUseOfCommandException("Command should not be lock"); } } @@ -382,7 +382,7 @@ void _interrupted() {} * Command#initialize() initialize()} is, inside the {@link Command#run() run()} method. */ private void startTiming() { - // m_startTime = Timer.getFPGATimestamp(); + m_startTime = Timer.getFPGATimestamp(); } /** diff --git a/MiniLib/src/edu/wpi/first/wpilibj/command/Subsystem.java b/MiniLib/src/edu/wpi/first/wpilibj/command/Subsystem.java index d54d883..525da0b 100755 --- a/MiniLib/src/edu/wpi/first/wpilibj/command/Subsystem.java +++ b/MiniLib/src/edu/wpi/first/wpilibj/command/Subsystem.java @@ -28,7 +28,7 @@ * * @see Command */ -public abstract class Subsystem{ +public abstract class Subsystem { /** * Whether or not getDefaultCommand() was called. */ @@ -89,16 +89,16 @@ public void periodic() { * @param command the default command (or null if there should be none) * @throws IllegalUseOfCommandException if the command does not require the subsystem */ -// public void setDefaultCommand(Command command) { -// if (command == null) { -// m_defaultCommand = null; -// } else { -// if (!Collections.list(command.getRequirements()).contains(this)) { -// throw new IllegalUseOfCommandException("A default command must require the subsystem"); -// } -// m_defaultCommand = command; -// } - // } + public void setDefaultCommand(Command command) { + if (command == null) { + m_defaultCommand = null; + } else { + if (!Collections.list(command.getRequirements()).contains(this)) { + throw new IllegalUseOfCommandException("A default command must require the subsystem"); + } + m_defaultCommand = command; + } + } /** * Returns the default command (or null if there is none). From 09c905e8702d43b1fa85da76eff4f36273412be7 Mon Sep 17 00:00:00 2001 From: Antonio Dias Date: Sun, 6 Oct 2019 20:12:42 -0700 Subject: [PATCH 3/5] ready to test --- MiniLib/src/frc/team670/robot/Robot.java | 23 +++++++------------ .../team670/robot/subsystems/DriveBase.java | 10 +++++++- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/MiniLib/src/frc/team670/robot/Robot.java b/MiniLib/src/frc/team670/robot/Robot.java index 838f04f..2eae134 100755 --- a/MiniLib/src/frc/team670/robot/Robot.java +++ b/MiniLib/src/frc/team670/robot/Robot.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; +import frc.team670.robot.commands.drive.TimeDrive; import frc.team670.robot.subsystems.DriveBase; import frc.team670.robot.utils.Logger; @@ -26,7 +27,7 @@ public class Robot extends TimedRobot { public static DriveBase driveBase = new DriveBase(); - private Command autonomousCommand, operatorControl; +// private Command autonomousCommand, operatorControl; public Robot() { } @@ -43,22 +44,16 @@ public void robotInit() { oi = new OI(); - try - { - Logger.CustomLogger.setup(); + try { + Logger.CustomLogger.setup(); + } catch (Throwable e) { + Logger.logException(e); } - catch (Throwable e) { Logger.logException(e);} Logger.consoleLog(); - - Logger.consoleLog(); - System.out.println("Robot init"); - - - System.out.println("LED Setup Run"); - - // autonomousCommand = oi.getSelectedAutonCommand(); + // The command we want to test goes here + Scheduler.getInstance().add(new TimeDrive(5, 0.2)); } /** @@ -116,8 +111,6 @@ public void teleopInit() { // Scheduler.getInstance().add(new SafelyResetExtension()); // } - //TODO Test cocmmand goes here - Logger.consoleLog("Teleop Started"); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/MiniLib/src/frc/team670/robot/subsystems/DriveBase.java b/MiniLib/src/frc/team670/robot/subsystems/DriveBase.java index 1e2fa8a..4054d5a 100755 --- a/MiniLib/src/frc/team670/robot/subsystems/DriveBase.java +++ b/MiniLib/src/frc/team670/robot/subsystems/DriveBase.java @@ -11,12 +11,15 @@ import java.util.Arrays; import java.util.List; +import edu.wpi.first.wpilibj.command.Subsystem; +import frc.team670.robot.commands.drive.TimeDrive; + /** * Represents a tank drive base. * * @author lakshbhambhani */ -public class DriveBase { +public class DriveBase extends Subsystem { public DriveBase() { @@ -85,4 +88,9 @@ public void stop() { // SmartDashboard.putString("Left Encoder Inches", "null"); // } // } + + @Override + public void initDefaultCommand() { + setDefaultCommand(null); + } } \ No newline at end of file From 813adca54929c2ac469cb3c88c449947ea04d475 Mon Sep 17 00:00:00 2001 From: Antonio Dias Date: Sun, 6 Oct 2019 20:12:59 -0700 Subject: [PATCH 4/5] add exception back in --- .../command/IllegalUseOfCommandException.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 MiniLib/src/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java diff --git a/MiniLib/src/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java b/MiniLib/src/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java new file mode 100644 index 0000000..d46eb4f --- /dev/null +++ b/MiniLib/src/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.command; + +/** + * This exception will be thrown if a command is used illegally. There are several ways for this to + * happen. + * + *

Basically, a command becomes "locked" after it is first started or added to a command group. + *

+ * + *

This exception should be thrown if (after a command has been locked) its requirements change, + * it is put into multiple command groups, it is started from outside its command group, or it adds + * a new child.

+ */ +public class IllegalUseOfCommandException extends RuntimeException { + /** + * Instantiates an {@link IllegalUseOfCommandException}. + */ + public IllegalUseOfCommandException() { + } + + /** + * Instantiates an {@link IllegalUseOfCommandException} with the given message. + * + * @param message the message + */ + public IllegalUseOfCommandException(String message) { + super(message); + } +} \ No newline at end of file From a6a424ea2936836b6f394a1d8e41980bdeaf30f5 Mon Sep 17 00:00:00 2001 From: Antonio Dias Date: Sun, 6 Oct 2019 20:13:22 -0700 Subject: [PATCH 5/5] test command --- .../robot/commands/drive/TimeDrive.java | 28 ++++++++----------- 1 file changed, 11 insertions(+), 17 deletions(-) diff --git a/MiniLib/src/frc/team670/robot/commands/drive/TimeDrive.java b/MiniLib/src/frc/team670/robot/commands/drive/TimeDrive.java index 3a5a436..acd994c 100755 --- a/MiniLib/src/frc/team670/robot/commands/drive/TimeDrive.java +++ b/MiniLib/src/frc/team670/robot/commands/drive/TimeDrive.java @@ -8,6 +8,7 @@ package frc.team670.robot.commands.drive; import edu.wpi.first.wpilibj.command.Command; +import frc.team670.robot.Robot; import frc.team670.robot.utils.Logger; /** @@ -22,35 +23,34 @@ public TimeDrive(double seconds, double speed) { this.speed = speed; this.seconds = seconds; executeCounter = 0; - //requires(Robot.driveBase); + requires(Robot.driveBase); } // Called just before this Command runs the first time @Override protected void initialize() { setTimeout(seconds); - //Logger.consoleLog("Speed: %s Seconds: %s", speed, seconds); + Logger.consoleLog("Speed: %s Seconds: %s", speed, seconds); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - //Robot.driveBase.tankDrive(speed, speed, false); - //Logger.consoleLog(); - + Robot.driveBase.tankDrive(speed, speed, false); + Logger.consoleLog(); } // Make this return true when this Command no longer needs to run execute() - //@Override -// protected boolean isFinished() { -// //return isTimedOut(); -// } + @Override + protected boolean isFinished() { + return isTimedOut(); + } // Called once after isFinished returns true @Override protected void end() { - // Robot.driveBase.stop(); - //ogger.consoleLog("Speed: %s Seconds: %s", speed, seconds); + Robot.driveBase.stop(); + Logger.consoleLog("Speed: %s Seconds: %s", speed, seconds); } // Called when another command which requires one or more of the same @@ -59,10 +59,4 @@ protected void end() { protected void interrupted() { end(); } - -@Override -protected boolean isFinished() { - // TODO Auto-generated method stub - return false; -} }