Merge pull request #3 from adias408/functional_sample_command

Functional sample command

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;
     }
   }
 

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();
  }
 
   /**

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();
   }
 
   /**

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.
+ *
+ * <p> Basically, a command becomes "locked" after it is first started or added to a command group.
+ * </p>
+ *
+ * <p> 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. </p>
+ */
+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);
+  }
+}

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).

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

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;
-}
 }

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);
+  }
 }