From 6bde9b08cd2b97741bd6adf94a2bd1dcb35f0c99 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 15 Apr 2017 13:41:48 -0400 Subject: [PATCH 01/71] Everyone dies alone. --- .../sprocket/utils/CameraServers.java | 406 ------------------ 1 file changed, 406 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/utils/CameraServers.java diff --git a/src/org/montclairrobotics/sprocket/utils/CameraServers.java b/src/org/montclairrobotics/sprocket/utils/CameraServers.java deleted file mode 100644 index 4143400..0000000 --- a/src/org/montclairrobotics/sprocket/utils/CameraServers.java +++ /dev/null @@ -1,406 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2016. 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 org.montclairrobotics.sprocket.utils; -/** - * The FIRST CameraServer library, modified for multiple USB cameras - * - * USAGE: - * construct with an array of camera names - * call start to start broadcast - * - * call switchTo() to cycle through cameras, - * or call switchTo(id) to jump to one camera - */ -import java.io.DataInputStream; -import java.io.DataOutputStream; -import java.io.IOException; -import java.net.InetSocketAddress; -import java.net.ServerSocket; -import java.net.Socket; -import java.nio.ByteBuffer; -import java.util.ArrayDeque; -import java.util.Deque; - -import com.ni.vision.NIVision; -import com.ni.vision.NIVision.Image; -import com.ni.vision.NIVision.RawData; -import com.ni.vision.VisionException; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.vision.USBCamera; - -// replicates CameraServer.cpp in java lib - -public class CameraServers { - - private static final int kPort = 1180; - private static final byte[] kMagicNumber = {0x01, 0x00, 0x00, 0x00}; - private static final int kSize640x480 = 0; - private static final int kSize320x240 = 1; - private static final int kSize160x120 = 2; - private static final int kHardwareCompression = -1; - private static final String kDefaultCameraName = "cam1"; - private static final int kMaxImageSize = 200000; - - private Thread serverThread; - private int m_quality; - private boolean m_autoCaptureStarted; - private boolean m_hwClient = true; - private USBCamera m_camera; - - private USBCamera[] cams; - private String[] camNames; - private int i=0; - - private CameraData m_imageData; - private Deque m_imageDataPool; - - private class CameraData { - RawData data; - int start; - - public CameraData(RawData d, int s) { - data = d; - start = s; - } - } - - public CameraServers(String... camNamesInput) { - cams=new USBCamera[camNamesInput.length]; - camNames=camNamesInput; - m_quality = 50; - m_camera = null; - m_imageData = null; - m_imageDataPool = new ArrayDeque<>(3); - for (int i = 0; i < 3; i++) { - m_imageDataPool.addLast(ByteBuffer.allocateDirect(kMaxImageSize)); - } - serverThread = new Thread(new Runnable() { - public void run() { - try { - serve(); - } catch (IOException e) { - // do stuff here - } catch (InterruptedException e) { - // do stuff here - } - } - }); - serverThread.setName("CameraServer Send Thread"); - serverThread.start(); - } - - private synchronized void setImageData(RawData data, int start) { - if (m_imageData != null && m_imageData.data != null) { - m_imageData.data.free(); - if (m_imageData.data.getBuffer() != null) - m_imageDataPool.addLast(m_imageData.data.getBuffer()); - m_imageData = null; - } - m_imageData = new CameraData(data, start); - notifyAll(); - } - - /** - * Manually change the image that is served by the MJPEG stream. This can be - * called to pass custom annotated images to the dashboard. Note that, for - * 640x480 video, this method could take between 40 and 50 milliseconds to - * complete. - * - * - * - * @param image The IMAQ image to show on the dashboard - */ - public void setImage(Image image) { - // handle multi-threadedness - - /* Flatten the IMAQ image to a JPEG */ - - RawData data = - NIVision.imaqFlatten(image, NIVision.FlattenType.FLATTEN_IMAGE, - NIVision.CompressionType.COMPRESSION_JPEG, 10 * m_quality); - ByteBuffer buffer = data.getBuffer(); - boolean hwClient; - - synchronized (this) { - hwClient = m_hwClient; - } - - /* Find the start of the JPEG data */ - int index = 0; - if (hwClient) { - while (index < buffer.limit() - 1) { - if ((buffer.get(index) & 0xff) == 0xFF && (buffer.get(index + 1) & 0xff) == 0xD8) - break; - index++; - } - } - - if (buffer.limit() - index - 1 <= 2) { - throw new VisionException("data size of flattened image is less than 2. Try another camera! "); - } - - setImageData(data, index); - } - - /** - * Start automatically capturing images to send to the dashboard. You should - * call this method to just see a camera feed on the dashboard without doing - * any vision processing on the roboRIO. - */ - /*public void startAutomaticCapture() { - startAutomaticCapture(USBCamera.kDefaultCameraName); - }*/ - - public void start() - { - for(int i=0;i=cams.length||cams[id]==null)return; - //pause - synchronized(this) { - m_camera.stopCapture(); - m_camera=cams[id]; - m_camera.startCapture(); - } - //resume - } - public void switchTo() - { - i=(i+1)%cams.length; - switchTo(i); - } - - /** - * check if auto capture is started - *@return if auto capture is started - */ - public synchronized boolean isAutoCaptureStarted() { - return m_autoCaptureStarted; - } - - /** - * Sets the size of the image to use. Use the public kSize constants to set - * the correct mode, or set it directory on a camera and call the appropriate - * autoCapture method - *$ - * @param size The size to use - */ - public synchronized void setSize(int size) { - if (m_camera == null) - return; - switch (size) { - case kSize640x480: - m_camera.setSize(640, 480); - break; - case kSize320x240: - m_camera.setSize(320, 240); - break; - case kSize160x120: - m_camera.setSize(160, 120); - break; - } - } - - /** - * Set the quality of the compressed image sent to the dashboard - * - * @param quality The quality of the JPEG image, from 0 to 100 - */ - public synchronized void setQuality(int quality) { - m_quality = quality > 100 ? 100 : quality < 0 ? 0 : quality; - } - - /** - * Get the quality of the compressed image sent to the dashboard - * - * @return The quality, from 0 to 100 - */ - public synchronized int getQuality() { - return m_quality; - } - - /** - * Run the M-JPEG server. - * - * This function listens for a connection from the dashboard in a background - * thread, then sends back the M-JPEG stream. - * - * @throws IOException if the Socket connection fails - * @throws InterruptedException if the sleep is interrupted - */ - protected void serve() throws IOException, InterruptedException { - - ServerSocket socket = new ServerSocket(); - socket.setReuseAddress(true); - InetSocketAddress address = new InetSocketAddress(kPort); - socket.bind(address); - - while (true) { - try { - Socket s = socket.accept(); - - DataInputStream is = new DataInputStream(s.getInputStream()); - DataOutputStream os = new DataOutputStream(s.getOutputStream()); - - int fps = is.readInt(); - int compression = is.readInt(); - int size = is.readInt(); - - if (compression != kHardwareCompression) { - DriverStation.reportError("Choose \"USB Camera HW\" on the dashboard", false); - s.close(); - continue; - } - - // Wait for the camera - synchronized (this) { - System.out.println("Camera not yet ready, awaiting image"); - if (m_camera == null) - wait(); - m_hwClient = compression == kHardwareCompression; - if (!m_hwClient) - setQuality(100 - compression); - else if (m_camera != null) - m_camera.setFPS(fps); - setSize(size); - } - - long period = (long) (1000 / (1.0 * fps)); - while (true) { - long t0 = System.currentTimeMillis(); - CameraData imageData = null; - synchronized (this) { - wait(); - imageData = m_imageData; - m_imageData = null; - } - - if (imageData == null) - continue; - // Set the buffer position to the start of the data, - // and then create a new wrapper for the data at - // exactly that position - imageData.data.getBuffer().position(imageData.start); - byte[] imageArray = new byte[imageData.data.getBuffer().remaining()]; - imageData.data.getBuffer().get(imageArray, 0, imageData.data.getBuffer().remaining()); - - // write numbers - try { - os.write(kMagicNumber); - os.writeInt(imageArray.length); - os.write(imageArray); - os.flush(); - long dt = System.currentTimeMillis() - t0; - - if (dt < period) { - Thread.sleep(period - dt); - } - } catch (IOException | UnsupportedOperationException ex) { - DriverStation.reportError(ex.getMessage(), true); - break; - } finally { - imageData.data.free(); - if (imageData.data.getBuffer() != null) { - synchronized (this) { - m_imageDataPool.addLast(imageData.data.getBuffer()); - } - } - } - } - } catch (IOException ex) { - DriverStation.reportError(ex.getMessage(), true); - continue; - } - } - } -} \ No newline at end of file From 1eb97f86a2bed84de978c101ad556860619073d5 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 15 Apr 2017 13:52:27 -0400 Subject: [PATCH 02/71] If you are 17, 20% of your life is already gone. --- .../sprocket/utils/DoubleInput.java | 17 ----------- .../sprocket/utils/InputDouble.java | 29 +++++++++++++++++++ 2 files changed, 29 insertions(+), 17 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/utils/DoubleInput.java create mode 100644 src/org/montclairrobotics/sprocket/utils/InputDouble.java diff --git a/src/org/montclairrobotics/sprocket/utils/DoubleInput.java b/src/org/montclairrobotics/sprocket/utils/DoubleInput.java deleted file mode 100644 index 65844af..0000000 --- a/src/org/montclairrobotics/sprocket/utils/DoubleInput.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.montclairrobotics.sprocket.utils; - -public class DoubleInput implements Input{ - - private Double val; - - public DoubleInput(double val) - { - this.val=val; - } - - @Override - public Double get() { - return val; - } - -} diff --git a/src/org/montclairrobotics/sprocket/utils/InputDouble.java b/src/org/montclairrobotics/sprocket/utils/InputDouble.java new file mode 100644 index 0000000..c238e73 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/utils/InputDouble.java @@ -0,0 +1,29 @@ +package org.montclairrobotics.sprocket.utils; + +public abstract class InputDouble extends Number implements Input{ + + @Override + public double doubleValue() { + // TODO Auto-generated method stub + return get(); + } + + @Override + public float floatValue() { + // TODO Auto-generated method stub + return get().floatValue(); + } + + @Override + public int intValue() { + // TODO Auto-generated method stub + return get().intValue(); + } + + @Override + public long longValue() { + // TODO Auto-generated method stub + return get().longValue(); + } + +} From 1f15585d425dc555bb0865c20c6a791e17652d1d Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 15 Apr 2017 14:19:44 -0400 Subject: [PATCH 03/71] Began removing WPI interactions into their own package Music recordings have been invented closer to the civil war than today --- .../montclairrobotics/sprocket/core/Motor.java | 7 +++++++ .../sprocket/drive/ControlledMotor.java | 17 +++++++++-------- .../sprocket/drive/DriveModule.java | 2 +- .../sprocket/drive/DriveTrainBuilder.java | 6 +++--- .../{motors/Motor.java => frc/FRCMotor.java} | 7 ++++--- .../sprocket/motors/Module.java | 1 + 6 files changed, 25 insertions(+), 15 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/core/Motor.java rename src/org/montclairrobotics/sprocket/{motors/Motor.java => frc/FRCMotor.java} (91%) diff --git a/src/org/montclairrobotics/sprocket/core/Motor.java b/src/org/montclairrobotics/sprocket/core/Motor.java new file mode 100644 index 0000000..9ecb7ec --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/Motor.java @@ -0,0 +1,7 @@ +package org.montclairrobotics.sprocket.core; + +public interface Motor { + + void set(double power); + +} diff --git a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java index 6775678..447875c 100644 --- a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java +++ b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java @@ -1,9 +1,9 @@ package org.montclairrobotics.sprocket.drive; +import org.montclairrobotics.sprocket.core.Motor; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.Togglable; @@ -18,8 +18,9 @@ * using Joystick inputs. If you're using button pairs, you could also use * ControlledMotor to run a shooter forward and backward using the button pair. */ -public class ControlledMotor extends Motor implements Updatable,Togglable { +public class ControlledMotor implements Updatable,Togglable { + private Motor output; private Input input; private boolean enabled=true; @@ -29,8 +30,8 @@ public class ControlledMotor extends Motor implements Updatable,Togglable { * @param motor the motor which should be controlled * @param input the input to read from */ - public ControlledMotor(SpeedController motor, Input input) { - super(motor); + public ControlledMotor(Motor motor, Input input) { + this.output=motor; this.input = input; Updater.add(this, Priority.OUTPUT); } @@ -44,8 +45,8 @@ public ControlledMotor(SpeedController motor, Input input) { * @param reverseInput this input is true if the motor should go in reverse. Can be null. * @param speed the speed at which to run the motor (whether forwards or backwards) */ - public ControlledMotor(SpeedController motor, Input forwardInput, Input reverseInput, double speed) { - super(motor); + public ControlledMotor(Motor motor, Input forwardInput, Input reverseInput, double speed) { + this.output=motor; this.input = new ButtonPairInput(forwardInput, reverseInput, speed); Updater.add(this, Priority.OUTPUT); } @@ -58,14 +59,14 @@ public ControlledMotor(SpeedController motor, Input forwardInput, Input * @param forwardInput this input is true if the motor should go forwards. Can be null. * @param reverseInput this input is true if the motor should go in reverse. Can be null. */ - public ControlledMotor(SpeedController motor, Input forwardInput, Input reverseInput) { + public ControlledMotor(Motor motor, Input forwardInput, Input reverseInput) { this(motor, forwardInput, reverseInput, 1.0); } @Override public void update() { if(enabled) - set(input.get()); + output.set(input.get()); } @Override diff --git a/src/org/montclairrobotics/sprocket/drive/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index 2d5dce2..a499f0f 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -1,11 +1,11 @@ package org.montclairrobotics.sprocket.drive; +import org.montclairrobotics.sprocket.core.Motor; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.Polar; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.motors.Module; -import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.utils.PID; import org.montclairrobotics.sprocket.utils.PID; diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java index 9f14bc1..64f8b33 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java @@ -3,12 +3,12 @@ import java.util.ArrayList; import org.montclairrobotics.sprocket.control.ArcadeDriveInput; +import org.montclairrobotics.sprocket.frc.FRCMotor; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.Polar; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.motors.Module; -import org.montclairrobotics.sprocket.motors.Motor; import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.pipeline.Pipeline; import org.montclairrobotics.sprocket.pipeline.Step; @@ -47,7 +47,7 @@ public DriveTrainBuilder addDriveModule(DriveModule module) { } public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle force, SEncoder enc, PID pid) { - modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, new Motor(motor))); + modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, new FRCMotor(motor))); return this; } @@ -56,7 +56,7 @@ public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle fo } - public DriveTrainBuilder addWheels(Vector offset, Angle force, Motor... motors) { + public DriveTrainBuilder addWheels(Vector offset, Angle force, FRCMotor... motors) { modules.add(new DriveModule(offset, new Polar(1, force), motors)); return this; } diff --git a/src/org/montclairrobotics/sprocket/motors/Motor.java b/src/org/montclairrobotics/sprocket/frc/FRCMotor.java similarity index 91% rename from src/org/montclairrobotics/sprocket/motors/Motor.java rename to src/org/montclairrobotics/sprocket/frc/FRCMotor.java index 5e39e29..f7c8164 100644 --- a/src/org/montclairrobotics/sprocket/motors/Motor.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCMotor.java @@ -1,5 +1,6 @@ -package org.montclairrobotics.sprocket.motors; +package org.montclairrobotics.sprocket.frc; +import org.montclairrobotics.sprocket.core.Motor; import org.montclairrobotics.sprocket.utils.Utils; import com.ctre.CANTalon; @@ -7,7 +8,7 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Talon; -public class Motor { +public class FRCMotor implements Motor{ public enum MotorType { CANTALON, @@ -25,7 +26,7 @@ public enum MotorType { private boolean brakeMode=true; - public Motor(SpeedController motor) { + public FRCMotor(SpeedController motor) { if(motor == null) { throw new IllegalArgumentException("SpeedController argument was null when instantiating Motor object"); } diff --git a/src/org/montclairrobotics/sprocket/motors/Module.java b/src/org/montclairrobotics/sprocket/motors/Module.java index 4db94a3..b51c0fb 100644 --- a/src/org/montclairrobotics/sprocket/motors/Module.java +++ b/src/org/montclairrobotics/sprocket/motors/Module.java @@ -1,5 +1,6 @@ package org.montclairrobotics.sprocket.motors; +import org.montclairrobotics.sprocket.core.Motor; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.PID; From 034162fbf92046bf4aefc5ec473405245c588e43 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 15 Apr 2017 15:08:14 -0400 Subject: [PATCH 04/71] The commit which will make Rafi kill me. WIP; SprocketRobot next --- .../sprocket/SprocketRobot.java | 1 - .../sprocket/auto/AutoMode.java | 1 - .../sprocket/auto/states/Disable.java | 1 - .../auto/states/DriveEncoderGyro.java | 2 - .../sprocket/auto/states/DriveEncoders.java | 1 - .../sprocket/auto/states/DriveTime.java | 2 - .../sprocket/auto/states/Enable.java | 1 - .../sprocket/auto/states/TurnGyro.java | 4 - .../sprocket/control/ArcadeDriveInput.java | 3 +- .../sprocket/control/Button.java | 80 ------------------- .../sprocket/control/ButtonListener.java | 55 +++++++++++++ .../sprocket/control/ExponentDriveInput.java | 3 +- .../control/FieldCentricDriveInput.java | 15 ++-- .../sprocket/control/JoystickYAxis.java | 10 +-- .../sprocket/control/SquaredDriveInput.java | 3 +- .../sprocket/control/ToggleButton.java | 52 ------------ .../sprocket/core/Button.java | 7 ++ .../sprocket/core/Debugger.java | 6 ++ .../sprocket/core/Encoder.java | 13 +++ .../sprocket/core/Joystick.java | 6 ++ .../sprocket/drive/ControlledMotor.java | 9 +-- .../sprocket/drive/DTInput.java | 1 - .../sprocket/drive/DriveModule.java | 1 - .../sprocket/drive/DriveTrain.java | 1 - .../sprocket/drive/DriveTrainBuilder.java | 2 - .../sprocket/drive/TankMapper.java | 1 - .../sprocket/drive/steps/AccelLimit.java | 2 - .../sprocket/drive/steps/Deadzone.java | 1 - .../sprocket/drive/steps/GyroCorrection.java | 4 - .../sprocket/drive/steps/SpeedLimiter.java | 1 - .../sprocket/drive/steps/TurnLimiter.java | 2 - .../sprocket/drive/utils/GyroLock.java | 7 -- .../{control => frc}/DashboardButton.java | 6 +- .../sprocket/frc/DashboardDebug.java | 20 +++++ .../{control => frc}/DashboardInput.java | 2 +- .../FRCButton.java} | 11 ++- .../sprocket/{utils => frc}/Grip.java | 3 +- .../{utils => frc}/GripContourReport.java | 4 +- .../sprocket/motors/Module.java | 1 - .../sprocket/motors/SEncoder.java | 22 ++--- .../sprocket/states/StateMachine.java | 2 - .../sprocket/utils/Action.java | 8 ++ .../sprocket/utils/Debug.java | 10 ++- .../sprocket/utils/PIDTuner.java | 21 +++-- .../sprocket/utils/Togglable.java | 9 --- 45 files changed, 174 insertions(+), 243 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/control/Button.java create mode 100644 src/org/montclairrobotics/sprocket/control/ButtonListener.java delete mode 100644 src/org/montclairrobotics/sprocket/control/ToggleButton.java create mode 100644 src/org/montclairrobotics/sprocket/core/Button.java create mode 100644 src/org/montclairrobotics/sprocket/core/Debugger.java create mode 100644 src/org/montclairrobotics/sprocket/core/Encoder.java create mode 100644 src/org/montclairrobotics/sprocket/core/Joystick.java rename src/org/montclairrobotics/sprocket/{control => frc}/DashboardButton.java (74%) create mode 100644 src/org/montclairrobotics/sprocket/frc/DashboardDebug.java rename src/org/montclairrobotics/sprocket/{control => frc}/DashboardInput.java (91%) rename src/org/montclairrobotics/sprocket/{control/JoystickButton.java => frc/FRCButton.java} (70%) rename src/org/montclairrobotics/sprocket/{utils => frc}/Grip.java (89%) rename src/org/montclairrobotics/sprocket/{utils => frc}/GripContourReport.java (92%) create mode 100644 src/org/montclairrobotics/sprocket/utils/Action.java delete mode 100644 src/org/montclairrobotics/sprocket/utils/Togglable.java diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/SprocketRobot.java index c00e6e8..8bc8f7a 100644 --- a/src/org/montclairrobotics/sprocket/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/SprocketRobot.java @@ -2,7 +2,6 @@ import java.util.ArrayList; import java.util.Arrays; -import java.util.List; import org.montclairrobotics.sprocket.auto.AutoMode; import org.montclairrobotics.sprocket.auto.AutoState; diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index 9f4107c..9f5ae78 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -5,7 +5,6 @@ import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Debug; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * The AutoMode class is the fundamental component to Sprocket's autonomous diff --git a/src/org/montclairrobotics/sprocket/auto/states/Disable.java b/src/org/montclairrobotics/sprocket/auto/states/Disable.java index 21082e6..b3436f9 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/Disable.java +++ b/src/org/montclairrobotics/sprocket/auto/states/Disable.java @@ -1,7 +1,6 @@ package org.montclairrobotics.sprocket.auto.states; import org.montclairrobotics.sprocket.states.State; -import org.montclairrobotics.sprocket.utils.Togglable; public class Disable implements State{ diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java index 633d447..289ba31 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java @@ -1,8 +1,6 @@ package org.montclairrobotics.sprocket.auto.states; -import org.montclairrobotics.sprocket.control.DashboardInput; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; -import org.montclairrobotics.sprocket.drive.utils.GyroLock; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Distance; diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index 99851b4..cc7676e 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -2,7 +2,6 @@ import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoState; -import org.montclairrobotics.sprocket.control.DashboardInput; import org.montclairrobotics.sprocket.drive.DriveTrain; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.XY; diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java b/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java index 68bb197..1030751 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java @@ -1,12 +1,10 @@ package org.montclairrobotics.sprocket.auto.states; -import org.montclairrobotics.sprocket.auto.AutoState; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.geometry.XY; import org.montclairrobotics.sprocket.utils.Input; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * This auto state drives for a certain amount of time (in seconds) and also diff --git a/src/org/montclairrobotics/sprocket/auto/states/Enable.java b/src/org/montclairrobotics/sprocket/auto/states/Enable.java index ed38b46..3f5fca4 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/Enable.java +++ b/src/org/montclairrobotics/sprocket/auto/states/Enable.java @@ -1,7 +1,6 @@ package org.montclairrobotics.sprocket.auto.states; import org.montclairrobotics.sprocket.states.State; -import org.montclairrobotics.sprocket.utils.Togglable; public class Enable implements State{ diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java index 35007a7..32eef33 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java @@ -1,16 +1,12 @@ package org.montclairrobotics.sprocket.auto.states; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoState; -import org.montclairrobotics.sprocket.control.DashboardInput; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; -import org.montclairrobotics.sprocket.utils.PID; public class TurnGyro extends AutoState { diff --git a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java index 58a2dac..d099925 100644 --- a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java @@ -1,8 +1,8 @@ package org.montclairrobotics.sprocket.control; +import org.montclairrobotics.sprocket.core.Joystick; import org.montclairrobotics.sprocket.drive.DTInput; import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.geometry.XY; @@ -10,7 +10,6 @@ import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; -import edu.wpi.first.wpilibj.Joystick; /** * This class takes inputs from a single Joystick and acts as an arcade drive diff --git a/src/org/montclairrobotics/sprocket/control/Button.java b/src/org/montclairrobotics/sprocket/control/Button.java deleted file mode 100644 index 789c129..0000000 --- a/src/org/montclairrobotics/sprocket/control/Button.java +++ /dev/null @@ -1,80 +0,0 @@ -package org.montclairrobotics.sprocket.control; - -import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.utils.Input; - -import edu.wpi.first.wpilibj.Joystick; - -/** - * @author MHS Robotics - * Button is a class which allows for developers to program custom behaviors for - * when buttons are pressed/held/unpressed. This class works by hooking into - * the Sprocket control loop and calling anonymous functions passed in by - * developers. This system makes defining button behaviors incredibly simple. - */ -public abstract class Button implements Updatable, Input { - - private boolean wasPressed=false; - - private ButtonAction pressAction, releaseAction, heldAction, offAction; - - public Button() - { - Updater.add(this, Priority.CONTROL); - } - - /** - * Sets what should be run when the button is pressed - * @param action - */ - public void setPressAction(ButtonAction action) { - pressAction = action; - } - - /** - * Sets what should be run when the button is released - * @param action - */ - public void setReleaseAction(ButtonAction action) { - releaseAction = action; - } - - /** - * Sets what should be run when the button is pressed and every tick while it is held - * @param action - */ - public void setHeldAction(ButtonAction action) { - heldAction = action; - } - - /** - * Sets what should be run when the button is not pressed and for every tick where that is the case - * @param action - */ - public void setOffAction(ButtonAction action) { - offAction = action; - } - - @Override - public void update() { - boolean pressed = get(); - if(pressed) { - if(heldAction != null) heldAction.onAction(); - } else { - if(offAction != null) offAction.onAction(); - } - - if(pressed && !wasPressed) { - if(pressAction != null) pressAction.onAction(); - } - if(!pressed && wasPressed) { - if(releaseAction != null) releaseAction.onAction(); - } - - wasPressed = pressed; - } - @Override - public abstract Boolean get(); -} diff --git a/src/org/montclairrobotics/sprocket/control/ButtonListener.java b/src/org/montclairrobotics/sprocket/control/ButtonListener.java new file mode 100644 index 0000000..38392f0 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/control/ButtonListener.java @@ -0,0 +1,55 @@ +package org.montclairrobotics.sprocket.control; + +import org.montclairrobotics.sprocket.loop.Priority; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Action; +import org.montclairrobotics.sprocket.utils.Input; + + +/** + * @author MHS Robotics + * Button is a class which allows for developers to program custom behaviors for + * when buttons are pressed/held/unpressed. This class works by hooking into + * the Sprocket control loop and calling anonymous functions passed in by + * developers. This system makes defining button behaviors incredibly simple. + */ +public class ButtonListener implements Updatable { + + private boolean wasPressed=false; + + private Action action; + + private Input button; + + public ButtonListener(Input button) + { + this.button=button; + Updater.add(this, Priority.CONTROL); + } + + public void setAction(Action action) + { + this.action=action; + } + + @Override + public void update() { + boolean pressed = button.get(); + if(action!=null) + { + if(pressed && !wasPressed) { + action.onEnable(); + } + if(!pressed && wasPressed) { + action.onDisable(); + } + if(pressed) { + action.enabled(); + } else { + action.disabled(); + } + } + wasPressed = pressed; + } +} diff --git a/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java b/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java index f53d4f2..3b00cb7 100644 --- a/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java @@ -1,6 +1,7 @@ package org.montclairrobotics.sprocket.control; -import edu.wpi.first.wpilibj.Joystick; +import org.montclairrobotics.sprocket.core.Joystick; + public class ExponentDriveInput extends ArcadeDriveInput { diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index ec63497..ab4e037 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -1,19 +1,14 @@ package org.montclairrobotics.sprocket.control; import org.montclairrobotics.sprocket.SprocketRobot; +import org.montclairrobotics.sprocket.core.Joystick; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.geometry.XY; -import org.montclairrobotics.sprocket.utils.Input; -import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.Togglable; +import org.montclairrobotics.sprocket.utils.Action; -import edu.wpi.first.wpilibj.Joystick; -public class FieldCentricDriveInput extends ArcadeDriveInput implements Togglable{ +public class FieldCentricDriveInput extends ArcadeDriveInput implements Action{ private GyroCorrection gyro; @@ -78,11 +73,11 @@ public Angle getTurn() { } @Override - public void enable() { + public void onEnable() { SprocketRobot.getDriveTrain().setTempInput(this); } @Override - public void disable() { + public void onDisable() { SprocketRobot.getDriveTrain().useDefaultInput(); } } diff --git a/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java b/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java index d1e8517..162f422 100644 --- a/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java +++ b/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java @@ -1,8 +1,8 @@ package org.montclairrobotics.sprocket.control; +import org.montclairrobotics.sprocket.core.Joystick; import org.montclairrobotics.sprocket.utils.Input; -import edu.wpi.first.wpilibj.Joystick; /** * This class is a pretty simple wrapper around the Joystick Y-axis which just @@ -13,15 +13,9 @@ public class JoystickYAxis implements Input { private Joystick stick; - /** - * @param port The port ID of the Joystick - */ - public JoystickYAxis(int port) { - stick = new Joystick(port); - } /** - * @param stick The WPILIB Joystick + * @param stick The Joystick */ public JoystickYAxis(Joystick stick) { this.stick = stick; diff --git a/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java b/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java index 9e0a574..c218fce 100644 --- a/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java @@ -1,6 +1,7 @@ package org.montclairrobotics.sprocket.control; -import edu.wpi.first.wpilibj.Joystick; +import org.montclairrobotics.sprocket.core.Joystick; + public class SquaredDriveInput extends ArcadeDriveInput{ diff --git a/src/org/montclairrobotics/sprocket/control/ToggleButton.java b/src/org/montclairrobotics/sprocket/control/ToggleButton.java deleted file mode 100644 index 1e80a23..0000000 --- a/src/org/montclairrobotics/sprocket/control/ToggleButton.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.montclairrobotics.sprocket.control; - -import org.montclairrobotics.sprocket.utils.Togglable; - -import edu.wpi.first.wpilibj.Joystick; - -public class ToggleButton extends JoystickButton { - - Togglable obj; - - public ToggleButton(Joystick stick, int id, Togglable togglable) { - super(stick, id); - this.obj = togglable; - - this.setPressAction(new ButtonAction() { - @Override - public void onAction() { - obj.enable(); - } - }); - - this.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - obj.disable(); - } - }); - } - - public ToggleButton(int stick, int id, Togglable togglable) { - this(new Joystick(stick), id, togglable); - } - - public ToggleButton disableWhenPressed() { - this.setPressAction(new ButtonAction() { - @Override - public void onAction() { - obj.disable(); - } - }); - - this.setReleaseAction(new ButtonAction() { - @Override - public void onAction() { - obj.enable(); - } - }); - - return this; - } - -} diff --git a/src/org/montclairrobotics/sprocket/core/Button.java b/src/org/montclairrobotics/sprocket/core/Button.java new file mode 100644 index 0000000..5662256 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/Button.java @@ -0,0 +1,7 @@ +package org.montclairrobotics.sprocket.core; + +import org.montclairrobotics.sprocket.utils.Input; + +public interface Button extends Input{ + +} diff --git a/src/org/montclairrobotics/sprocket/core/Debugger.java b/src/org/montclairrobotics/sprocket/core/Debugger.java new file mode 100644 index 0000000..a12f29c --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/Debugger.java @@ -0,0 +1,6 @@ +package org.montclairrobotics.sprocket.core; + +public interface Debugger { + public void debugStr(String key,String val); + public void debugNum(String key,double val); +} diff --git a/src/org/montclairrobotics/sprocket/core/Encoder.java b/src/org/montclairrobotics/sprocket/core/Encoder.java new file mode 100644 index 0000000..56cc24c --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/Encoder.java @@ -0,0 +1,13 @@ +package org.montclairrobotics.sprocket.core; + +import org.montclairrobotics.sprocket.utils.Input; + +public interface Encoder extends Input{ + public double getSpeed(); + public double getDistance(); + public default Double get() + { + return getSpeed(); + } + public void reset(); +} diff --git a/src/org/montclairrobotics/sprocket/core/Joystick.java b/src/org/montclairrobotics/sprocket/core/Joystick.java new file mode 100644 index 0000000..a624a40 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/Joystick.java @@ -0,0 +1,6 @@ +package org.montclairrobotics.sprocket.core; + +public interface Joystick { + public double getX(); + public double getY(); +} diff --git a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java index 447875c..a19b150 100644 --- a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java +++ b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java @@ -4,10 +4,9 @@ import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Input; -import org.montclairrobotics.sprocket.utils.Togglable; -import edu.wpi.first.wpilibj.SpeedController; /** * ControlledMotor is a wrapper class for Motors which allows a motor to be @@ -18,7 +17,7 @@ * using Joystick inputs. If you're using button pairs, you could also use * ControlledMotor to run a shooter forward and backward using the button pair. */ -public class ControlledMotor implements Updatable,Togglable { +public class ControlledMotor implements Updatable,Action { private Motor output; private Input input; @@ -70,12 +69,12 @@ public void update() { } @Override - public void enable() { + public void onEnable() { enabled=true; } @Override - public void disable() { + public void onDisable() { enabled=false; } diff --git a/src/org/montclairrobotics/sprocket/drive/DTInput.java b/src/org/montclairrobotics/sprocket/drive/DTInput.java index 997cbdf..7314257 100644 --- a/src/org/montclairrobotics/sprocket/drive/DTInput.java +++ b/src/org/montclairrobotics/sprocket/drive/DTInput.java @@ -1,7 +1,6 @@ package org.montclairrobotics.sprocket.drive; import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.Vector; public interface DTInput { diff --git a/src/org/montclairrobotics/sprocket/drive/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index a499f0f..3970bde 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -8,7 +8,6 @@ import org.montclairrobotics.sprocket.motors.Module; import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.PID; /** * DriveModule is a class that extends Motor which provides additional behaviors diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java index 9028642..dc05c1f 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java @@ -3,7 +3,6 @@ import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Distance; -import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java index 64f8b33..9ab9014 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java @@ -5,7 +5,6 @@ import org.montclairrobotics.sprocket.control.ArcadeDriveInput; import org.montclairrobotics.sprocket.frc.FRCMotor; import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.Polar; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.motors.Module; @@ -13,7 +12,6 @@ import org.montclairrobotics.sprocket.pipeline.Pipeline; import org.montclairrobotics.sprocket.pipeline.Step; import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.PID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.SpeedController; diff --git a/src/org/montclairrobotics/sprocket/drive/TankMapper.java b/src/org/montclairrobotics/sprocket/drive/TankMapper.java index e0c7587..65ce832 100644 --- a/src/org/montclairrobotics/sprocket/drive/TankMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/TankMapper.java @@ -1,6 +1,5 @@ package org.montclairrobotics.sprocket.drive; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.utils.Debug; diff --git a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java index ce1c17c..aea5c90 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java @@ -1,6 +1,5 @@ package org.montclairrobotics.sprocket.drive.steps; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; @@ -10,7 +9,6 @@ import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.pipeline.Step; import org.montclairrobotics.sprocket.utils.Debug; -import org.montclairrobotics.sprocket.utils.Togglable; import org.montclairrobotics.sprocket.utils.Utils; public class AccelLimit implements Step,Togglable{ diff --git a/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java b/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java index 464f893..dd3cce7 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java @@ -1,6 +1,5 @@ package org.montclairrobotics.sprocket.drive.steps; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Radians; diff --git a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java index 72849c7..dcb5391 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java @@ -1,17 +1,13 @@ package org.montclairrobotics.sprocket.drive.steps; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Radians; -import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.pipeline.Step; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.Togglable; import org.montclairrobotics.sprocket.utils.Utils; public class GyroCorrection implements Step, Togglable { diff --git a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java index 5157937..a74236c 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java @@ -3,7 +3,6 @@ import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Togglable; public class SpeedLimiter implements Step, Togglable{ diff --git a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java index eb6ddaf..ba58bef 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java @@ -3,9 +3,7 @@ import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Radians; -import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Togglable; public class TurnLimiter implements Step, Togglable{ diff --git a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java index 4238e56..a779a9a 100644 --- a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java +++ b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java @@ -1,18 +1,11 @@ package org.montclairrobotics.sprocket.drive.utils; -import org.montclairrobotics.sprocket.SprocketRobot; -import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.pipeline.Step; import org.montclairrobotics.sprocket.utils.Debug; -import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.Togglable; public class GyroLock implements Updatable, Togglable { diff --git a/src/org/montclairrobotics/sprocket/control/DashboardButton.java b/src/org/montclairrobotics/sprocket/frc/DashboardButton.java similarity index 74% rename from src/org/montclairrobotics/sprocket/control/DashboardButton.java rename to src/org/montclairrobotics/sprocket/frc/DashboardButton.java index 72d4696..18bed23 100644 --- a/src/org/montclairrobotics/sprocket/control/DashboardButton.java +++ b/src/org/montclairrobotics/sprocket/frc/DashboardButton.java @@ -1,9 +1,11 @@ -package org.montclairrobotics.sprocket.control; +package org.montclairrobotics.sprocket.frc; + +import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class DashboardButton extends Button{ +public class DashboardButton implements Input{ Command c; diff --git a/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java new file mode 100644 index 0000000..22e338f --- /dev/null +++ b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java @@ -0,0 +1,20 @@ +package org.montclairrobotics.sprocket.frc; + +import org.montclairrobotics.sprocket.core.Debugger; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class DashboardDebug implements Debugger{ + + @Override + public void debugStr(String key, String val) { + SmartDashboard.putString(key, val); + + } + + @Override + public void debugNum(String key, double val) { + SmartDashboard.putNumber(key, val); + + } +} diff --git a/src/org/montclairrobotics/sprocket/control/DashboardInput.java b/src/org/montclairrobotics/sprocket/frc/DashboardInput.java similarity index 91% rename from src/org/montclairrobotics/sprocket/control/DashboardInput.java rename to src/org/montclairrobotics/sprocket/frc/DashboardInput.java index 6592bac..7c95b4d 100644 --- a/src/org/montclairrobotics/sprocket/control/DashboardInput.java +++ b/src/org/montclairrobotics/sprocket/frc/DashboardInput.java @@ -1,4 +1,4 @@ -package org.montclairrobotics.sprocket.control; +package org.montclairrobotics.sprocket.frc; import org.montclairrobotics.sprocket.utils.Input; diff --git a/src/org/montclairrobotics/sprocket/control/JoystickButton.java b/src/org/montclairrobotics/sprocket/frc/FRCButton.java similarity index 70% rename from src/org/montclairrobotics/sprocket/control/JoystickButton.java rename to src/org/montclairrobotics/sprocket/frc/FRCButton.java index 9f91f12..7f09cad 100644 --- a/src/org/montclairrobotics/sprocket/control/JoystickButton.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCButton.java @@ -1,11 +1,10 @@ -package org.montclairrobotics.sprocket.control; +package org.montclairrobotics.sprocket.frc; -import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.Joystick; -public class JoystickButton extends Button{ +public class FRCButton implements Input{ private Joystick stick; private int id; @@ -16,7 +15,7 @@ public class JoystickButton extends Button{ * @param buttonId The raw button ID of the button you're binding to. On most * Joysticks the ID is specified on the buttons themselves. */ - public JoystickButton(Joystick stick, int buttonId) { + public FRCButton(Joystick stick, int buttonId) { this.stick = stick; this.id = buttonId; } @@ -27,7 +26,7 @@ public JoystickButton(Joystick stick, int buttonId) { * @param buttonId The raw button ID of the button you're binding to. On most * Joysticks the ID is specified on the buttons themselves. */ - public JoystickButton(int stick, int buttonId) { + public FRCButton(int stick, int buttonId) { this(new Joystick(stick), buttonId); } @Override diff --git a/src/org/montclairrobotics/sprocket/utils/Grip.java b/src/org/montclairrobotics/sprocket/frc/Grip.java similarity index 89% rename from src/org/montclairrobotics/sprocket/utils/Grip.java rename to src/org/montclairrobotics/sprocket/frc/Grip.java index b2763b7..7c81d3f 100644 --- a/src/org/montclairrobotics/sprocket/utils/Grip.java +++ b/src/org/montclairrobotics/sprocket/frc/Grip.java @@ -1,8 +1,9 @@ -package org.montclairrobotics.sprocket.utils; +package org.montclairrobotics.sprocket.frc; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Debug; import edu.wpi.first.wpilibj.networktables.NetworkTable; diff --git a/src/org/montclairrobotics/sprocket/utils/GripContourReport.java b/src/org/montclairrobotics/sprocket/frc/GripContourReport.java similarity index 92% rename from src/org/montclairrobotics/sprocket/utils/GripContourReport.java rename to src/org/montclairrobotics/sprocket/frc/GripContourReport.java index 7c31a32..fd636f4 100644 --- a/src/org/montclairrobotics/sprocket/utils/GripContourReport.java +++ b/src/org/montclairrobotics/sprocket/frc/GripContourReport.java @@ -1,4 +1,6 @@ -package org.montclairrobotics.sprocket.utils; +package org.montclairrobotics.sprocket.frc; + +import org.montclairrobotics.sprocket.utils.Debug; import edu.wpi.first.wpilibj.networktables.NetworkTable; diff --git a/src/org/montclairrobotics/sprocket/motors/Module.java b/src/org/montclairrobotics/sprocket/motors/Module.java index b51c0fb..8ff1b05 100644 --- a/src/org/montclairrobotics/sprocket/motors/Module.java +++ b/src/org/montclairrobotics/sprocket/motors/Module.java @@ -4,7 +4,6 @@ import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.PID; /** * DriveModule is a class that extends Motor which provides additional behaviors diff --git a/src/org/montclairrobotics/sprocket/motors/SEncoder.java b/src/org/montclairrobotics/sprocket/motors/SEncoder.java index f5f4559..6f92ecc 100644 --- a/src/org/montclairrobotics/sprocket/motors/SEncoder.java +++ b/src/org/montclairrobotics/sprocket/motors/SEncoder.java @@ -1,11 +1,11 @@ package org.montclairrobotics.sprocket.motors; +import org.montclairrobotics.sprocket.core.Encoder; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; -import edu.wpi.first.wpilibj.Encoder; public class SEncoder implements Input { @@ -14,7 +14,7 @@ public class SEncoder implements Input { private double ticksPerInch; - public SEncoder(int a, int b, double ticksPerInch, boolean reverse) { + /*public SEncoder(int a, int b, double ticksPerInch, boolean reverse) { enc = new Encoder(a, b, reverse); eId = a; Debug.msg("enc-" + eId, "init"); @@ -23,7 +23,7 @@ public SEncoder(int a, int b, double ticksPerInch, boolean reverse) { public SEncoder(int a, int b, double ticksPerInch) { this(a, b, ticksPerInch, false); - } + }*/ public SEncoder(Encoder e, double ticksPerInch) { enc = e; @@ -33,27 +33,27 @@ public SEncoder(Encoder e, double ticksPerInch) { - public int getTicks() { - return enc.getRaw(); + public double getRawDistance() { + return enc.getDistance(); } - public double getTickRate() { - return enc.getRate(); + public double getRawSpeed() { + return enc.getSpeed(); } - public Distance getInches() { - return new Distance(getTicks()/ticksPerInch); + public Distance getDistance() { + return new Distance(getRawDistance()/ticksPerInch); } public Distance getSpeed() { - return new Distance(getTickRate()/ticksPerInch); + return new Distance(getRawSpeed()/ticksPerInch); } public void reset() { enc.reset(); } - public Encoder getWPIEncoder() { + public Encoder getRawEncoder() { return enc; } diff --git a/src/org/montclairrobotics/sprocket/states/StateMachine.java b/src/org/montclairrobotics/sprocket/states/StateMachine.java index 8c16306..2b826e5 100644 --- a/src/org/montclairrobotics/sprocket/states/StateMachine.java +++ b/src/org/montclairrobotics/sprocket/states/StateMachine.java @@ -4,7 +4,6 @@ import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class StateMachine implements State, Updatable{ @@ -76,6 +75,5 @@ public void startState() { states[index].start(); } - SmartDashboard.putString("I hope it gets here","It does!"); } } diff --git a/src/org/montclairrobotics/sprocket/utils/Action.java b/src/org/montclairrobotics/sprocket/utils/Action.java new file mode 100644 index 0000000..cbac309 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/utils/Action.java @@ -0,0 +1,8 @@ +package org.montclairrobotics.sprocket.utils; + +public interface Action { + public default void onEnable(){} + public default void enabled(){} + public default void onDisable(){} + public default void disabled(){} +} diff --git a/src/org/montclairrobotics/sprocket/utils/Debug.java b/src/org/montclairrobotics/sprocket/utils/Debug.java index 8627e59..6bed353 100644 --- a/src/org/montclairrobotics/sprocket/utils/Debug.java +++ b/src/org/montclairrobotics/sprocket/utils/Debug.java @@ -1,21 +1,25 @@ package org.montclairrobotics.sprocket.utils; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.montclairrobotics.sprocket.core.Debugger; +import org.montclairrobotics.sprocket.frc.DashboardDebug; + public class Debug { public static boolean DEBUG_MODE=true; + public static Debugger debugger=new DashboardDebug(); + public static void num(String key,double value) { if(DEBUG_MODE) - SmartDashboard.putNumber(key, value); + debugger.debugNum(key, value); } public static void string(String key,String value) { if(DEBUG_MODE) - SmartDashboard.putString(key, value); + debugger.debugStr(key, value); } public static void msg(String key,double value) diff --git a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java index 655ef42..0a9ac83 100644 --- a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java +++ b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java @@ -1,20 +1,19 @@ package org.montclairrobotics.sprocket.utils; -import org.montclairrobotics.sprocket.control.Button; -import org.montclairrobotics.sprocket.control.ButtonAction; -import org.montclairrobotics.sprocket.control.DashboardButton; -import org.montclairrobotics.sprocket.control.DashboardInput; +import org.montclairrobotics.sprocket.control.ButtonListener; +import org.montclairrobotics.sprocket.frc.DashboardButton; +import org.montclairrobotics.sprocket.frc.DashboardInput; public class PIDTuner extends PID { private Input TempP; - private Button test; - private Button apply; - private Button run; + private Input test; + private Input apply; + private Input run; private DashboardInput realP,realI,realD; private Input cyclesPer10Sec; - public PIDTuner(Input TempP,Input cyclesPer10Sec,Button test,Button apply,Button run) + public PIDTuner(Input TempP,Input cyclesPer10Sec,Input test,Input apply,Input run) { super(); this.TempP=TempP; @@ -27,16 +26,16 @@ public PIDTuner(Input TempP,Input cyclesPer10Sec,Button test,But realI=new DashboardInput("PID Tuner I"); realD=new DashboardInput("PID Tuner D"); - apply.setPressAction(new ButtonAction(){ + new ButtonListener(apply).setAction(new Action(){ @Override - public void onAction() { + public void onEnable() { recalculatePIDs(); }}); } - public PIDTuner(Button runButton) { + public PIDTuner(Input runButton) { this(new DashboardInput("Temp P"), new DashboardInput("Cycles/10sec"), new DashboardButton("Test"), new DashboardButton("Apply"), runButton); } diff --git a/src/org/montclairrobotics/sprocket/utils/Togglable.java b/src/org/montclairrobotics/sprocket/utils/Togglable.java deleted file mode 100644 index 506f2e9..0000000 --- a/src/org/montclairrobotics/sprocket/utils/Togglable.java +++ /dev/null @@ -1,9 +0,0 @@ -package org.montclairrobotics.sprocket.utils; - -public interface Togglable { - - public void enable(); - - public void disable(); - -} From d368b8542b265d9db7939005b13b318bc7a3c293 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 15 Apr 2017 15:34:24 -0400 Subject: [PATCH 05/71] Began reimplementing FRC code in the right spot, and fiddling with the encoders --- .../sprocket/auto/states/TurnEncoders.java | 4 ++-- .../sprocket/control/ButtonAction.java | 17 ----------------- .../sprocket/core/Encoder.java | 6 +----- .../sprocket/drive/DriveTrainBuilder.java | 2 +- .../sprocket/drive/steps/AccelLimit.java | 7 ++++--- .../sprocket/drive/steps/GyroCorrection.java | 7 ++++--- .../sprocket/drive/steps/SpeedLimiter.java | 10 ++++++---- .../sprocket/drive/steps/TurnLimiter.java | 10 ++++++---- .../sprocket/drive/utils/GyroLock.java | 7 ++++--- .../sprocket/frc/FRCEncoder.java | 18 ++++++++++++++++++ .../sprocket/frc/FRCJoystick.java | 7 +++++++ .../sprocket/motors/Module.java | 2 +- 12 files changed, 54 insertions(+), 43 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/control/ButtonAction.java create mode 100644 src/org/montclairrobotics/sprocket/frc/FRCEncoder.java create mode 100644 src/org/montclairrobotics/sprocket/frc/FRCJoystick.java diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java index 4d0723e..4242f3e 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java @@ -20,7 +20,7 @@ public TurnEncoders(Angle turn, Angle turnSpeed) { @Override public void userStart() { module = SprocketRobot.getDriveTrain().getModules()[0]; - double modulePos = module.getEnc().getInches().get(); + double modulePos = module.getEnc().getDistance().get(); double finalPos = ((module.getOffset().getMagnitude() * 2 * Math.PI)/360) * turn.toDegrees(); if(module.getOffset().getX() < 0) { finalPos += modulePos; @@ -36,7 +36,7 @@ public void stateUpdate() { @Override public boolean isDone() { - return Math.abs(module.getEnc().getInches().get() - finalPos) < 1.0; + return Math.abs(module.getEnc().getDistance().get() - finalPos) < 1.0; } @Override diff --git a/src/org/montclairrobotics/sprocket/control/ButtonAction.java b/src/org/montclairrobotics/sprocket/control/ButtonAction.java deleted file mode 100644 index 9d58a50..0000000 --- a/src/org/montclairrobotics/sprocket/control/ButtonAction.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.montclairrobotics.sprocket.control; - -/** - * ButtonAction is a class which developers use to define button behavior. - * When programming button behaviors, you have to extend this class and pass the - * ButtonAction instance into a Button object. - * @author MHS Robotics - * - */ -public abstract class ButtonAction { - - /** - * This is run when a specific button event occurs - */ - public abstract void onAction(); - -} diff --git a/src/org/montclairrobotics/sprocket/core/Encoder.java b/src/org/montclairrobotics/sprocket/core/Encoder.java index 56cc24c..e13c7e6 100644 --- a/src/org/montclairrobotics/sprocket/core/Encoder.java +++ b/src/org/montclairrobotics/sprocket/core/Encoder.java @@ -2,12 +2,8 @@ import org.montclairrobotics.sprocket.utils.Input; -public interface Encoder extends Input{ +public interface Encoder { public double getSpeed(); public double getDistance(); - public default Double get() - { - return getSpeed(); - } public void reset(); } diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java index 9ab9014..78ae716 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java @@ -3,6 +3,7 @@ import java.util.ArrayList; import org.montclairrobotics.sprocket.control.ArcadeDriveInput; +import org.montclairrobotics.sprocket.core.Joystick; import org.montclairrobotics.sprocket.frc.FRCMotor; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Polar; @@ -13,7 +14,6 @@ import org.montclairrobotics.sprocket.pipeline.Step; import org.montclairrobotics.sprocket.utils.PID; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.SpeedController; /** diff --git a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java index aea5c90..f63e173 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java @@ -8,10 +8,11 @@ import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.pipeline.Step; +import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Utils; -public class AccelLimit implements Step,Togglable{ +public class AccelLimit implements Step,Action{ private Distance maxAccel; private Angle maxTurn; @@ -70,11 +71,11 @@ public DTTarget get(DTTarget in) { } } @Override - public void enable() { + public void onEnable() { enabled=true; } @Override - public void disable() { + public void onDisable() { enabled=false; } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java index dcb5391..e4737e5 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java @@ -5,12 +5,13 @@ import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.pipeline.Step; +import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; import org.montclairrobotics.sprocket.utils.Utils; -public class GyroCorrection implements Step, Togglable { +public class GyroCorrection implements Step, Action { private PID pid; private boolean enabled=true; @@ -126,12 +127,12 @@ public PID getPID() } @Override - public void enable() { + public void onEnable() { enabled=true; } @Override - public void disable() { + public void onDisable() { enabled=false; } public void setTargetAngle(Angle a,boolean relative) { diff --git a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java index a74236c..1a372fe 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java @@ -3,11 +3,12 @@ import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.pipeline.Step; +import org.montclairrobotics.sprocket.utils.Action; -public class SpeedLimiter implements Step, Togglable{ +public class SpeedLimiter implements Step, Action{ private double maxSpeed; - private boolean enabled; + private boolean enabled=true; public SpeedLimiter(double maxSpeed) { @@ -15,17 +16,18 @@ public SpeedLimiter(double maxSpeed) } @Override - public void enable() { + public void onEnable() { enabled=true; } @Override - public void disable() { + public void onDisable() { enabled=false; } @Override public DTTarget get(DTTarget in) { + if(!enabled)return in; Vector out=in.getDirection(); if(out.getMagnitude()>maxSpeed) { diff --git a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java index ba58bef..de86a1f 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java @@ -4,11 +4,12 @@ import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.pipeline.Step; +import org.montclairrobotics.sprocket.utils.Action; -public class TurnLimiter implements Step, Togglable{ +public class TurnLimiter implements Step, Action{ private double maxSpeed; - private boolean enabled; + private boolean enabled=true; public TurnLimiter(double maxSpeed) { @@ -16,17 +17,18 @@ public TurnLimiter(double maxSpeed) } @Override - public void enable() { + public void onEnable() { enabled=true; } @Override - public void disable() { + public void onDisable() { enabled=false; } @Override public DTTarget get(DTTarget in) { + if(!enabled)return in; Angle out=in.getTurn(); if(Math.abs(out.toRadians())>maxSpeed) { diff --git a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java index a779a9a..61773dc 100644 --- a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java +++ b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java @@ -5,9 +5,10 @@ import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Debug; -public class GyroLock implements Updatable, Togglable { +public class GyroLock implements Updatable, Action { private GyroCorrection gyro; private boolean lastLock; @@ -21,11 +22,11 @@ public GyroLock(GyroCorrection gyro) Updater.add(this, Priority.HIGH); } - public void enable() { + public void onEnable() { enabled = true; } - public void disable() { + public void onDisable() { enabled = false; } diff --git a/src/org/montclairrobotics/sprocket/frc/FRCEncoder.java b/src/org/montclairrobotics/sprocket/frc/FRCEncoder.java new file mode 100644 index 0000000..1a0d118 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/frc/FRCEncoder.java @@ -0,0 +1,18 @@ +package org.montclairrobotics.sprocket.frc; + + +public class FRCEncoder extends edu.wpi.first.wpilibj.Encoder implements org.montclairrobotics.sprocket.core.Encoder{ + + public FRCEncoder(int channelA, int channelB) { + super(channelA, channelB); + // TODO Auto-generated constructor stub + } + + @Override + public double getSpeed() { + // TODO Auto-generated method stub + return super.getRate(); + } + + +} diff --git a/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java b/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java new file mode 100644 index 0000000..59a41d3 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java @@ -0,0 +1,7 @@ +package org.montclairrobotics.sprocket.frc; + +public class FRCJoystick extends edu.wpi.first.wpilibj.Joystick implements org.montclairrobotics.sprocket.core.Joystick{ + public FRCJoystick(int port) { + super(port); + } +} diff --git a/src/org/montclairrobotics/sprocket/motors/Module.java b/src/org/montclairrobotics/sprocket/motors/Module.java index 8ff1b05..ed8a31c 100644 --- a/src/org/montclairrobotics/sprocket/motors/Module.java +++ b/src/org/montclairrobotics/sprocket/motors/Module.java @@ -97,7 +97,7 @@ public SEncoder getEnc() { public Distance getDistance() { if(enc == null) return Distance.ZERO; - return enc.getInches(); + return enc.getDistance(); } public boolean hasEncoder() { From 26ab19c5723910f5668335ec082dc08f51c9040a Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 15 Apr 2017 15:57:07 -0400 Subject: [PATCH 06/71] States are now actions as well --- .../sprocket/SprocketRobot.java | 4 +-- .../sprocket/auto/AutoMode.java | 6 ++-- .../sprocket/auto/AutoState.java | 4 +-- .../sprocket/auto/states/Delay.java | 2 +- .../sprocket/auto/states/Disable.java | 34 ------------------- .../auto/states/DriveEncoderGyro.java | 24 ++++++------- .../sprocket/auto/states/DriveEncoders.java | 2 +- .../sprocket/auto/states/DriveTime.java | 2 +- .../sprocket/auto/states/Enable.java | 34 ------------------- .../sprocket/auto/states/TurnEncoders.java | 2 +- .../sprocket/auto/states/TurnGyro.java | 2 +- .../sprocket/states/MultiState.java | 12 +++---- .../sprocket/states/State.java | 7 ++-- .../sprocket/states/StateMachine.java | 18 +++++----- 14 files changed, 42 insertions(+), 111 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/auto/states/Disable.java delete mode 100644 src/org/montclairrobotics/sprocket/auto/states/Enable.java diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/SprocketRobot.java index 8bc8f7a..49fb248 100644 --- a/src/org/montclairrobotics/sprocket/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/SprocketRobot.java @@ -62,14 +62,14 @@ public void update(){} public final void disabledInit() { if(selectedAutoMode!=null) { - selectedAutoMode.stop(); + selectedAutoMode.onDisable(); } } @Override public final void autonomousInit() { selectedAutoMode = chooser.getSelected(); - selectedAutoMode.start(); + selectedAutoMode.onEnable(); start(); userAutonomousInit(); } diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index 9f5ae78..57d50d8 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -27,14 +27,14 @@ public AutoMode(String name, State... states) super(states); this.name = name; } - public void start() + public void onEnable() { super.start(true); Debug.msg("Auto Mode Running:",name); } - public void stop() + public void onDisable() { - super.stop(); + super.onDisable(); SprocketRobot.getDriveTrain().useDefaultInput(); } public String toString() diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index cd66d74..701325f 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -16,7 +16,7 @@ public abstract class AutoState extends AutoDTInput implements State { private double t;//time state was started - public final void start() + public final void onEnable() { t=Updater.getTime(); SprocketRobot.getDriveTrain().setTempInput(this); @@ -27,7 +27,7 @@ public final void start() * Run when the AutoState beigns */ public void userStart(){} - public final void stop() + public final void onDisable() { userStop(); /*tgtDir=Vector.ZERO; diff --git a/src/org/montclairrobotics/sprocket/auto/states/Delay.java b/src/org/montclairrobotics/sprocket/auto/states/Delay.java index 4c507da..e23b53d 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/Delay.java +++ b/src/org/montclairrobotics/sprocket/auto/states/Delay.java @@ -27,7 +27,7 @@ public void userStart() { } @Override - public void stateUpdate() { + public void enabled() { // TODO Auto-generated method stub } diff --git a/src/org/montclairrobotics/sprocket/auto/states/Disable.java b/src/org/montclairrobotics/sprocket/auto/states/Disable.java deleted file mode 100644 index b3436f9..0000000 --- a/src/org/montclairrobotics/sprocket/auto/states/Disable.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.montclairrobotics.sprocket.auto.states; - -import org.montclairrobotics.sprocket.states.State; - -public class Disable implements State{ - - private Togglable obj; - - public Disable(Togglable obj) - { - this.obj=obj; - } - - @Override - public void start() { - obj.disable(); - } - - @Override - public void stop() { - - } - - @Override - public void stateUpdate() { - - } - - @Override - public boolean isDone() { - return true; - } - -} diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java index 289ba31..98875a8 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java @@ -21,13 +21,13 @@ public DriveEncoderGyro(Distance d,Angle a,boolean relative,double speed, double new DriveEncoders(d,speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override - public void start() { + public void onEnable() { driveGyro.setTargetAngle(a,relative); } @Override - public void stop() {} + public void onDisable() {} @Override - public void stateUpdate() { + public void enabled() { driveGyro.use(); } @@ -46,13 +46,13 @@ public DriveEncoderGyro(Distance d, double speed, double maxEncAccel, double max new DriveEncoders(d, speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override - public void start() { + public void onEnable() { driveGyro.setTargetAngleRelative(); } @Override - public void stop() {} + public void onDisable() {} @Override - public void stateUpdate() { + public void enabled() { driveGyro.use(); } @@ -71,13 +71,13 @@ public DriveEncoderGyro(Input dInput, Input a, boolean relative, new DriveEncoders(dInput,speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override - public void start() { + public void onEnable() { driveGyro.setTargetAngle(new Degrees(a.get()),relative); } @Override - public void stop() {} + public void onDisable() {} @Override - public void stateUpdate() { + public void enabled() { driveGyro.use(); } @Override @@ -95,13 +95,13 @@ public DriveEncoderGyro(Input d, Input speed, double maxEncAccel new DriveEncoders(d, speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override - public void start() { + public void onEnable() { driveGyro.setTargetAngleRelative(); } @Override - public void stop() {} + public void onDisable() {} @Override - public void stateUpdate() { + public void enabled() { driveGyro.use(); } diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index cc7676e..2f625e1 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -55,7 +55,7 @@ public void userStart() { } @Override - public void stateUpdate() { + public void enabled() { /* tgtDir = new XY(0,Utils.constrain( Math.sqrt(Math.abs(2*maxAccel.get()*(stopDist.get()-dt.getDistance().get()))) diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java b/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java index 1030751..c0befda 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java @@ -35,7 +35,7 @@ public DriveTime(Input timeInput, double power) { } @Override - public void stateUpdate() { + public void enabled() { } /* @Override diff --git a/src/org/montclairrobotics/sprocket/auto/states/Enable.java b/src/org/montclairrobotics/sprocket/auto/states/Enable.java deleted file mode 100644 index 3f5fca4..0000000 --- a/src/org/montclairrobotics/sprocket/auto/states/Enable.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.montclairrobotics.sprocket.auto.states; - -import org.montclairrobotics.sprocket.states.State; - -public class Enable implements State{ - - private Togglable obj; - - public Enable(Togglable obj) - { - this.obj=obj; - } - - @Override - public void start() { - obj.enable(); - } - - @Override - public void stop() { - - } - - @Override - public void stateUpdate() { - - } - - @Override - public boolean isDone() { - return true; - } - -} diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java index 4242f3e..d32ad55 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java @@ -30,7 +30,7 @@ public void userStart() { } @Override - public void stateUpdate() { + public void enabled() { tgtTurn = turnSpeed; } diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java index 32eef33..7abc8ba 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java @@ -60,7 +60,7 @@ public void userStart() } @Override - public void stateUpdate() { + public void enabled() { gyro.use(); if(Math.abs(gyro.getError().toDegrees())>tolerance.toDegrees()) { diff --git a/src/org/montclairrobotics/sprocket/states/MultiState.java b/src/org/montclairrobotics/sprocket/states/MultiState.java index ed8c84f..0622c46 100644 --- a/src/org/montclairrobotics/sprocket/states/MultiState.java +++ b/src/org/montclairrobotics/sprocket/states/MultiState.java @@ -42,23 +42,23 @@ public MultiState(State... states) { } @Override - public void start() { + public void onEnable() { for(State s : states) { - s.start(); + s.onEnable(); } } @Override - public void stop() { + public void onDisable() { for(State s : states) { - s.stop(); + s.onDisable(); } } @Override - public void stateUpdate() { + public void enabled() { for(State s : states) { - s.stateUpdate(); + s.enabled(); } } diff --git a/src/org/montclairrobotics/sprocket/states/State.java b/src/org/montclairrobotics/sprocket/states/State.java index 9c2ee0c..ccaa4f5 100644 --- a/src/org/montclairrobotics/sprocket/states/State.java +++ b/src/org/montclairrobotics/sprocket/states/State.java @@ -1,8 +1,7 @@ package org.montclairrobotics.sprocket.states; -public interface State { - void start(); - void stop(); - void stateUpdate(); +import org.montclairrobotics.sprocket.utils.Action; + +public interface State extends Action{ boolean isDone(); } diff --git a/src/org/montclairrobotics/sprocket/states/StateMachine.java b/src/org/montclairrobotics/sprocket/states/StateMachine.java index 2b826e5..08dc7d5 100644 --- a/src/org/montclairrobotics/sprocket/states/StateMachine.java +++ b/src/org/montclairrobotics/sprocket/states/StateMachine.java @@ -24,29 +24,29 @@ public void start(boolean top) startState(); } @Override - public void start() { + public void onEnable() { start(true); } @Override - public void stop() { + public void onDisable() { if(isDone())return; - states[index].stop(); + states[index].onDisable(); index=-1; top=false; } @Override - public void stateUpdate() { + public void enabled() { if(isDone())return; - states[index].stateUpdate(); + states[index].enabled(); while(states[index].isDone()) { - states[index].stop(); + states[index].onDisable(); index++; if(isDone())return; startState(); - states[index].stateUpdate(); + states[index].enabled(); } } @@ -62,7 +62,7 @@ public State[] getStates() public void update() { if(top) { - stateUpdate(); + enabled(); } } public void startState() @@ -73,7 +73,7 @@ public void startState() } else { - states[index].start(); + states[index].onEnable(); } } } From 1a9920997456114d45137119c199c8ba90e17593 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 15 Apr 2017 15:58:41 -0400 Subject: [PATCH 07/71] I like these names better, but it doesnt matter --- .../sprocket/SprocketRobot.java | 4 ++-- .../sprocket/auto/AutoMode.java | 6 +++--- .../sprocket/auto/AutoState.java | 4 ++-- .../sprocket/auto/states/DriveEncoderGyro.java | 16 ++++++++-------- .../sprocket/control/ButtonListener.java | 4 ++-- .../sprocket/control/FieldCentricDriveInput.java | 4 ++-- .../sprocket/drive/ControlledMotor.java | 4 ++-- .../sprocket/drive/steps/AccelLimit.java | 4 ++-- .../sprocket/drive/steps/GyroCorrection.java | 4 ++-- .../sprocket/drive/steps/SpeedLimiter.java | 4 ++-- .../sprocket/drive/steps/TurnLimiter.java | 4 ++-- .../sprocket/drive/utils/GyroLock.java | 4 ++-- .../sprocket/states/MultiState.java | 8 ++++---- .../sprocket/states/StateMachine.java | 10 +++++----- .../montclairrobotics/sprocket/utils/Action.java | 4 ++-- .../sprocket/utils/PIDTuner.java | 2 +- 16 files changed, 43 insertions(+), 43 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/SprocketRobot.java index 49fb248..8bc8f7a 100644 --- a/src/org/montclairrobotics/sprocket/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/SprocketRobot.java @@ -62,14 +62,14 @@ public void update(){} public final void disabledInit() { if(selectedAutoMode!=null) { - selectedAutoMode.onDisable(); + selectedAutoMode.stop(); } } @Override public final void autonomousInit() { selectedAutoMode = chooser.getSelected(); - selectedAutoMode.onEnable(); + selectedAutoMode.start(); start(); userAutonomousInit(); } diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index 57d50d8..9f5ae78 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -27,14 +27,14 @@ public AutoMode(String name, State... states) super(states); this.name = name; } - public void onEnable() + public void start() { super.start(true); Debug.msg("Auto Mode Running:",name); } - public void onDisable() + public void stop() { - super.onDisable(); + super.stop(); SprocketRobot.getDriveTrain().useDefaultInput(); } public String toString() diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index 701325f..cd66d74 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -16,7 +16,7 @@ public abstract class AutoState extends AutoDTInput implements State { private double t;//time state was started - public final void onEnable() + public final void start() { t=Updater.getTime(); SprocketRobot.getDriveTrain().setTempInput(this); @@ -27,7 +27,7 @@ public final void onEnable() * Run when the AutoState beigns */ public void userStart(){} - public final void onDisable() + public final void stop() { userStop(); /*tgtDir=Vector.ZERO; diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java index 98875a8..bf083c7 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java @@ -21,11 +21,11 @@ public DriveEncoderGyro(Distance d,Angle a,boolean relative,double speed, double new DriveEncoders(d,speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override - public void onEnable() { + public void start() { driveGyro.setTargetAngle(a,relative); } @Override - public void onDisable() {} + public void stop() {} @Override public void enabled() { driveGyro.use(); @@ -46,11 +46,11 @@ public DriveEncoderGyro(Distance d, double speed, double maxEncAccel, double max new DriveEncoders(d, speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override - public void onEnable() { + public void start() { driveGyro.setTargetAngleRelative(); } @Override - public void onDisable() {} + public void stop() {} @Override public void enabled() { driveGyro.use(); @@ -71,11 +71,11 @@ public DriveEncoderGyro(Input dInput, Input a, boolean relative, new DriveEncoders(dInput,speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override - public void onEnable() { + public void start() { driveGyro.setTargetAngle(new Degrees(a.get()),relative); } @Override - public void onDisable() {} + public void stop() {} @Override public void enabled() { driveGyro.use(); @@ -95,11 +95,11 @@ public DriveEncoderGyro(Input d, Input speed, double maxEncAccel new DriveEncoders(d, speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override - public void onEnable() { + public void start() { driveGyro.setTargetAngleRelative(); } @Override - public void onDisable() {} + public void stop() {} @Override public void enabled() { driveGyro.use(); diff --git a/src/org/montclairrobotics/sprocket/control/ButtonListener.java b/src/org/montclairrobotics/sprocket/control/ButtonListener.java index 38392f0..2847af1 100644 --- a/src/org/montclairrobotics/sprocket/control/ButtonListener.java +++ b/src/org/montclairrobotics/sprocket/control/ButtonListener.java @@ -39,10 +39,10 @@ public void update() { if(action!=null) { if(pressed && !wasPressed) { - action.onEnable(); + action.start(); } if(!pressed && wasPressed) { - action.onDisable(); + action.stop(); } if(pressed) { action.enabled(); diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index ab4e037..26381d2 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -73,11 +73,11 @@ public Angle getTurn() { } @Override - public void onEnable() { + public void start() { SprocketRobot.getDriveTrain().setTempInput(this); } @Override - public void onDisable() { + public void stop() { SprocketRobot.getDriveTrain().useDefaultInput(); } } diff --git a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java index a19b150..fe4be8c 100644 --- a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java +++ b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java @@ -69,12 +69,12 @@ public void update() { } @Override - public void onEnable() { + public void start() { enabled=true; } @Override - public void onDisable() { + public void stop() { enabled=false; } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java index f63e173..98f6f38 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java @@ -71,11 +71,11 @@ public DTTarget get(DTTarget in) { } } @Override - public void onEnable() { + public void start() { enabled=true; } @Override - public void onDisable() { + public void stop() { enabled=false; } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java index e4737e5..d173cde 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java @@ -127,12 +127,12 @@ public PID getPID() } @Override - public void onEnable() { + public void start() { enabled=true; } @Override - public void onDisable() { + public void stop() { enabled=false; } public void setTargetAngle(Angle a,boolean relative) { diff --git a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java index 1a372fe..1be9721 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java @@ -16,12 +16,12 @@ public SpeedLimiter(double maxSpeed) } @Override - public void onEnable() { + public void start() { enabled=true; } @Override - public void onDisable() { + public void stop() { enabled=false; } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java index de86a1f..5a74f71 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java @@ -17,12 +17,12 @@ public TurnLimiter(double maxSpeed) } @Override - public void onEnable() { + public void start() { enabled=true; } @Override - public void onDisable() { + public void stop() { enabled=false; } diff --git a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java index 61773dc..78a65c7 100644 --- a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java +++ b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java @@ -22,11 +22,11 @@ public GyroLock(GyroCorrection gyro) Updater.add(this, Priority.HIGH); } - public void onEnable() { + public void start() { enabled = true; } - public void onDisable() { + public void stop() { enabled = false; } diff --git a/src/org/montclairrobotics/sprocket/states/MultiState.java b/src/org/montclairrobotics/sprocket/states/MultiState.java index 0622c46..2034dfc 100644 --- a/src/org/montclairrobotics/sprocket/states/MultiState.java +++ b/src/org/montclairrobotics/sprocket/states/MultiState.java @@ -42,16 +42,16 @@ public MultiState(State... states) { } @Override - public void onEnable() { + public void start() { for(State s : states) { - s.onEnable(); + s.start(); } } @Override - public void onDisable() { + public void stop() { for(State s : states) { - s.onDisable(); + s.stop(); } } diff --git a/src/org/montclairrobotics/sprocket/states/StateMachine.java b/src/org/montclairrobotics/sprocket/states/StateMachine.java index 08dc7d5..80dba4c 100644 --- a/src/org/montclairrobotics/sprocket/states/StateMachine.java +++ b/src/org/montclairrobotics/sprocket/states/StateMachine.java @@ -24,14 +24,14 @@ public void start(boolean top) startState(); } @Override - public void onEnable() { + public void start() { start(true); } @Override - public void onDisable() { + public void stop() { if(isDone())return; - states[index].onDisable(); + states[index].stop(); index=-1; top=false; } @@ -42,7 +42,7 @@ public void enabled() { states[index].enabled(); while(states[index].isDone()) { - states[index].onDisable(); + states[index].stop(); index++; if(isDone())return; startState(); @@ -73,7 +73,7 @@ public void startState() } else { - states[index].onEnable(); + states[index].start(); } } } diff --git a/src/org/montclairrobotics/sprocket/utils/Action.java b/src/org/montclairrobotics/sprocket/utils/Action.java index cbac309..4fd309c 100644 --- a/src/org/montclairrobotics/sprocket/utils/Action.java +++ b/src/org/montclairrobotics/sprocket/utils/Action.java @@ -1,8 +1,8 @@ package org.montclairrobotics.sprocket.utils; public interface Action { - public default void onEnable(){} + public default void start(){} public default void enabled(){} - public default void onDisable(){} + public default void stop(){} public default void disabled(){} } diff --git a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java index 0a9ac83..f96ccfd 100644 --- a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java +++ b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java @@ -28,7 +28,7 @@ public PIDTuner(Input TempP,Input cyclesPer10Sec,Input new ButtonListener(apply).setAction(new Action(){ @Override - public void onEnable() { + public void start() { recalculatePIDs(); }}); From f640d91b0698d087408a66ac7c98731e15a5b49d Mon Sep 17 00:00:00 2001 From: MHSRobotics Date: Thu, 11 May 2017 20:15:30 -0400 Subject: [PATCH 08/71] Not near done; need to work out inheritances and class/interfaces for SprocketRobot and FRCRobot. Want to be able to define stuff in SprocketRobot and have FRCRobot extend it, yet FRCRobot has to extend IterativeRobot. Very tricky Robot. --- .../sprocket/core/AutoSelector.java | 13 ++++ .../sprocket/core/SprocketRobot.java | 37 +++++++++++ .../sprocket/frc/DashboardSelector.java | 32 ++++++++++ .../sprocket/frc/FRCRobot.java | 63 +++++++++++++++++++ .../sprocket/utils/Input.java | 2 +- 5 files changed, 146 insertions(+), 1 deletion(-) create mode 100644 src/org/montclairrobotics/sprocket/core/AutoSelector.java create mode 100644 src/org/montclairrobotics/sprocket/core/SprocketRobot.java create mode 100644 src/org/montclairrobotics/sprocket/frc/DashboardSelector.java create mode 100644 src/org/montclairrobotics/sprocket/frc/FRCRobot.java diff --git a/src/org/montclairrobotics/sprocket/core/AutoSelector.java b/src/org/montclairrobotics/sprocket/core/AutoSelector.java new file mode 100644 index 0000000..50e48ab --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/AutoSelector.java @@ -0,0 +1,13 @@ +package org.montclairrobotics.sprocket.core; + +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.utils.Input; + +public interface AutoSelector extends Input{ + + void addAutoMode(AutoMode mode); + + void setAutoModes(AutoMode[] modes); + + void update(); +} diff --git a/src/org/montclairrobotics/sprocket/core/SprocketRobot.java b/src/org/montclairrobotics/sprocket/core/SprocketRobot.java new file mode 100644 index 0000000..1e56628 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/SprocketRobot.java @@ -0,0 +1,37 @@ +package org.montclairrobotics.sprocket.core; + +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.drive.DriveTrain; +import org.montclairrobotics.sprocket.frc.DashboardSelector; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; + +public interface SprocketRobot extends Updatable{ + + public static AutoSelector selector=null; + public static DriveTrain driveTrain=null; + public static AutoMode selectedMode=null; + + public default void setup(){userSetup();} + public default void userSetup(){} + + public default void autoStart(){ + selectedMode=selector.get(); + + userAutoStart(); + } + public default void userAutoStart(){} + + public default void teleopStart(){userTeleopStart();} + public default void userTeleopStart(){} + + public default void loop(){Updater.loop();} + + public default void update(){} + + public default void disable(){userDisable();}; + public default void userDisable(){} + + public default void disableLoop(){userDisableLoop();} + public default void userDisableLoop(){} +} diff --git a/src/org/montclairrobotics/sprocket/frc/DashboardSelector.java b/src/org/montclairrobotics/sprocket/frc/DashboardSelector.java new file mode 100644 index 0000000..7fd1088 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/frc/DashboardSelector.java @@ -0,0 +1,32 @@ +package org.montclairrobotics.sprocket.frc; + +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.core.AutoSelector; + +public class DashboardSelector implements AutoSelector{ + + @Override + public void addAutoMode(AutoMode mode) { + // TODO Auto-generated method stub + + } + + @Override + public void setAutoModes(AutoMode[] modes) { + // TODO Auto-generated method stub + + } + + @Override + public AutoMode get() { + // TODO Auto-generated method stub + return null; + } + + @Override + public void update() { + // TODO Auto-generated method stub + + } + +} diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java new file mode 100644 index 0000000..ebdbd05 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -0,0 +1,63 @@ +package org.montclairrobotics.sprocket.frc; + +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.core.AutoSelector; +import org.montclairrobotics.sprocket.core.SprocketRobot; +import org.montclairrobotics.sprocket.drive.DriveTrain; +import org.montclairrobotics.sprocket.loop.Priority; +import org.montclairrobotics.sprocket.loop.Updater; + +import edu.wpi.first.wpilibj.IterativeRobot; + +public class FRCRobot extends IterativeRobot implements SprocketRobot{ + + + @Override + public final void robotInit() + { + setup(); + } + @Override + public final void autonomousInit() + { + autoStart(); + } + @Override + public final void teleopInit() + { + teleopStart(); + } + @Override + public final void autonomousPeriodic() + { + loop(); + } + @Override + public final void teleopPeriodic() + { + loop(); + } + @Override + public final void disabledInit() + { + disable(); + } + @Override + public final void disabledPeriodic() + { + disableLoop(); + } + + //========================================================== + @Override + public final void setup() + { + SprocketRobot.selector=new DashboardSelector(); + } + + public void autoStart() + { + selectedMode=selector.get(); + } + +} diff --git a/src/org/montclairrobotics/sprocket/utils/Input.java b/src/org/montclairrobotics/sprocket/utils/Input.java index 47f5678..232fbeb 100644 --- a/src/org/montclairrobotics/sprocket/utils/Input.java +++ b/src/org/montclairrobotics/sprocket/utils/Input.java @@ -4,7 +4,7 @@ public interface Input { public T get(); - public static Input neg(Input a) + public static Input opposite(Input a) { return new Input(){ From 052fcbce5d3f5485935037af4858d9cb1bd1f8e0 Mon Sep 17 00:00:00 2001 From: MHSRobotics Date: Thu, 25 May 2017 18:37:48 -0400 Subject: [PATCH 09/71] Working on auto --- src/org/montclairrobotics/sprocket/SprocketRobot.java | 2 +- .../montclairrobotics/sprocket/auto/states/DriveEncoders.java | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/SprocketRobot.java index c00e6e8..8731479 100644 --- a/src/org/montclairrobotics/sprocket/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/SprocketRobot.java @@ -101,6 +101,7 @@ public final void sprocketUpdate() @Override public final void disabledPeriodic() { super.disabledPeriodic(); + SmartDashboard.putData("AUTO:",chooser); } @Override @@ -137,6 +138,5 @@ public void sendAutoModes() { chooser.addObject(mode+"", mode); } - SmartDashboard.putData("AUTO:",chooser); } } diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index 99851b4..ea3730a 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -73,6 +73,8 @@ public void stateUpdate() { double tgtV2inTicks=2*maxEncAccel*(stopDist.get()-dt.get().get()); double tgtV=Math.sqrt(Math.abs(tgtV2inTicks))/maxEncTicksPerSec*(stopDist.get()-dt.get().get()>0?1:-1); + Debug.num("dt-get-get", dt.get().get()); + Debug.num("stopDist", stopDist.get()); tgtV=Utils.constrain(tgtV, -speed, speed); tgtDir = new XY(0,tgtV); From d182761f315dc8476aec3ba543a1eb8d7367bfeb Mon Sep 17 00:00:00 2001 From: MHSRobotics Date: Thu, 8 Jun 2017 20:04:19 -0400 Subject: [PATCH 10/71] Auto Betterer --- .../sprocket/auto/states/TurnGyro.java | 9 ++++--- .../sprocket/drive/steps/GyroCorrection.java | 27 ++++++++++++++++--- .../montclairrobotics/sprocket/utils/PID.java | 15 +++++++++++ 3 files changed, 43 insertions(+), 8 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java index 35007a7..39eb8d0 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java @@ -29,8 +29,8 @@ public void setTgt(Angle tgt) { private double incorrectTime; private boolean relative; - private static final Angle tolerance=new Degrees(5); - private static final double timeAtTarget=0.2; + private static final Angle tolerance=new Degrees(4); + private static final double timeAtTarget=0.5; public TurnGyro(Angle tgt,GyroCorrection gyro,boolean relative) { @@ -60,7 +60,7 @@ public void userStart() gyro.setTargetAngleReset(tgt); } gyro.setMinMaxOut(-0.5, 0.5); - incorrectTime=0; + incorrectTime=Updater.getTime()-timeAtTarget+0.1; } @Override @@ -73,6 +73,7 @@ public void stateUpdate() { Debug.msg("gyroError", gyro.getError().toDegrees()); Debug.msg("incorrectTime", incorrectTime); Debug.msg("cur-time", Updater.getTime()); + Debug.msg("timeCorrect", Updater.getTime()-incorrectTime); Debug.msg("IS-DONE",isDone()); } @@ -84,7 +85,7 @@ public void userStop() @Override public boolean isDone() { - return (incorrectTime>timeAtTarget); + return (Updater.getTime()-incorrectTime>timeAtTarget); } } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java index 72849c7..3333fbf 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java @@ -23,16 +23,27 @@ public class GyroCorrection implements Step, Togglable { private double minOut=-1; private double maxOut=1; + private double maxError; + private double powMaxError; - public GyroCorrection(Input gyro,PID pid) + public GyroCorrection(Input gyro,PID pid,double maxError,double powIfMaxError) { this(pid); this.pid.setInput(gyro); + this.maxError=maxError; + this.powMaxError=powIfMaxError; } - public GyroCorrection(PID pid) + public GyroCorrection(PID pid,double maxError,double powIfMaxError) { this.pid=pid.copy(); this.pid.setMinMax(-180, 179, -1, 1); + + this.maxError=maxError; + this.powMaxError=powIfMaxError; + } + public GyroCorrection(PID pid) + { + this(pid,180,1); } public void use() @@ -56,8 +67,16 @@ public DTTarget get(DTTarget in) { DTTarget out=in; if(enabled&&used) { - double tgt=pid.get(); - tgt=Utils.constrain(tgt, minOut, maxOut); + double tgt; + if(Math.abs(pid.getError())>maxError) + { + tgt=powMaxError*(pid.getError()*pid.getP()>0?1:-1);//dont let rich see this + } + else + { + tgt=pid.get(); + tgt=Utils.constrain(tgt, minOut, maxOut); + } Angle tgtAngle=new Radians(tgt); out=new DTTarget(in.getDirection(),tgtAngle); } diff --git a/src/org/montclairrobotics/sprocket/utils/PID.java b/src/org/montclairrobotics/sprocket/utils/PID.java index 1e47807..1a246f9 100644 --- a/src/org/montclairrobotics/sprocket/utils/PID.java +++ b/src/org/montclairrobotics/sprocket/utils/PID.java @@ -203,4 +203,19 @@ public double getTarget() { return target; } + + public double getP() + { + return P; + } + + public double getI() + { + return I; + } + + public double getD() + { + return D; + } } \ No newline at end of file From dcde855c87e253ca2db538a1c6937d6db3cf3146 Mon Sep 17 00:00:00 2001 From: MHSRobotics Date: Mon, 12 Jun 2017 18:15:18 -0400 Subject: [PATCH 11/71] Auto Tweak --- .../sprocket/drive/steps/GyroCorrection.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java index 3333fbf..6b62851 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java @@ -24,14 +24,14 @@ public class GyroCorrection implements Step, Togglable { private double minOut=-1; private double maxOut=1; private double maxError; - private double powMaxError; + private double farP; - public GyroCorrection(Input gyro,PID pid,double maxError,double powIfMaxError) + public GyroCorrection(Input gyro,PID pid,double maxError,double farP) { this(pid); this.pid.setInput(gyro); this.maxError=maxError; - this.powMaxError=powIfMaxError; + this.farP=farP; } public GyroCorrection(PID pid,double maxError,double powIfMaxError) { @@ -39,7 +39,7 @@ public GyroCorrection(PID pid,double maxError,double powIfMaxError) this.pid.setMinMax(-180, 179, -1, 1); this.maxError=maxError; - this.powMaxError=powIfMaxError; + this.farP=powIfMaxError; } public GyroCorrection(PID pid) { @@ -70,7 +70,7 @@ public DTTarget get(DTTarget in) { double tgt; if(Math.abs(pid.getError())>maxError) { - tgt=powMaxError*(pid.getError()*pid.getP()>0?1:-1);//dont let rich see this + tgt=farP*pid.getError()*(pid.getP()>0?1:-1); } else { From f25b3df6c19817676257371710364b5af78fb6d1 Mon Sep 17 00:00:00 2001 From: MHSRobotics Date: Mon, 12 Jun 2017 18:31:41 -0400 Subject: [PATCH 12/71] Time for a new feature! --- .../control/FieldCentricDriveInput.java | 13 ++++++- .../sprocket/utils/SmoothData.java | 32 +++++++++++++++++ .../sprocket/utils/SmoothInput.java | 16 +++++++++ .../sprocket/utils/SmoothVectorInput.java | 34 +++++++++++++++++++ 4 files changed, 94 insertions(+), 1 deletion(-) create mode 100644 src/org/montclairrobotics/sprocket/utils/SmoothData.java create mode 100644 src/org/montclairrobotics/sprocket/utils/SmoothInput.java create mode 100644 src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index ec63497..1068722 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -9,6 +9,7 @@ import org.montclairrobotics.sprocket.geometry.XY; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; +import org.montclairrobotics.sprocket.utils.SmoothVectorInput; import org.montclairrobotics.sprocket.utils.Togglable; import edu.wpi.first.wpilibj.Joystick; @@ -18,9 +19,12 @@ public class FieldCentricDriveInput extends ArcadeDriveInput implements Togglabl private GyroCorrection gyro; private Vector field,robot; + private SmoothVectorInput fieldInput; private boolean forwards; private boolean rotToVector; + + private static final int SMOOTH_LEN=10; public FieldCentricDriveInput(Joystick stick,GyroCorrection gyro) { @@ -31,12 +35,19 @@ public FieldCentricDriveInput(Joystick stick,GyroCorrection gyro,boolean rotToVe super(stick); this.gyro=gyro; this.rotToVector=rotToVector; + fieldInput=new SmoothVectorInput(SMOOTH_LEN,new Input(){ + + @Override + public Vector get() { + // TODO Auto-generated method stub + return getRaw(); + }}); } @Override public void update() { super.update(); - field=getRaw(); + field=fieldInput.get(); if(field.getMagnitude()>0.1) { robot=field.rotate(gyro.getCurrentAngleReset().negative()); diff --git a/src/org/montclairrobotics/sprocket/utils/SmoothData.java b/src/org/montclairrobotics/sprocket/utils/SmoothData.java new file mode 100644 index 0000000..0adad65 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/utils/SmoothData.java @@ -0,0 +1,32 @@ +package org.montclairrobotics.sprocket.utils; + +public class SmoothData { + private int len; + private double[]data; + private int i; + private double sum; + + public SmoothData(int len) + { + this.len=len; + data=new double[len]; + i=0; + sum=0; + } + + public double smooth(double in) + { + sum+=in; + if(i{ + + private Input inp; + + public SmoothInput(int len,Input inp) { + super(len); + this.inp=inp; + } + + public Double get() + { + return smooth(inp.get()); + } +} diff --git a/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java b/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java new file mode 100644 index 0000000..24f2978 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java @@ -0,0 +1,34 @@ +package org.montclairrobotics.sprocket.utils; + +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.geometry.XY; + +public class SmoothVectorInput implements Input{ + + private SmoothInput x,y; + + public SmoothVectorInput(int len,Input inp) + { + x=new SmoothInput(len,new Input(){ + + @Override + public Double get() { + // TODO Auto-generated method stub + return inp.get().getX(); + }}); + y=new SmoothInput(len,new Input(){ + + @Override + public Double get() { + // TODO Auto-generated method stub + return inp.get().getY(); + }}); + } + + @Override + public Vector get() { + // TODO Auto-generated method stub + return new XY(x.get(),y.get()); + } + +} From 098b14394f38aa081e26f8adfa6a590ef91366a9 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 23 Jun 2017 21:03:14 -0400 Subject: [PATCH 13/71] Move Action --- .../montclairrobotics/sprocket/{utils => actions}/Action.java | 2 +- src/org/montclairrobotics/sprocket/control/ButtonListener.java | 2 +- .../sprocket/control/FieldCentricDriveInput.java | 2 +- src/org/montclairrobotics/sprocket/drive/ControlledMotor.java | 2 +- src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java | 2 +- .../montclairrobotics/sprocket/drive/steps/GyroCorrection.java | 2 +- .../montclairrobotics/sprocket/drive/steps/SpeedLimiter.java | 2 +- src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java | 2 +- src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java | 2 +- src/org/montclairrobotics/sprocket/states/State.java | 2 +- src/org/montclairrobotics/sprocket/utils/PIDTuner.java | 1 + 11 files changed, 11 insertions(+), 10 deletions(-) rename src/org/montclairrobotics/sprocket/{utils => actions}/Action.java (76%) diff --git a/src/org/montclairrobotics/sprocket/utils/Action.java b/src/org/montclairrobotics/sprocket/actions/Action.java similarity index 76% rename from src/org/montclairrobotics/sprocket/utils/Action.java rename to src/org/montclairrobotics/sprocket/actions/Action.java index 4fd309c..dbe1b7a 100644 --- a/src/org/montclairrobotics/sprocket/utils/Action.java +++ b/src/org/montclairrobotics/sprocket/actions/Action.java @@ -1,4 +1,4 @@ -package org.montclairrobotics.sprocket.utils; +package org.montclairrobotics.sprocket.actions; public interface Action { public default void start(){} diff --git a/src/org/montclairrobotics/sprocket/control/ButtonListener.java b/src/org/montclairrobotics/sprocket/control/ButtonListener.java index 2847af1..8b6cc29 100644 --- a/src/org/montclairrobotics/sprocket/control/ButtonListener.java +++ b/src/org/montclairrobotics/sprocket/control/ButtonListener.java @@ -1,9 +1,9 @@ package org.montclairrobotics.sprocket.control; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Input; diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index 27b4ebb..5053162 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -1,11 +1,11 @@ package org.montclairrobotics.sprocket.control; import org.montclairrobotics.sprocket.SprocketRobot; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.Joystick; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.SmoothVectorInput; import org.montclairrobotics.sprocket.utils.Input; diff --git a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java index fe4be8c..4eb8fe9 100644 --- a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java +++ b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java @@ -1,10 +1,10 @@ package org.montclairrobotics.sprocket.drive; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.Motor; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Input; diff --git a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java index 98f6f38..a4334aa 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java @@ -1,5 +1,6 @@ package org.montclairrobotics.sprocket.drive.steps; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; @@ -8,7 +9,6 @@ import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Utils; diff --git a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java index a77453d..17f1f1e 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java @@ -1,11 +1,11 @@ package org.montclairrobotics.sprocket.drive.steps; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.PID; diff --git a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java index 1be9721..156e444 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java @@ -1,9 +1,9 @@ package org.montclairrobotics.sprocket.drive.steps; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Action; public class SpeedLimiter implements Step, Action{ diff --git a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java index 5a74f71..1d8c014 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java @@ -1,10 +1,10 @@ package org.montclairrobotics.sprocket.drive.steps; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Action; public class TurnLimiter implements Step, Action{ diff --git a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java index 78a65c7..eed213c 100644 --- a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java +++ b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java @@ -1,11 +1,11 @@ package org.montclairrobotics.sprocket.drive.utils; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.utils.Action; import org.montclairrobotics.sprocket.utils.Debug; public class GyroLock implements Updatable, Action { diff --git a/src/org/montclairrobotics/sprocket/states/State.java b/src/org/montclairrobotics/sprocket/states/State.java index ccaa4f5..51ae465 100644 --- a/src/org/montclairrobotics/sprocket/states/State.java +++ b/src/org/montclairrobotics/sprocket/states/State.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.states; -import org.montclairrobotics.sprocket.utils.Action; +import org.montclairrobotics.sprocket.actions.Action; public interface State extends Action{ boolean isDone(); diff --git a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java index f96ccfd..bddd8e1 100644 --- a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java +++ b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java @@ -1,5 +1,6 @@ package org.montclairrobotics.sprocket.utils; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.control.ButtonListener; import org.montclairrobotics.sprocket.frc.DashboardButton; import org.montclairrobotics.sprocket.frc.DashboardInput; From 09ea005b3c9c35e9762fcce195d595ab17c82efa Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 23 Jun 2017 22:12:45 -0400 Subject: [PATCH 14/71] Sourcetree crashed so I lost a few commits, so this is big. It has more moves of files, some new documentation, and (finally) an elegant way of auto mode selection has been started. --- .../sprocket/SprocketRobot.java | 149 ++++++++++-------- .../sprocket/actions/Action.java | 23 +++ .../sprocket/actions/MultiAction.java | 45 ++++++ .../{states => actions}/MultiState.java | 36 ++--- .../sprocket/actions/State.java | 11 ++ .../{states => actions}/StateMachine.java | 19 +-- .../sprocket/auto/AutoMode.java | 6 +- .../sprocket/auto/AutoState.java | 6 +- .../auto/states/DriveEncoderGyro.java | 6 +- .../sprocket/auto/states/DriveEncoders.java | 2 +- .../sprocket/auto/states/TurnEncoders.java | 2 +- .../control/FieldCentricDriveInput.java | 4 +- .../sprocket/drive/DriveTrain.java | 2 +- .../sprocket/loop/DisabledUpdater.java | 18 +++ .../sprocket/states/State.java | 7 - 15 files changed, 205 insertions(+), 131 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/actions/MultiAction.java rename src/org/montclairrobotics/sprocket/{states => actions}/MultiState.java (64%) create mode 100644 src/org/montclairrobotics/sprocket/actions/State.java rename src/org/montclairrobotics/sprocket/{states => actions}/StateMachine.java (81%) create mode 100644 src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java delete mode 100644 src/org/montclairrobotics/sprocket/states/State.java diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/SprocketRobot.java index b53e6ea..20ef30e 100644 --- a/src/org/montclairrobotics/sprocket/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/SprocketRobot.java @@ -3,12 +3,15 @@ import java.util.ArrayList; import java.util.Arrays; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.auto.AutoMode; import org.montclairrobotics.sprocket.auto.AutoState; import org.montclairrobotics.sprocket.drive.DriveTrain; +import org.montclairrobotics.sprocket.loop.DisabledUpdater; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -22,87 +25,57 @@ */ public abstract class SprocketRobot extends IterativeRobot implements Updatable{ - private SendableChooser chooser; - private AutoMode[] autoModes; - private AutoMode selectedAutoMode; - private AutoState runState; + public enum Mode {AUTO,TELEOP,TEST}; - public SprocketRobot() - { - Updater.add(this,Priority.NORMAL); - autoModes=new AutoMode[0]; - } + public Input + autoActionInput, + teleopActionInput, + testActionInput; - private static DriveTrain driveTrain; + private Action currentAction; - public static DriveTrain getDriveTrain() { - return driveTrain; - } + private static DriveTrain driveTrain; - public static void setDriveTrain(DriveTrain dt) { - driveTrain = dt; + public SprocketRobot() + { + Updater.add(this,Priority.NORMAL); } + //Stuff you can override if you feel like it - @Override - public void startCompetition() { - super.startCompetition(); - } - @Override public void robotInit(){} - public void userStart(){} - public void reset(){} - public void userTeleopInit(){} - public void userAutonomousInit(){} - public void userTestInit(){} - public void update(){} + public void userStart(Mode mode){} + public void userStop(){} + public void userUpdate(){} + + //STUFF WE OVERRIDE IN THIS CLASS @Override - public final void disabledInit() { - if(selectedAutoMode!=null) - { - selectedAutoMode.stop(); - } + public void startCompetition() { + super.startCompetition(); } - + @Override public final void autonomousInit() { - selectedAutoMode = chooser.getSelected(); - selectedAutoMode.start(); - start(); - userAutonomousInit(); + start(Mode.AUTO); } @Override public final void teleopInit() { - start(); - userTeleopInit(); + start(Mode.TELEOP); } @Override public final void testInit() { - start(); - userTestInit(); - } - - public final void start() - { - userStart(); - reset(); - sendAutoModes(); - } - public final void sprocketUpdate() - { - Updater.loop(); + start(Mode.TEST); } @Override - public final void disabledPeriodic() { - super.disabledPeriodic(); - SmartDashboard.putData("AUTO:",chooser); + public final void disabledInit() { + stop(); } - + @Override public final void autonomousPeriodic() { sprocketUpdate(); @@ -116,26 +89,66 @@ public final void teleopPeriodic() { @Override public final void testPeriodic() { sprocketUpdate(); + } + + @Override + public final void disabledPeriodic() { + sprocketDisabled(); } + - public void setAutoModes(AutoMode... modes) + //OUR STUFF HERE + public static DriveTrain getMainDriveTrain() { + return driveTrain; + } + + public static void setMainDriveTrain(DriveTrain dt) { + driveTrain = dt; + } + public final void start(Mode mode) { - this.autoModes=modes; - sendAutoModes(); + switch(mode) + { + case AUTO: + if(autoActionInput!=null) + currentAction=autoActionInput.get(); + break; + case TELEOP: + if(teleopActionInput!=null) + currentAction=teleopActionInput.get(); + break; + case TEST: + if(testActionInput!=null) + currentAction=testActionInput.get(); + break; + } + if(currentAction!=null) + { + currentAction.start(); + } + userStart(mode); } - - public void addAutoMode(AutoMode mode) { - ArrayList modes = new ArrayList(Arrays.asList(autoModes)); - modes.add(mode); - autoModes = modes.toArray(autoModes); + public final void sprocketUpdate() + { + Updater.loop(); } - - public void sendAutoModes() + public final void update() { - chooser=new SendableChooser(); - for(AutoMode mode:autoModes) + if(currentAction!=null) { - chooser.addObject(mode+"", mode); + currentAction.enabled(); } + userUpdate(); + } + public final void stop() + { + if(currentAction!=null) + { + currentAction.stop(); + } + } + public final void sprocketDisabled() + { + DisabledUpdater.loop(); } } diff --git a/src/org/montclairrobotics/sprocket/actions/Action.java b/src/org/montclairrobotics/sprocket/actions/Action.java index dbe1b7a..b50db73 100644 --- a/src/org/montclairrobotics/sprocket/actions/Action.java +++ b/src/org/montclairrobotics/sprocket/actions/Action.java @@ -1,8 +1,31 @@ package org.montclairrobotics.sprocket.actions; +/** + * This is designed to be the main base for every "thing" or action the robot can do + * This is extended by States, and consequently Auto Modes, + * so things that can be activated by entering "auto" mode, or "test" mode + * Also, buttons should take these as their actions, + * so that you can make something happen if you press a button. + * This hopefully should make it simple to implement routines, both in autonomous and during teleop + * + */ + public interface Action { + /** + * Called once when the object is activated + */ public default void start(){} + /** + * Called every loop the object is active, after it is activated + */ public default void enabled(){} + /** + * Called once when the object is deactivated + */ public default void stop(){} + /** + * Called every loop when the object is deactivated + * Note: this may not be called by every implementation + */ public default void disabled(){} } diff --git a/src/org/montclairrobotics/sprocket/actions/MultiAction.java b/src/org/montclairrobotics/sprocket/actions/MultiAction.java new file mode 100644 index 0000000..260ba22 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/MultiAction.java @@ -0,0 +1,45 @@ +package org.montclairrobotics.sprocket.actions; + +/** + * One can perform multiple actions with the same trigger with this utility object + * + */ + +public class MultiAction implements Action { + + private Action[] actions; + + public MultiAction(Action... actions) + { + this.actions=actions; + } + + + @Override + public void start() { + for(Action a : actions) { + a.start(); + } + } + + @Override + public void enabled() { + for(Action a : actions) { + a.enabled(); + } + } + + @Override + public void stop() { + for(Action a : actions) { + a.stop(); + } + } + + @Override + public void disabled() { + for(Action a : actions) { + a.disabled(); + } + } +} diff --git a/src/org/montclairrobotics/sprocket/states/MultiState.java b/src/org/montclairrobotics/sprocket/actions/MultiState.java similarity index 64% rename from src/org/montclairrobotics/sprocket/states/MultiState.java rename to src/org/montclairrobotics/sprocket/actions/MultiState.java index 2034dfc..b99618c 100644 --- a/src/org/montclairrobotics/sprocket/states/MultiState.java +++ b/src/org/montclairrobotics/sprocket/actions/MultiState.java @@ -1,18 +1,23 @@ -package org.montclairrobotics.sprocket.states; +package org.montclairrobotics.sprocket.actions; import org.montclairrobotics.sprocket.utils.Input; -public class MultiState implements State { +/** + * Can perform multiple actions at once + * + */ + +public class MultiState extends MultiAction implements State { - private State[] states; private Input done; public MultiState(Input done, State... states) { - this.states = states; + super(states); this.done = done; } public MultiState(int stateToStopAt, State... states) { + super(states); if(stateToStopAt < 0 || stateToStopAt > states.length) { this.done = new Input() { @Override @@ -33,34 +38,11 @@ public Boolean get() { } }; } - - this.states = states; } public MultiState(State... states) { this(-1, states); } - - @Override - public void start() { - for(State s : states) { - s.start(); - } - } - - @Override - public void stop() { - for(State s : states) { - s.stop(); - } - } - - @Override - public void enabled() { - for(State s : states) { - s.enabled(); - } - } @Override public boolean isDone() { diff --git a/src/org/montclairrobotics/sprocket/actions/State.java b/src/org/montclairrobotics/sprocket/actions/State.java new file mode 100644 index 0000000..00fe4ee --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/State.java @@ -0,0 +1,11 @@ +package org.montclairrobotics.sprocket.actions; + +/** + * The basic unit of a State Machine, very similar to Action, + * except States also know when they are finished. + * Consequently, state machines can be made which move from state to state when each one is finished. + * + */ +public interface State extends Action{ + boolean isDone(); +} diff --git a/src/org/montclairrobotics/sprocket/states/StateMachine.java b/src/org/montclairrobotics/sprocket/actions/StateMachine.java similarity index 81% rename from src/org/montclairrobotics/sprocket/states/StateMachine.java rename to src/org/montclairrobotics/sprocket/actions/StateMachine.java index 80dba4c..052eeea 100644 --- a/src/org/montclairrobotics/sprocket/states/StateMachine.java +++ b/src/org/montclairrobotics/sprocket/actions/StateMachine.java @@ -1,4 +1,4 @@ -package org.montclairrobotics.sprocket.states; +package org.montclairrobotics.sprocket.actions; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; @@ -21,11 +21,11 @@ public void start(boolean top) { this.top=top; index=0; - startState(); + states[index].start(); } @Override public void start() { - start(true); + start(false); } @Override @@ -45,7 +45,7 @@ public void enabled() { states[index].stop(); index++; if(isDone())return; - startState(); + states[index].start(); states[index].enabled(); } } @@ -65,15 +65,4 @@ public void update() { enabled(); } } - public void startState() - { - if(states[index] instanceof StateMachine) - { - ((StateMachine)states[index]).start(false); - } - else - { - states[index].start(); - } - } } diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index 9f5ae78..302fe74 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -1,8 +1,8 @@ package org.montclairrobotics.sprocket.auto; import org.montclairrobotics.sprocket.SprocketRobot; -import org.montclairrobotics.sprocket.states.State; -import org.montclairrobotics.sprocket.states.StateMachine; +import org.montclairrobotics.sprocket.actions.State; +import org.montclairrobotics.sprocket.actions.StateMachine; import org.montclairrobotics.sprocket.utils.Debug; @@ -35,7 +35,7 @@ public void start() public void stop() { super.stop(); - SprocketRobot.getDriveTrain().useDefaultInput(); + SprocketRobot.getMainDriveTrain().useDefaultInput(); } public String toString() { diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index cd66d74..6b7ee4c 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -1,10 +1,10 @@ package org.montclairrobotics.sprocket.auto; import org.montclairrobotics.sprocket.SprocketRobot; +import org.montclairrobotics.sprocket.actions.State; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.states.State; /** * This object represents a singular state or step in an autonomous routine. @@ -19,7 +19,7 @@ public abstract class AutoState extends AutoDTInput implements State { public final void start() { t=Updater.getTime(); - SprocketRobot.getDriveTrain().setTempInput(this); + SprocketRobot.getMainDriveTrain().setTempInput(this); userStart(); } @@ -33,7 +33,7 @@ public final void stop() /*tgtDir=Vector.ZERO; tgtTurn=Angle.ZERO;*/ //output.inputType=DTInput.Type.SPEED; - SprocketRobot.getDriveTrain().useDefaultInput(); + SprocketRobot.getMainDriveTrain().useDefaultInput(); } /** diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java index bf083c7..858915e 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java @@ -1,12 +1,12 @@ package org.montclairrobotics.sprocket.auto.states; +import org.montclairrobotics.sprocket.actions.MultiState; +import org.montclairrobotics.sprocket.actions.State; +import org.montclairrobotics.sprocket.actions.StateMachine; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Distance; -import org.montclairrobotics.sprocket.states.MultiState; -import org.montclairrobotics.sprocket.states.State; -import org.montclairrobotics.sprocket.states.StateMachine; import org.montclairrobotics.sprocket.utils.Input; public class DriveEncoderGyro extends StateMachine{ diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index 3609d91..9902d8c 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -49,7 +49,7 @@ public void userStart() { if(speedDashInput != null) { this.speed = speedDashInput.get(); } - this.dt = SprocketRobot.getDriveTrain(); + this.dt = SprocketRobot.getMainDriveTrain(); stopDist = new Distance(dt.getDistance().getY()+tgtDistance.get()); forwards=tgtDistance.get()>0; } diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java index d32ad55..2f82c15 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java @@ -19,7 +19,7 @@ public TurnEncoders(Angle turn, Angle turnSpeed) { @Override public void userStart() { - module = SprocketRobot.getDriveTrain().getModules()[0]; + module = SprocketRobot.getMainDriveTrain().getModules()[0]; double modulePos = module.getEnc().getDistance().get(); double finalPos = ((module.getOffset().getMagnitude() * 2 * Math.PI)/360) * turn.toDegrees(); if(module.getOffset().getX() < 0) { diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index 5053162..cc256f3 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -86,10 +86,10 @@ public Angle getTurn() { @Override public void start() { - SprocketRobot.getDriveTrain().setTempInput(this); + SprocketRobot.getMainDriveTrain().setTempInput(this); } @Override public void stop() { - SprocketRobot.getDriveTrain().useDefaultInput(); + SprocketRobot.getMainDriveTrain().useDefaultInput(); } } diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java index dc05c1f..ef97ccf 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java @@ -24,7 +24,7 @@ public DriveTrain(DriveModule... modules) { this.modules = modules; input=new ZeroDTInput(); pipeline=new ZeroPipeline(); - SprocketRobot.setDriveTrain(this); + SprocketRobot.setMainDriveTrain(this); Updater.add(this, Priority.DRIVE_CALC); } diff --git a/src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java b/src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java new file mode 100644 index 0000000..12ed2c8 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java @@ -0,0 +1,18 @@ +package org.montclairrobotics.sprocket.loop; + +import java.util.ArrayList; + +public class DisabledUpdater { + public static ArrayList updatables; + public static void add(Updatable u) + { + updatables.add(u); + } + public static void loop() + { + for(Updatable u:updatables) + { + u.update(); + } + } +} diff --git a/src/org/montclairrobotics/sprocket/states/State.java b/src/org/montclairrobotics/sprocket/states/State.java deleted file mode 100644 index 51ae465..0000000 --- a/src/org/montclairrobotics/sprocket/states/State.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.montclairrobotics.sprocket.states; - -import org.montclairrobotics.sprocket.actions.Action; - -public interface State extends Action{ - boolean isDone(); -} From 8e9ce6d96483a8ce41f61ea4b241bebf2f1b8b16 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 24 Jun 2017 13:08:28 -0400 Subject: [PATCH 15/71] Bye --- .../sprocket/SprocketRobot.java | 7 ---- .../control/FieldCentricDriveInput.java | 2 +- .../sprocket/core/Encoder.java | 1 - .../sprocket/core/SprocketRobot.java | 37 ------------------- .../sprocket/frc/FRCRobot.java | 5 --- .../sprocket/utils/InputDouble.java | 5 +++ 6 files changed, 6 insertions(+), 51 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/core/SprocketRobot.java diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/SprocketRobot.java index 20ef30e..7fd8f9e 100644 --- a/src/org/montclairrobotics/sprocket/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/SprocketRobot.java @@ -1,11 +1,6 @@ package org.montclairrobotics.sprocket; -import java.util.ArrayList; -import java.util.Arrays; - import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.auto.AutoState; import org.montclairrobotics.sprocket.drive.DriveTrain; import org.montclairrobotics.sprocket.loop.DisabledUpdater; import org.montclairrobotics.sprocket.loop.Priority; @@ -14,8 +9,6 @@ import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.IterativeRobot; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index cc256f3..8c8e206 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -6,8 +6,8 @@ import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.utils.SmoothVectorInput; import org.montclairrobotics.sprocket.utils.Input; +import org.montclairrobotics.sprocket.utils.SmoothVectorInput; public class FieldCentricDriveInput extends ArcadeDriveInput implements Action{ diff --git a/src/org/montclairrobotics/sprocket/core/Encoder.java b/src/org/montclairrobotics/sprocket/core/Encoder.java index e13c7e6..eac459c 100644 --- a/src/org/montclairrobotics/sprocket/core/Encoder.java +++ b/src/org/montclairrobotics/sprocket/core/Encoder.java @@ -1,6 +1,5 @@ package org.montclairrobotics.sprocket.core; -import org.montclairrobotics.sprocket.utils.Input; public interface Encoder { public double getSpeed(); diff --git a/src/org/montclairrobotics/sprocket/core/SprocketRobot.java b/src/org/montclairrobotics/sprocket/core/SprocketRobot.java deleted file mode 100644 index 1e56628..0000000 --- a/src/org/montclairrobotics/sprocket/core/SprocketRobot.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.montclairrobotics.sprocket.core; - -import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.drive.DriveTrain; -import org.montclairrobotics.sprocket.frc.DashboardSelector; -import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; - -public interface SprocketRobot extends Updatable{ - - public static AutoSelector selector=null; - public static DriveTrain driveTrain=null; - public static AutoMode selectedMode=null; - - public default void setup(){userSetup();} - public default void userSetup(){} - - public default void autoStart(){ - selectedMode=selector.get(); - - userAutoStart(); - } - public default void userAutoStart(){} - - public default void teleopStart(){userTeleopStart();} - public default void userTeleopStart(){} - - public default void loop(){Updater.loop();} - - public default void update(){} - - public default void disable(){userDisable();}; - public default void userDisable(){} - - public default void disableLoop(){userDisableLoop();} - public default void userDisableLoop(){} -} diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java index ebdbd05..09a79c5 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -1,11 +1,6 @@ package org.montclairrobotics.sprocket.frc; -import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.core.AutoSelector; import org.montclairrobotics.sprocket.core.SprocketRobot; -import org.montclairrobotics.sprocket.drive.DriveTrain; -import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updater; import edu.wpi.first.wpilibj.IterativeRobot; diff --git a/src/org/montclairrobotics/sprocket/utils/InputDouble.java b/src/org/montclairrobotics/sprocket/utils/InputDouble.java index c238e73..a64ad80 100644 --- a/src/org/montclairrobotics/sprocket/utils/InputDouble.java +++ b/src/org/montclairrobotics/sprocket/utils/InputDouble.java @@ -2,6 +2,11 @@ public abstract class InputDouble extends Number implements Input{ + /** + * + */ + private static final long serialVersionUID = 7875067748687693370L; + @Override public double doubleValue() { // TODO Auto-generated method stub From 0d2b08797323fff3eaac2f9fe1c0aeb64d9b546c Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 24 Jun 2017 13:09:01 -0400 Subject: [PATCH 16/71] Hi --- src/org/montclairrobotics/sprocket/auto/AutoMode.java | 2 +- src/org/montclairrobotics/sprocket/auto/AutoState.java | 2 +- .../montclairrobotics/sprocket/auto/states/DriveEncoders.java | 2 +- .../montclairrobotics/sprocket/auto/states/TurnEncoders.java | 2 +- .../sprocket/control/FieldCentricDriveInput.java | 2 +- .../montclairrobotics/sprocket/{ => core}/SprocketRobot.java | 2 +- src/org/montclairrobotics/sprocket/drive/DriveTrain.java | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) rename src/org/montclairrobotics/sprocket/{ => core}/SprocketRobot.java (98%) diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index 302fe74..82a611f 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -1,8 +1,8 @@ package org.montclairrobotics.sprocket.auto; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.actions.State; import org.montclairrobotics.sprocket.actions.StateMachine; +import org.montclairrobotics.sprocket.core.SprocketRobot; import org.montclairrobotics.sprocket.utils.Debug; diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index 6b7ee4c..8496b49 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.auto; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.actions.State; +import org.montclairrobotics.sprocket.core.SprocketRobot; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Updater; diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index 9902d8c..adf2c00 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.auto.states; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoState; +import org.montclairrobotics.sprocket.core.SprocketRobot; import org.montclairrobotics.sprocket.drive.DriveTrain; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.XY; diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java index 2f82c15..6875929 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.auto.states; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoState; +import org.montclairrobotics.sprocket.core.SprocketRobot; import org.montclairrobotics.sprocket.drive.DriveModule; import org.montclairrobotics.sprocket.geometry.Angle; diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index 8c8e206..ac91142 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -1,8 +1,8 @@ package org.montclairrobotics.sprocket.control; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.Joystick; +import org.montclairrobotics.sprocket.core.SprocketRobot; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/core/SprocketRobot.java similarity index 98% rename from src/org/montclairrobotics/sprocket/SprocketRobot.java rename to src/org/montclairrobotics/sprocket/core/SprocketRobot.java index 7fd8f9e..168f78f 100644 --- a/src/org/montclairrobotics/sprocket/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/core/SprocketRobot.java @@ -1,4 +1,4 @@ -package org.montclairrobotics.sprocket; +package org.montclairrobotics.sprocket.core; import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.DriveTrain; diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java index ef97ccf..b0deb73 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.drive; -import org.montclairrobotics.sprocket.SprocketRobot; +import org.montclairrobotics.sprocket.core.SprocketRobot; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.Vector; From 5f2e6fc344e5d9c12de08c632c905e468ffeae02 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 24 Jun 2017 13:26:17 -0400 Subject: [PATCH 17/71] NO! --- .../sprocket/actions/Button.java | 76 +++++++++++++++++++ .../sprocket/actions/Togglable.java | 6 ++ .../sprocket/control/ButtonListener.java | 55 -------------- .../sprocket/utils/PIDTuner.java | 4 +- 4 files changed, 84 insertions(+), 57 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/actions/Button.java create mode 100644 src/org/montclairrobotics/sprocket/actions/Togglable.java delete mode 100644 src/org/montclairrobotics/sprocket/control/ButtonListener.java diff --git a/src/org/montclairrobotics/sprocket/actions/Button.java b/src/org/montclairrobotics/sprocket/actions/Button.java new file mode 100644 index 0000000..3ba6522 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/Button.java @@ -0,0 +1,76 @@ +package org.montclairrobotics.sprocket.actions; + +import org.montclairrobotics.sprocket.loop.Priority; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Input; + + +/** + * @author MHS Robotics + * Button is a class which allows for developers to program custom behaviors for + * when buttons are pressed/held/unpressed. This class works by hooking into + * the Sprocket control loop and calling anonymous functions passed in by + * developers. This system makes defining button behaviors incredibly simple. + */ +public class Button implements Updatable,Togglable { + + private Action action; + private boolean wasPressed=false; + private Input button; + public boolean enabled=true;; + + public Button() + { + Updater.add(this,Priority.CONTROL); + } + public Button(Input button) + { + this.button=button; + Updater.add(this, Priority.CONTROL); + } + + public Button setButton(Input button) + { + this.button=button; + return this; + } + public Button setAction(Action action) + { + this.action=action; + return this; + } + + @Override + public void update() { + if(enabled) + { + boolean pressed = button.get(); + if(action!=null) + { + if(pressed && !wasPressed) { + action.start(); + } + if(!pressed && wasPressed) { + action.stop(); + } + if(pressed) { + action.enabled(); + } else { + action.disabled(); + } + } + wasPressed = pressed; + } + } + @Override + public void enable() { + // TODO Auto-generated method stub + enabled=true; + } + @Override + public void disable() { + // TODO Auto-generated method stub + enabled=false; + } +} diff --git a/src/org/montclairrobotics/sprocket/actions/Togglable.java b/src/org/montclairrobotics/sprocket/actions/Togglable.java new file mode 100644 index 0000000..8dedbc2 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/Togglable.java @@ -0,0 +1,6 @@ +package org.montclairrobotics.sprocket.actions; + +public interface Togglable { + public void enable(); + public void disable(); +} diff --git a/src/org/montclairrobotics/sprocket/control/ButtonListener.java b/src/org/montclairrobotics/sprocket/control/ButtonListener.java deleted file mode 100644 index 8b6cc29..0000000 --- a/src/org/montclairrobotics/sprocket/control/ButtonListener.java +++ /dev/null @@ -1,55 +0,0 @@ -package org.montclairrobotics.sprocket.control; - -import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.utils.Input; - - -/** - * @author MHS Robotics - * Button is a class which allows for developers to program custom behaviors for - * when buttons are pressed/held/unpressed. This class works by hooking into - * the Sprocket control loop and calling anonymous functions passed in by - * developers. This system makes defining button behaviors incredibly simple. - */ -public class ButtonListener implements Updatable { - - private boolean wasPressed=false; - - private Action action; - - private Input button; - - public ButtonListener(Input button) - { - this.button=button; - Updater.add(this, Priority.CONTROL); - } - - public void setAction(Action action) - { - this.action=action; - } - - @Override - public void update() { - boolean pressed = button.get(); - if(action!=null) - { - if(pressed && !wasPressed) { - action.start(); - } - if(!pressed && wasPressed) { - action.stop(); - } - if(pressed) { - action.enabled(); - } else { - action.disabled(); - } - } - wasPressed = pressed; - } -} diff --git a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java index bddd8e1..961d6d7 100644 --- a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java +++ b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.utils; import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.control.ButtonListener; +import org.montclairrobotics.sprocket.actions.Button; import org.montclairrobotics.sprocket.frc.DashboardButton; import org.montclairrobotics.sprocket.frc.DashboardInput; @@ -27,7 +27,7 @@ public PIDTuner(Input TempP,Input cyclesPer10Sec,Input realI=new DashboardInput("PID Tuner I"); realD=new DashboardInput("PID Tuner D"); - new ButtonListener(apply).setAction(new Action(){ + new Button(apply).setAction(new Action(){ @Override public void start() { recalculatePIDs(); From abb4376f65b375e2950751676e76874d11267cac Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 24 Jun 2017 17:16:33 -0400 Subject: [PATCH 18/71] More actions --- .../sprocket/actions/Button.java | 14 +++-- .../sprocket/actions/SplitButton.java | 54 +++++++++++++++++++ .../sprocket/actions/Toggle.java | 24 +++++++++ 3 files changed, 88 insertions(+), 4 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/actions/SplitButton.java create mode 100644 src/org/montclairrobotics/sprocket/actions/Toggle.java diff --git a/src/org/montclairrobotics/sprocket/actions/Button.java b/src/org/montclairrobotics/sprocket/actions/Button.java index 3ba6522..5045e93 100644 --- a/src/org/montclairrobotics/sprocket/actions/Button.java +++ b/src/org/montclairrobotics/sprocket/actions/Button.java @@ -13,7 +13,7 @@ * the Sprocket control loop and calling anonymous functions passed in by * developers. This system makes defining button behaviors incredibly simple. */ -public class Button implements Updatable,Togglable { +public class Button implements Updatable,Togglable,Input { private Action action; private boolean wasPressed=false; @@ -40,12 +40,16 @@ public Button setAction(Action action) this.action=action; return this; } + public Action getAction() + { + return action; + } @Override public void update() { if(enabled) { - boolean pressed = button.get(); + boolean pressed = get(); if(action!=null) { if(pressed && !wasPressed) { @@ -65,12 +69,14 @@ public void update() { } @Override public void enable() { - // TODO Auto-generated method stub enabled=true; } @Override public void disable() { - // TODO Auto-generated method stub enabled=false; } + @Override + public Boolean get() { + return button.get(); + } } diff --git a/src/org/montclairrobotics/sprocket/actions/SplitButton.java b/src/org/montclairrobotics/sprocket/actions/SplitButton.java new file mode 100644 index 0000000..04b5dbb --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/SplitButton.java @@ -0,0 +1,54 @@ +package org.montclairrobotics.sprocket.actions; + +import org.montclairrobotics.sprocket.utils.Input; + +public class SplitButton extends Button{ + + private Input onButton; + private Input offButton; + + private boolean active=false; + + public SplitButton() + { + super(); + } + public SplitButton(Input onButton,Input offButton) + { + this.onButton=onButton; + this.offButton=offButton; + } + + public Input getOnButton() { + return onButton; + } + public SplitButton setOnButton(Input onButton) { + this.onButton = onButton; + return this; + } + public Input getOffButton() { + return offButton; + } + public SplitButton setOffButton(Input offButton) { + this.offButton = offButton; + return this; + } + + public void update() + { + if(onButton.get()) + { + active=true; + } + else if(active&&offButton.get()) + { + active=false; + } + super.update(); + } + public Boolean get() + { + return active; + } + +} diff --git a/src/org/montclairrobotics/sprocket/actions/Toggle.java b/src/org/montclairrobotics/sprocket/actions/Toggle.java new file mode 100644 index 0000000..33d20fd --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/Toggle.java @@ -0,0 +1,24 @@ +package org.montclairrobotics.sprocket.actions; + +public class Toggle implements Action{ + + private Togglable obj; + + public Toggle(Togglable obj) + { + this.obj=obj; + } + + @Override + public void start() + { + obj.enable(); + } + + @Override + public void stop() + { + obj.disable(); + } + +} From 53fd05d1a38d4dfb015cadc1751c3df0298940ae Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 24 Jun 2017 17:21:37 -0400 Subject: [PATCH 19/71] SprocketRobot Renames --- .../sprocket/core/SprocketRobot.java | 37 ++++++++----------- 1 file changed, 16 insertions(+), 21 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/core/SprocketRobot.java b/src/org/montclairrobotics/sprocket/core/SprocketRobot.java index 168f78f..ad438c2 100644 --- a/src/org/montclairrobotics/sprocket/core/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/core/SprocketRobot.java @@ -36,11 +36,10 @@ public SprocketRobot() //Stuff you can override if you feel like it - @Override - public void robotInit(){} - public void userStart(Mode mode){} - public void userStop(){} - public void userUpdate(){} + public void init(){} + public void start(Mode mode){} + public void stop(){} + public void update(){} //STUFF WE OVERRIDE IN THIS CLASS @@ -48,25 +47,29 @@ public void userUpdate(){} public void startCompetition() { super.startCompetition(); } - + @Override + public void robotInit() + { + init(); + } @Override public final void autonomousInit() { - start(Mode.AUTO); + sprocketStart(Mode.AUTO); } @Override public final void teleopInit() { - start(Mode.TELEOP); + sprocketStart(Mode.TELEOP); } @Override public final void testInit() { - start(Mode.TEST); + sprocketStart(Mode.TEST); } @Override public final void disabledInit() { - stop(); + sprocketStop(); } @Override @@ -98,7 +101,7 @@ public static DriveTrain getMainDriveTrain() { public static void setMainDriveTrain(DriveTrain dt) { driveTrain = dt; } - public final void start(Mode mode) + public final void sprocketStart(Mode mode) { switch(mode) { @@ -119,21 +122,13 @@ public final void start(Mode mode) { currentAction.start(); } - userStart(mode); + start(mode); } public final void sprocketUpdate() { Updater.loop(); } - public final void update() - { - if(currentAction!=null) - { - currentAction.enabled(); - } - userUpdate(); - } - public final void stop() + public final void sprocketStop() { if(currentAction!=null) { From ff390aabd806553c8c9e593302b51692a2b8fc78 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 24 Jun 2017 18:20:55 -0400 Subject: [PATCH 20/71] This might just be the best class I have made yet. --- .../sprocket/drive/utils/MyState.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 src/org/montclairrobotics/sprocket/drive/utils/MyState.java diff --git a/src/org/montclairrobotics/sprocket/drive/utils/MyState.java b/src/org/montclairrobotics/sprocket/drive/utils/MyState.java new file mode 100644 index 0000000..fc0c5df --- /dev/null +++ b/src/org/montclairrobotics/sprocket/drive/utils/MyState.java @@ -0,0 +1,14 @@ +package org.montclairrobotics.sprocket.drive.utils; + +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.utils.Input; + +public class MyState { + public Input + location, + velocity; + public Input + rotation, + rotationSpeed; +} From 6fb9d6c61bdc90d16afc7b93c8319ddadc750e6d Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 24 Jun 2017 18:23:30 -0400 Subject: [PATCH 21/71] Get and Set --- .../sprocket/drive/utils/MyState.java | 28 +++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/drive/utils/MyState.java b/src/org/montclairrobotics/sprocket/drive/utils/MyState.java index fc0c5df..91773cb 100644 --- a/src/org/montclairrobotics/sprocket/drive/utils/MyState.java +++ b/src/org/montclairrobotics/sprocket/drive/utils/MyState.java @@ -5,10 +5,34 @@ import org.montclairrobotics.sprocket.utils.Input; public class MyState { - public Input + private static Input location, velocity; - public Input + private static Input rotation, rotationSpeed; + public static Vector getLocation() { + return location.get(); + } + public static void setLocation(Input location) { + MyState.location = location; + } + public static Vector getVelocity() { + return velocity.get(); + } + public static void setVelocity(Input velocity) { + MyState.velocity = velocity; + } + public static Angle getRotation() { + return rotation.get(); + } + public static void setRotation(Input rotation) { + MyState.rotation = rotation; + } + public static Angle getRotationSpeed() { + return rotationSpeed.get(); + } + public static void setRotationSpeed(Input rotationSpeed) { + MyState.rotationSpeed = rotationSpeed; + } } From 20b28fd8473c720442d507c7a12310834dcdf796 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Mon, 26 Jun 2017 18:38:43 -0400 Subject: [PATCH 22/71] Added api.getMyZRState(Array[12]) --- .../sprocket/auto/AutoMode.java | 4 +- .../sprocket/drive/utils/MyState.java | 44 ++++++++++++------- 2 files changed, 31 insertions(+), 17 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index 82a611f..c2571ea 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -7,11 +7,11 @@ /** - * The AutoMode class is the fundamental component to Sprocket's autonomous + * The AutoMode class is the master class Sprocket's autonomous * framework. All AutoModes consist of an AutoMode class which contains several * AutoStates. These AutoModes contain a StateMachine which runs all * the AutoStates that it contains before ending. When instantiated, these modes - * will automatically be spent to a SmartDashboard chooser. + * will automatically be sent to a SmartDashboard chooser. */ public class AutoMode extends StateMachine{ diff --git a/src/org/montclairrobotics/sprocket/drive/utils/MyState.java b/src/org/montclairrobotics/sprocket/drive/utils/MyState.java index 91773cb..891b64f 100644 --- a/src/org/montclairrobotics/sprocket/drive/utils/MyState.java +++ b/src/org/montclairrobotics/sprocket/drive/utils/MyState.java @@ -5,34 +5,48 @@ import org.montclairrobotics.sprocket.utils.Input; public class MyState { - private static Input + public static Input location, velocity; - private static Input + public static Input rotation, rotationSpeed; - public static Vector getLocation() { + public static Vector getAbsLocation() { + if(location==null)return Vector.ZERO; return location.get(); } - public static void setLocation(Input location) { - MyState.location = location; - } public static Vector getVelocity() { + if(velocity==null)return Vector.ZERO; return velocity.get(); } - public static void setVelocity(Input velocity) { - MyState.velocity = velocity; - } - public static Angle getRotation() { + public static Angle getAbsRotation() { + if(rotation==null)return Angle.ZERO; return rotation.get(); } - public static void setRotation(Input rotation) { - MyState.rotation = rotation; - } public static Angle getRotationSpeed() { + if(rotationSpeed==null)return Angle.ZERO; return rotationSpeed.get(); } - public static void setRotationSpeed(Input rotationSpeed) { - MyState.rotationSpeed = rotationSpeed; + + private Vector zeroLoc=Vector.ZERO; + private Angle zeroAngle=Angle.ZERO; + + public MyState() + { + zeroLoc=getAbsLocation(); + zeroAngle=getAbsRotation(); + } + public MyState(Vector loc,Angle rot) + { + zeroLoc=loc.subtract(getAbsLocation()); + zeroAngle=rot.subtract(getAbsRotation()); + } + public Vector getRelLocation() + { + return getAbsLocation().subtract(zeroLoc); + } + public Angle getRelRotation() + { + return getAbsRotation().subtract(zeroAngle); } } From b6786398574eb54a06875fd5c04a22e5a5c1a276 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 16:04:28 -0400 Subject: [PATCH 23/71] I forgot what I did but here is a commit I think --- .../sprocket/auto/AutoMode.java | 4 +- .../sprocket/auto/AutoState.java | 6 +- .../sprocket/auto/states/DriveEncoders.java | 4 +- .../sprocket/auto/states/TurnEncoders.java | 4 +- .../control/FieldCentricDriveInput.java | 6 +- .../sprocket/core/Defaults.java | 7 + .../sprocket/core/IRobot.java | 12 ++ .../{SprocketRobot.java => Sprocket.java} | 16 +- .../sprocket/drive/DriveTrain.java | 4 +- .../sprocket/frc/FRCRobot.java | 161 +++++++++++++----- 10 files changed, 152 insertions(+), 72 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/core/Defaults.java create mode 100644 src/org/montclairrobotics/sprocket/core/IRobot.java rename src/org/montclairrobotics/sprocket/core/{SprocketRobot.java => Sprocket.java} (88%) diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index c2571ea..39523f2 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -2,7 +2,7 @@ import org.montclairrobotics.sprocket.actions.State; import org.montclairrobotics.sprocket.actions.StateMachine; -import org.montclairrobotics.sprocket.core.SprocketRobot; +import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.utils.Debug; @@ -35,7 +35,7 @@ public void start() public void stop() { super.stop(); - SprocketRobot.getMainDriveTrain().useDefaultInput(); + Sprocket.getMainDriveTrain().useDefaultInput(); } public String toString() { diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index 8496b49..fe987fe 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.auto; import org.montclairrobotics.sprocket.actions.State; -import org.montclairrobotics.sprocket.core.SprocketRobot; +import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Updater; @@ -19,7 +19,7 @@ public abstract class AutoState extends AutoDTInput implements State { public final void start() { t=Updater.getTime(); - SprocketRobot.getMainDriveTrain().setTempInput(this); + Sprocket.getMainDriveTrain().setTempInput(this); userStart(); } @@ -33,7 +33,7 @@ public final void stop() /*tgtDir=Vector.ZERO; tgtTurn=Angle.ZERO;*/ //output.inputType=DTInput.Type.SPEED; - SprocketRobot.getMainDriveTrain().useDefaultInput(); + Sprocket.getMainDriveTrain().useDefaultInput(); } /** diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index adf2c00..aefffd5 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.auto.states; import org.montclairrobotics.sprocket.auto.AutoState; -import org.montclairrobotics.sprocket.core.SprocketRobot; +import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.drive.DriveTrain; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.XY; @@ -49,7 +49,7 @@ public void userStart() { if(speedDashInput != null) { this.speed = speedDashInput.get(); } - this.dt = SprocketRobot.getMainDriveTrain(); + this.dt = Sprocket.getMainDriveTrain(); stopDist = new Distance(dt.getDistance().getY()+tgtDistance.get()); forwards=tgtDistance.get()>0; } diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java index 6875929..ada5cb3 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.auto.states; import org.montclairrobotics.sprocket.auto.AutoState; -import org.montclairrobotics.sprocket.core.SprocketRobot; +import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.drive.DriveModule; import org.montclairrobotics.sprocket.geometry.Angle; @@ -19,7 +19,7 @@ public TurnEncoders(Angle turn, Angle turnSpeed) { @Override public void userStart() { - module = SprocketRobot.getMainDriveTrain().getModules()[0]; + module = Sprocket.getMainDriveTrain().getModules()[0]; double modulePos = module.getEnc().getDistance().get(); double finalPos = ((module.getOffset().getMagnitude() * 2 * Math.PI)/360) * turn.toDegrees(); if(module.getOffset().getX() < 0) { diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index ac91142..1134311 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -2,7 +2,7 @@ import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.Joystick; -import org.montclairrobotics.sprocket.core.SprocketRobot; +import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; @@ -86,10 +86,10 @@ public Angle getTurn() { @Override public void start() { - SprocketRobot.getMainDriveTrain().setTempInput(this); + Sprocket.getMainDriveTrain().setTempInput(this); } @Override public void stop() { - SprocketRobot.getMainDriveTrain().useDefaultInput(); + Sprocket.getMainDriveTrain().useDefaultInput(); } } diff --git a/src/org/montclairrobotics/sprocket/core/Defaults.java b/src/org/montclairrobotics/sprocket/core/Defaults.java new file mode 100644 index 0000000..0d42f03 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/Defaults.java @@ -0,0 +1,7 @@ +package org.montclairrobotics.sprocket.core; + +import org.montclairrobotics.sprocket.drive.DriveTrain; + +public class Defaults { + public static DriveTrain driveTrain; +} diff --git a/src/org/montclairrobotics/sprocket/core/IRobot.java b/src/org/montclairrobotics/sprocket/core/IRobot.java new file mode 100644 index 0000000..52d0c4b --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/IRobot.java @@ -0,0 +1,12 @@ +package org.montclairrobotics.sprocket.core; + +public interface IRobot { + + public enum Mode {AUTO,TELEOP,TEST}; + + //Stuff user overrides + public void init(){} + public void start(Mode mode){} + public void stop(){} + public void update(){} +} diff --git a/src/org/montclairrobotics/sprocket/core/SprocketRobot.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java similarity index 88% rename from src/org/montclairrobotics/sprocket/core/SprocketRobot.java rename to src/org/montclairrobotics/sprocket/core/Sprocket.java index ad438c2..f4531e3 100644 --- a/src/org/montclairrobotics/sprocket/core/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -16,10 +16,8 @@ * This class is basically just a wrapper around iterative robot which all Sprocket * robots must extend. */ -public abstract class SprocketRobot extends IterativeRobot implements Updatable{ +public class Sprocket extends IterativeRobot implements Updatable{ - public enum Mode {AUTO,TELEOP,TEST}; - public Input autoActionInput, teleopActionInput, @@ -27,20 +25,14 @@ public enum Mode {AUTO,TELEOP,TEST}; private Action currentAction; - private static DriveTrain driveTrain; + private IRobot robot; - public SprocketRobot() + public Sprocket(IRobot robot) { + this.robot=robot; Updater.add(this,Priority.NORMAL); } - //Stuff you can override if you feel like it - - public void init(){} - public void start(Mode mode){} - public void stop(){} - public void update(){} - //STUFF WE OVERRIDE IN THIS CLASS @Override diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java index b0deb73..94d2ee0 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.drive; -import org.montclairrobotics.sprocket.core.SprocketRobot; +import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.Vector; @@ -24,7 +24,7 @@ public DriveTrain(DriveModule... modules) { this.modules = modules; input=new ZeroDTInput(); pipeline=new ZeroPipeline(); - SprocketRobot.setMainDriveTrain(this); + Sprocket.setMainDriveTrain(this); Updater.add(this, Priority.DRIVE_CALC); } diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java index 09a79c5..61bd414 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -1,58 +1,127 @@ -package org.montclairrobotics.sprocket.frc; +package org.montclairrobotics.sprocket.core; -import org.montclairrobotics.sprocket.core.SprocketRobot; +import org.montclairrobotics.sprocket.actions.Action; +import org.montclairrobotics.sprocket.core.IRobot; +import org.montclairrobotics.sprocket.core.Sprocket; +import org.montclairrobotics.sprocket.drive.DriveTrain; +import org.montclairrobotics.sprocket.loop.DisabledUpdater; +import org.montclairrobotics.sprocket.loop.Priority; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.IterativeRobot; -public class FRCRobot extends IterativeRobot implements SprocketRobot{ - + +/** + * @author MHS Robotics + * This class is basically just a wrapper around iterative robot which all Sprocket + * robots must extend. + */ +public abstract class FRCRobot extends IterativeRobot implements IRobot{ + + public Sprocket sprocket; - @Override - public final void robotInit() - { - setup(); - } - @Override - public final void autonomousInit() - { - autoStart(); - } - @Override - public final void teleopInit() - { - teleopStart(); - } - @Override - public final void autonomousPeriodic() + public FRCRobot() { - loop(); - } - @Override - public final void teleopPeriodic() - { - loop(); - } - @Override - public final void disabledInit() - { - disable(); - } - @Override - public final void disabledPeriodic() - { - disableLoop(); + sprocket=new Sprocket(this); } - //========================================================== - @Override - public final void setup() - { - SprocketRobot.selector=new DashboardSelector(); + //STUFF WE OVERRIDE IN THIS CLASS + @Override + public void startCompetition() { + super.startCompetition(); + } + @Override + public void robotInit() + { + init(); + } + @Override + public final void autonomousInit() { + sprocket.sprocketStart(Mode.AUTO); + } + + @Override + public final void teleopInit() { + sprocketStart(Mode.TELEOP); + } + + @Override + public final void testInit() { + sprocketStart(Mode.TEST); + } + + @Override + public final void disabledInit() { + sprocketStop(); + } + + @Override + public final void autonomousPeriodic() { + sprocketUpdate(); + } + + @Override + public final void teleopPeriodic() { + sprocketUpdate(); + } + + @Override + public final void testPeriodic() { + sprocketUpdate(); + } + + @Override + public final void disabledPeriodic() { + sprocketDisabled(); + } + + + //OUR STUFF HERE + public static DriveTrain getMainDriveTrain() { + return driveTrain; } - public void autoStart() - { - selectedMode=selector.get(); + public static void setMainDriveTrain(DriveTrain dt) { + driveTrain = dt; } - + public final void sprocketStart(Mode mode) + { + switch(mode) + { + case AUTO: + if(autoActionInput!=null) + currentAction=autoActionInput.get(); + break; + case TELEOP: + if(teleopActionInput!=null) + currentAction=teleopActionInput.get(); + break; + case TEST: + if(testActionInput!=null) + currentAction=testActionInput.get(); + break; + } + if(currentAction!=null) + { + currentAction.start(); + } + start(mode); + } + public final void sprocketUpdate() + { + Updater.loop(); + } + public final void sprocketStop() + { + if(currentAction!=null) + { + currentAction.stop(); + } + } + public final void sprocketDisabled() + { + DisabledUpdater.loop(); + } } From 6bb7645903271e2f8fdd23416ae7c73bcc5c359b Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 21:20:17 -0400 Subject: [PATCH 24/71] I don't know, I think I did something I guess. Maybe. --- .../sprocket/actions/Reverse.java | 28 +++++++ .../sprocket/control/ArcadeDriveInput.java | 6 +- .../sprocket/control/ExponentDriveInput.java | 6 +- .../control/FieldCentricDriveInput.java | 6 +- .../sprocket/control/JoystickYAxis.java | 6 +- .../sprocket/control/SquaredDriveInput.java | 4 +- .../sprocket/core/Defaults.java | 7 -- .../{AutoSelector.java => IAutoSelector.java} | 2 +- .../core/{Button.java => IButton.java} | 2 +- .../core/{Debugger.java => IDebugger.java} | 2 +- .../core/{Encoder.java => IEncoder.java} | 2 +- .../core/{Joystick.java => IJoystick.java} | 2 +- .../sprocket/core/{Motor.java => IMotor.java} | 2 +- .../sprocket/core/IRobot.java | 12 +-- .../sprocket/core/Sprocket.java | 80 +++++-------------- .../sprocket/drive/ControlledMotor.java | 10 +-- .../sprocket/drive/DriveModule.java | 12 +-- .../sprocket/drive/DriveTrainBuilder.java | 10 +-- .../frc/{FRCButton.java => Button.java} | 6 +- .../sprocket/frc/DashboardDebug.java | 4 +- .../sprocket/frc/DashboardSelector.java | 4 +- .../sprocket/frc/FRCEncoder.java | 2 +- .../sprocket/frc/FRCJoystick.java | 2 +- .../sprocket/frc/FRCRobot.java | 70 +++------------- .../frc/{FRCMotor.java => Motor.java} | 8 +- .../sprocket/motors/Module.java | 10 +-- .../sprocket/motors/SEncoder.java | 8 +- .../sprocket/utils/Debug.java | 9 +-- .../montclairrobotics/sprocket/utils/PID.java | 4 +- 29 files changed, 127 insertions(+), 199 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/actions/Reverse.java delete mode 100644 src/org/montclairrobotics/sprocket/core/Defaults.java rename src/org/montclairrobotics/sprocket/core/{AutoSelector.java => IAutoSelector.java} (81%) rename src/org/montclairrobotics/sprocket/core/{Button.java => IButton.java} (67%) rename src/org/montclairrobotics/sprocket/core/{Debugger.java => IDebugger.java} (82%) rename src/org/montclairrobotics/sprocket/core/{Encoder.java => IEncoder.java} (82%) rename src/org/montclairrobotics/sprocket/core/{Joystick.java => IJoystick.java} (76%) rename src/org/montclairrobotics/sprocket/core/{Motor.java => IMotor.java} (74%) rename src/org/montclairrobotics/sprocket/frc/{FRCButton.java => Button.java} (84%) rename src/org/montclairrobotics/sprocket/frc/{FRCMotor.java => Motor.java} (90%) diff --git a/src/org/montclairrobotics/sprocket/actions/Reverse.java b/src/org/montclairrobotics/sprocket/actions/Reverse.java new file mode 100644 index 0000000..2dbe5ca --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/Reverse.java @@ -0,0 +1,28 @@ +package org.montclairrobotics.sprocket.actions; + +public class Reverse implements Action{ + + private Action a; + + public Reverse(Action a) + { + this.a=a; + } + + public void start() + { + a.stop(); + } + public void enabled() + { + a.disabled(); + } + public void stop() + { + a.start(); + } + public void disabled() + { + a.enabled(); + } +} diff --git a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java index d099925..2fbd997 100644 --- a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.control; -import org.montclairrobotics.sprocket.core.Joystick; +import org.montclairrobotics.sprocket.core.IJoystick; import org.montclairrobotics.sprocket.drive.DTInput; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Radians; @@ -19,7 +19,7 @@ */ public class ArcadeDriveInput implements DTInput, Updatable { - private Joystick stick; + private IJoystick stick; private Vector dir; private Angle turn; @@ -32,7 +32,7 @@ public class ArcadeDriveInput implements DTInput, Updatable { * Instantiates an ArcadeDriveInput with default scaling values * @param stick The joystick that will be used for DriveTrain control */ - public ArcadeDriveInput(Joystick stick) { + public ArcadeDriveInput(IJoystick stick) { this.stick = stick; Updater.add(this, Priority.INPUT); } diff --git a/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java b/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java index 3b00cb7..706e9ba 100644 --- a/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.control; -import org.montclairrobotics.sprocket.core.Joystick; +import org.montclairrobotics.sprocket.core.IJoystick; public class ExponentDriveInput extends ArcadeDriveInput @@ -8,11 +8,11 @@ public class ExponentDriveInput extends ArcadeDriveInput private double p; - public ExponentDriveInput(Joystick stick) { + public ExponentDriveInput(IJoystick stick) { this(stick,0.5); // TODO Auto-generated constructor stub } - public ExponentDriveInput(Joystick stick,double p) { + public ExponentDriveInput(IJoystick stick,double p) { super(stick); this.p=p; // TODO Auto-generated constructor stub diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index 1134311..bb1f477 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.control; import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.core.Joystick; +import org.montclairrobotics.sprocket.core.IJoystick; import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; @@ -22,11 +22,11 @@ public class FieldCentricDriveInput extends ArcadeDriveInput implements Action{ private static final int SMOOTH_LEN=10; - public FieldCentricDriveInput(Joystick stick,GyroCorrection gyro) + public FieldCentricDriveInput(IJoystick stick,GyroCorrection gyro) { this(stick,gyro,true); } - public FieldCentricDriveInput(Joystick stick,GyroCorrection gyro,boolean rotToVector) + public FieldCentricDriveInput(IJoystick stick,GyroCorrection gyro,boolean rotToVector) { super(stick); this.gyro=gyro; diff --git a/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java b/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java index 162f422..4e305a5 100644 --- a/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java +++ b/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.control; -import org.montclairrobotics.sprocket.core.Joystick; +import org.montclairrobotics.sprocket.core.IJoystick; import org.montclairrobotics.sprocket.utils.Input; @@ -11,13 +11,13 @@ */ public class JoystickYAxis implements Input { - private Joystick stick; + private IJoystick stick; /** * @param stick The Joystick */ - public JoystickYAxis(Joystick stick) { + public JoystickYAxis(IJoystick stick) { this.stick = stick; } diff --git a/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java b/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java index c218fce..115499c 100644 --- a/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.control; -import org.montclairrobotics.sprocket.core.Joystick; +import org.montclairrobotics.sprocket.core.IJoystick; public class SquaredDriveInput extends ArcadeDriveInput{ @@ -8,7 +8,7 @@ public class SquaredDriveInput extends ArcadeDriveInput{ private double xSensitivity = 1.0; private double ySensitivity = 1.0; - public SquaredDriveInput(Joystick stick) { + public SquaredDriveInput(IJoystick stick) { super(stick); // TODO Auto-generated constructor stub } diff --git a/src/org/montclairrobotics/sprocket/core/Defaults.java b/src/org/montclairrobotics/sprocket/core/Defaults.java deleted file mode 100644 index 0d42f03..0000000 --- a/src/org/montclairrobotics/sprocket/core/Defaults.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.montclairrobotics.sprocket.core; - -import org.montclairrobotics.sprocket.drive.DriveTrain; - -public class Defaults { - public static DriveTrain driveTrain; -} diff --git a/src/org/montclairrobotics/sprocket/core/AutoSelector.java b/src/org/montclairrobotics/sprocket/core/IAutoSelector.java similarity index 81% rename from src/org/montclairrobotics/sprocket/core/AutoSelector.java rename to src/org/montclairrobotics/sprocket/core/IAutoSelector.java index 50e48ab..ea724d1 100644 --- a/src/org/montclairrobotics/sprocket/core/AutoSelector.java +++ b/src/org/montclairrobotics/sprocket/core/IAutoSelector.java @@ -3,7 +3,7 @@ import org.montclairrobotics.sprocket.auto.AutoMode; import org.montclairrobotics.sprocket.utils.Input; -public interface AutoSelector extends Input{ +public interface IAutoSelector extends Input{ void addAutoMode(AutoMode mode); diff --git a/src/org/montclairrobotics/sprocket/core/Button.java b/src/org/montclairrobotics/sprocket/core/IButton.java similarity index 67% rename from src/org/montclairrobotics/sprocket/core/Button.java rename to src/org/montclairrobotics/sprocket/core/IButton.java index 5662256..0cf969f 100644 --- a/src/org/montclairrobotics/sprocket/core/Button.java +++ b/src/org/montclairrobotics/sprocket/core/IButton.java @@ -2,6 +2,6 @@ import org.montclairrobotics.sprocket.utils.Input; -public interface Button extends Input{ +public interface IButton extends Input{ } diff --git a/src/org/montclairrobotics/sprocket/core/Debugger.java b/src/org/montclairrobotics/sprocket/core/IDebugger.java similarity index 82% rename from src/org/montclairrobotics/sprocket/core/Debugger.java rename to src/org/montclairrobotics/sprocket/core/IDebugger.java index a12f29c..c055cc3 100644 --- a/src/org/montclairrobotics/sprocket/core/Debugger.java +++ b/src/org/montclairrobotics/sprocket/core/IDebugger.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.core; -public interface Debugger { +public interface IDebugger { public void debugStr(String key,String val); public void debugNum(String key,double val); } diff --git a/src/org/montclairrobotics/sprocket/core/Encoder.java b/src/org/montclairrobotics/sprocket/core/IEncoder.java similarity index 82% rename from src/org/montclairrobotics/sprocket/core/Encoder.java rename to src/org/montclairrobotics/sprocket/core/IEncoder.java index eac459c..be262ff 100644 --- a/src/org/montclairrobotics/sprocket/core/Encoder.java +++ b/src/org/montclairrobotics/sprocket/core/IEncoder.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.core; -public interface Encoder { +public interface IEncoder { public double getSpeed(); public double getDistance(); public void reset(); diff --git a/src/org/montclairrobotics/sprocket/core/Joystick.java b/src/org/montclairrobotics/sprocket/core/IJoystick.java similarity index 76% rename from src/org/montclairrobotics/sprocket/core/Joystick.java rename to src/org/montclairrobotics/sprocket/core/IJoystick.java index a624a40..c4901ca 100644 --- a/src/org/montclairrobotics/sprocket/core/Joystick.java +++ b/src/org/montclairrobotics/sprocket/core/IJoystick.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.core; -public interface Joystick { +public interface IJoystick { public double getX(); public double getY(); } diff --git a/src/org/montclairrobotics/sprocket/core/Motor.java b/src/org/montclairrobotics/sprocket/core/IMotor.java similarity index 74% rename from src/org/montclairrobotics/sprocket/core/Motor.java rename to src/org/montclairrobotics/sprocket/core/IMotor.java index 9ecb7ec..21e0d67 100644 --- a/src/org/montclairrobotics/sprocket/core/Motor.java +++ b/src/org/montclairrobotics/sprocket/core/IMotor.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.core; -public interface Motor { +public interface IMotor { void set(double power); diff --git a/src/org/montclairrobotics/sprocket/core/IRobot.java b/src/org/montclairrobotics/sprocket/core/IRobot.java index 52d0c4b..6dd6921 100644 --- a/src/org/montclairrobotics/sprocket/core/IRobot.java +++ b/src/org/montclairrobotics/sprocket/core/IRobot.java @@ -1,12 +1,12 @@ package org.montclairrobotics.sprocket.core; -public interface IRobot { +import org.montclairrobotics.sprocket.loop.Updatable; - public enum Mode {AUTO,TELEOP,TEST}; +public interface IRobot extends Updatable{ //Stuff user overrides - public void init(){} - public void start(Mode mode){} - public void stop(){} - public void update(){} + public default void init(){} + public default void start(Sprocket.MODE mode){} + public default void stop(){} + public default void update(){} } diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index f4531e3..8048557 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -2,6 +2,7 @@ import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.DriveTrain; +import org.montclairrobotics.sprocket.frc.DashboardDebug; import org.montclairrobotics.sprocket.loop.DisabledUpdater; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; @@ -16,75 +17,26 @@ * This class is basically just a wrapper around iterative robot which all Sprocket * robots must extend. */ -public class Sprocket extends IterativeRobot implements Updatable{ +public class Sprocket{ + public static DriveTrain driveTrain; + public static IDebugger debugger=new DashboardDebug(); + + private IRobot robot; + + public enum MODE {AUTO,TELEOP,TEST,DISABLED}; + public MODE curMode; public Input autoActionInput, teleopActionInput, testActionInput; - private Action currentAction; - private IRobot robot; - public Sprocket(IRobot robot) { this.robot=robot; - Updater.add(this,Priority.NORMAL); + Updater.add(robot,Priority.NORMAL); } - - - //STUFF WE OVERRIDE IN THIS CLASS - @Override - public void startCompetition() { - super.startCompetition(); - } - @Override - public void robotInit() - { - init(); - } - @Override - public final void autonomousInit() { - sprocketStart(Mode.AUTO); - } - - @Override - public final void teleopInit() { - sprocketStart(Mode.TELEOP); - } - - @Override - public final void testInit() { - sprocketStart(Mode.TEST); - } - - @Override - public final void disabledInit() { - sprocketStop(); - } - - @Override - public final void autonomousPeriodic() { - sprocketUpdate(); - } - - @Override - public final void teleopPeriodic() { - sprocketUpdate(); - } - - @Override - public final void testPeriodic() { - sprocketUpdate(); - } - - @Override - public final void disabledPeriodic() { - sprocketDisabled(); - } - - //OUR STUFF HERE public static DriveTrain getMainDriveTrain() { return driveTrain; @@ -93,7 +45,7 @@ public static DriveTrain getMainDriveTrain() { public static void setMainDriveTrain(DriveTrain dt) { driveTrain = dt; } - public final void sprocketStart(Mode mode) + public final void start(MODE mode) { switch(mode) { @@ -109,25 +61,29 @@ public final void sprocketStart(Mode mode) if(testActionInput!=null) currentAction=testActionInput.get(); break; + default: + break; } + curMode=mode; if(currentAction!=null) { currentAction.start(); } start(mode); } - public final void sprocketUpdate() + public final void update() { Updater.loop(); } - public final void sprocketStop() + public final void stop() { if(currentAction!=null) { currentAction.stop(); } + curMode=MODE.DISABLED; } - public final void sprocketDisabled() + public final void disabledUpdate() { DisabledUpdater.loop(); } diff --git a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java index 4eb8fe9..d153f4e 100644 --- a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java +++ b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.drive; import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.core.Motor; +import org.montclairrobotics.sprocket.core.IMotor; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; @@ -19,7 +19,7 @@ */ public class ControlledMotor implements Updatable,Action { - private Motor output; + private IMotor output; private Input input; private boolean enabled=true; @@ -29,7 +29,7 @@ public class ControlledMotor implements Updatable,Action { * @param motor the motor which should be controlled * @param input the input to read from */ - public ControlledMotor(Motor motor, Input input) { + public ControlledMotor(IMotor motor, Input input) { this.output=motor; this.input = input; Updater.add(this, Priority.OUTPUT); @@ -44,7 +44,7 @@ public ControlledMotor(Motor motor, Input input) { * @param reverseInput this input is true if the motor should go in reverse. Can be null. * @param speed the speed at which to run the motor (whether forwards or backwards) */ - public ControlledMotor(Motor motor, Input forwardInput, Input reverseInput, double speed) { + public ControlledMotor(IMotor motor, Input forwardInput, Input reverseInput, double speed) { this.output=motor; this.input = new ButtonPairInput(forwardInput, reverseInput, speed); Updater.add(this, Priority.OUTPUT); @@ -58,7 +58,7 @@ public ControlledMotor(Motor motor, Input forwardInput, Input * @param forwardInput this input is true if the motor should go forwards. Can be null. * @param reverseInput this input is true if the motor should go in reverse. Can be null. */ - public ControlledMotor(Motor motor, Input forwardInput, Input reverseInput) { + public ControlledMotor(IMotor motor, Input forwardInput, Input reverseInput) { this(motor, forwardInput, reverseInput, 1.0); } diff --git a/src/org/montclairrobotics/sprocket/drive/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index 3970bde..e104218 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.drive; -import org.montclairrobotics.sprocket.core.Motor; +import org.montclairrobotics.sprocket.core.IMotor; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.Polar; @@ -23,7 +23,7 @@ public class DriveModule extends Module{ private Vector offset; private Vector force; - private Motor[] motors; + private IMotor[] motors; private double power; @@ -39,7 +39,7 @@ public DriveModule(Vector offset, SEncoder enc, PID pid, Module.MotorInputType inputType, - Motor... motors + IMotor... motors ) { super(enc, pid, inputType, motors); this.offset = offset; @@ -47,7 +47,7 @@ public DriveModule(Vector offset, this.force = force; } - public DriveModule(Vector offset, Vector force,Motor... motors) { + public DriveModule(Vector offset, Vector force,IMotor... motors) { this(offset,force,null,null,Module.MotorInputType.PERCENT,motors); } @@ -56,12 +56,12 @@ public DriveModule(Vector offset, SEncoder enc, PID pid, Module.MotorInputType inputType, - Motor... motors + IMotor... motors ) { this(offset,new Polar(1,force),enc,pid,inputType,motors); } - public DriveModule(Vector offset, Angle force, Motor... motors) { + public DriveModule(Vector offset, Angle force, IMotor... motors) { this(offset,new Polar(1,force),null,null,Module.MotorInputType.PERCENT,motors); } diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java index 78ae716..1096c69 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java @@ -3,8 +3,8 @@ import java.util.ArrayList; import org.montclairrobotics.sprocket.control.ArcadeDriveInput; -import org.montclairrobotics.sprocket.core.Joystick; -import org.montclairrobotics.sprocket.frc.FRCMotor; +import org.montclairrobotics.sprocket.core.IJoystick; +import org.montclairrobotics.sprocket.frc.Motor; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Polar; import org.montclairrobotics.sprocket.geometry.Vector; @@ -45,7 +45,7 @@ public DriveTrainBuilder addDriveModule(DriveModule module) { } public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle force, SEncoder enc, PID pid) { - modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, new FRCMotor(motor))); + modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, new Motor(motor))); return this; } @@ -54,7 +54,7 @@ public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle fo } - public DriveTrainBuilder addWheels(Vector offset, Angle force, FRCMotor... motors) { + public DriveTrainBuilder addWheels(Vector offset, Angle force, Motor... motors) { modules.add(new DriveModule(offset, new Polar(1, force), motors)); return this; } @@ -99,7 +99,7 @@ public DriveTrainBuilder addSteps(Step... steps) { return this; } - public DriveTrainBuilder setArcadeDriveInput(Joystick stick) { + public DriveTrainBuilder setArcadeDriveInput(IJoystick stick) { return setInput(new ArcadeDriveInput(stick)); } diff --git a/src/org/montclairrobotics/sprocket/frc/FRCButton.java b/src/org/montclairrobotics/sprocket/frc/Button.java similarity index 84% rename from src/org/montclairrobotics/sprocket/frc/FRCButton.java rename to src/org/montclairrobotics/sprocket/frc/Button.java index 7f09cad..c2e12a8 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCButton.java +++ b/src/org/montclairrobotics/sprocket/frc/Button.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.Joystick; -public class FRCButton implements Input{ +public class Button implements Input{ private Joystick stick; private int id; @@ -15,7 +15,7 @@ public class FRCButton implements Input{ * @param buttonId The raw button ID of the button you're binding to. On most * Joysticks the ID is specified on the buttons themselves. */ - public FRCButton(Joystick stick, int buttonId) { + public Button(Joystick stick, int buttonId) { this.stick = stick; this.id = buttonId; } @@ -26,7 +26,7 @@ public FRCButton(Joystick stick, int buttonId) { * @param buttonId The raw button ID of the button you're binding to. On most * Joysticks the ID is specified on the buttons themselves. */ - public FRCButton(int stick, int buttonId) { + public Button(int stick, int buttonId) { this(new Joystick(stick), buttonId); } @Override diff --git a/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java index 22e338f..3978c20 100644 --- a/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java +++ b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java @@ -1,10 +1,10 @@ package org.montclairrobotics.sprocket.frc; -import org.montclairrobotics.sprocket.core.Debugger; +import org.montclairrobotics.sprocket.core.IDebugger; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class DashboardDebug implements Debugger{ +public class DashboardDebug implements IDebugger{ @Override public void debugStr(String key, String val) { diff --git a/src/org/montclairrobotics/sprocket/frc/DashboardSelector.java b/src/org/montclairrobotics/sprocket/frc/DashboardSelector.java index 7fd1088..079abb8 100644 --- a/src/org/montclairrobotics/sprocket/frc/DashboardSelector.java +++ b/src/org/montclairrobotics/sprocket/frc/DashboardSelector.java @@ -1,9 +1,9 @@ package org.montclairrobotics.sprocket.frc; import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.core.AutoSelector; +import org.montclairrobotics.sprocket.core.IAutoSelector; -public class DashboardSelector implements AutoSelector{ +public class DashboardSelector implements IAutoSelector{ @Override public void addAutoMode(AutoMode mode) { diff --git a/src/org/montclairrobotics/sprocket/frc/FRCEncoder.java b/src/org/montclairrobotics/sprocket/frc/FRCEncoder.java index 1a0d118..fb2f966 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCEncoder.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCEncoder.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.frc; -public class FRCEncoder extends edu.wpi.first.wpilibj.Encoder implements org.montclairrobotics.sprocket.core.Encoder{ +public class FRCEncoder extends edu.wpi.first.wpilibj.Encoder implements org.montclairrobotics.sprocket.core.IEncoder{ public FRCEncoder(int channelA, int channelB) { super(channelA, channelB); diff --git a/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java b/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java index 59a41d3..b73c7a6 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.frc; -public class FRCJoystick extends edu.wpi.first.wpilibj.Joystick implements org.montclairrobotics.sprocket.core.Joystick{ +public class FRCJoystick extends edu.wpi.first.wpilibj.Joystick implements org.montclairrobotics.sprocket.core.IJoystick{ public FRCJoystick(int port) { super(port); } diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java index 61bd414..af47c3b 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -1,8 +1,9 @@ -package org.montclairrobotics.sprocket.core; +package org.montclairrobotics.sprocket.frc; import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.IRobot; import org.montclairrobotics.sprocket.core.Sprocket; +import org.montclairrobotics.sprocket.core.Sprocket.MODE; import org.montclairrobotics.sprocket.drive.DriveTrain; import org.montclairrobotics.sprocket.loop.DisabledUpdater; import org.montclairrobotics.sprocket.loop.Priority; @@ -18,7 +19,7 @@ * This class is basically just a wrapper around iterative robot which all Sprocket * robots must extend. */ -public abstract class FRCRobot extends IterativeRobot implements IRobot{ +public abstract class FRCRobot extends IterativeRobot implements IRobot,Updatable{ public Sprocket sprocket; @@ -27,7 +28,6 @@ public FRCRobot() sprocket=new Sprocket(this); } - //STUFF WE OVERRIDE IN THIS CLASS @Override public void startCompetition() { super.startCompetition(); @@ -39,89 +39,41 @@ public void robotInit() } @Override public final void autonomousInit() { - sprocket.sprocketStart(Mode.AUTO); + sprocket.start(MODE.AUTO); } @Override public final void teleopInit() { - sprocketStart(Mode.TELEOP); + sprocket.start(MODE.TELEOP); } @Override public final void testInit() { - sprocketStart(Mode.TEST); + sprocket.start(MODE.TEST); } @Override public final void disabledInit() { - sprocketStop(); + sprocket.stop(); } @Override public final void autonomousPeriodic() { - sprocketUpdate(); + sprocket.update(); } @Override public final void teleopPeriodic() { - sprocketUpdate(); + sprocket.update(); } @Override public final void testPeriodic() { - sprocketUpdate(); + sprocket.update(); } @Override public final void disabledPeriodic() { - sprocketDisabled(); - } - - - //OUR STUFF HERE - public static DriveTrain getMainDriveTrain() { - return driveTrain; - } - - public static void setMainDriveTrain(DriveTrain dt) { - driveTrain = dt; - } - public final void sprocketStart(Mode mode) - { - switch(mode) - { - case AUTO: - if(autoActionInput!=null) - currentAction=autoActionInput.get(); - break; - case TELEOP: - if(teleopActionInput!=null) - currentAction=teleopActionInput.get(); - break; - case TEST: - if(testActionInput!=null) - currentAction=testActionInput.get(); - break; - } - if(currentAction!=null) - { - currentAction.start(); - } - start(mode); - } - public final void sprocketUpdate() - { - Updater.loop(); - } - public final void sprocketStop() - { - if(currentAction!=null) - { - currentAction.stop(); - } - } - public final void sprocketDisabled() - { - DisabledUpdater.loop(); + sprocket.disabledUpdate(); } } diff --git a/src/org/montclairrobotics/sprocket/frc/FRCMotor.java b/src/org/montclairrobotics/sprocket/frc/Motor.java similarity index 90% rename from src/org/montclairrobotics/sprocket/frc/FRCMotor.java rename to src/org/montclairrobotics/sprocket/frc/Motor.java index f7c8164..84ffbcf 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCMotor.java +++ b/src/org/montclairrobotics/sprocket/frc/Motor.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.frc; -import org.montclairrobotics.sprocket.core.Motor; +import org.montclairrobotics.sprocket.core.IMotor; import org.montclairrobotics.sprocket.utils.Utils; import com.ctre.CANTalon; @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Talon; -public class FRCMotor implements Motor{ +public class Motor implements IMotor{ public enum MotorType { CANTALON, @@ -26,7 +26,7 @@ public enum MotorType { private boolean brakeMode=true; - public FRCMotor(SpeedController motor) { + public Motor(SpeedController motor) { if(motor == null) { throw new IllegalArgumentException("SpeedController argument was null when instantiating Motor object"); } @@ -83,7 +83,7 @@ public MotorType getMotorType() { return motorType; } - public Motor constrain(double min, double max) { + public IMotor constrain(double min, double max) { this.minPower = min; this.maxPower = max; return this; diff --git a/src/org/montclairrobotics/sprocket/motors/Module.java b/src/org/montclairrobotics/sprocket/motors/Module.java index ed8a31c..5b5f248 100644 --- a/src/org/montclairrobotics/sprocket/motors/Module.java +++ b/src/org/montclairrobotics/sprocket/motors/Module.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.motors; -import org.montclairrobotics.sprocket.core.Motor; +import org.montclairrobotics.sprocket.core.IMotor; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.PID; @@ -20,7 +20,7 @@ public class Module { private static int id = 0; public enum MotorInputType {PERCENT, SPEED}; - private Motor[] motors; + private IMotor[] motors; private double power; @@ -37,7 +37,7 @@ public enum MotorInputType {PERCENT, SPEED}; * @param force The vector on which the module applies force * @param motors All the motors which are a part of this module */ - public Module(SEncoder enc, PID pid, MotorInputType inputType,Motor... motors) { + public Module(SEncoder enc, PID pid, MotorInputType inputType,IMotor... motors) { this.motors = motors; this.inputType = inputType; @@ -62,7 +62,7 @@ public Module(SEncoder enc, PID pid, MotorInputType inputType,Motor... motors) { id++; } - public Module(Motor... motors) + public Module(IMotor... motors) { this(null,null,MotorInputType.PERCENT,motors); } @@ -81,7 +81,7 @@ public void set(double val) pid.setTarget(power); power=(power+pid.get())/maxSpeed.get(); } - for(Motor motor:motors) + for(IMotor motor:motors) { motor.set(power); } diff --git a/src/org/montclairrobotics/sprocket/motors/SEncoder.java b/src/org/montclairrobotics/sprocket/motors/SEncoder.java index 6f92ecc..26681b8 100644 --- a/src/org/montclairrobotics/sprocket/motors/SEncoder.java +++ b/src/org/montclairrobotics/sprocket/motors/SEncoder.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.motors; -import org.montclairrobotics.sprocket.core.Encoder; +import org.montclairrobotics.sprocket.core.IEncoder; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; @@ -9,7 +9,7 @@ public class SEncoder implements Input { - private Encoder enc; + private IEncoder enc; private int eId; private double ticksPerInch; @@ -25,7 +25,7 @@ public SEncoder(int a, int b, double ticksPerInch) { this(a, b, ticksPerInch, false); }*/ - public SEncoder(Encoder e, double ticksPerInch) { + public SEncoder(IEncoder e, double ticksPerInch) { enc = e; this.ticksPerInch = ticksPerInch; } @@ -53,7 +53,7 @@ public void reset() { enc.reset(); } - public Encoder getRawEncoder() { + public IEncoder getRawEncoder() { return enc; } diff --git a/src/org/montclairrobotics/sprocket/utils/Debug.java b/src/org/montclairrobotics/sprocket/utils/Debug.java index 6bed353..1eeec0d 100644 --- a/src/org/montclairrobotics/sprocket/utils/Debug.java +++ b/src/org/montclairrobotics/sprocket/utils/Debug.java @@ -1,6 +1,7 @@ package org.montclairrobotics.sprocket.utils; -import org.montclairrobotics.sprocket.core.Debugger; +import org.montclairrobotics.sprocket.core.IDebugger; +import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.frc.DashboardDebug; @@ -8,18 +9,16 @@ public class Debug { public static boolean DEBUG_MODE=true; - public static Debugger debugger=new DashboardDebug(); - public static void num(String key,double value) { if(DEBUG_MODE) - debugger.debugNum(key, value); + Sprocket.debugger.debugNum(key, value); } public static void string(String key,String value) { if(DEBUG_MODE) - debugger.debugStr(key, value); + Sprocket.debugger.debugStr(key, value); } public static void msg(String key,double value) diff --git a/src/org/montclairrobotics/sprocket/utils/PID.java b/src/org/montclairrobotics/sprocket/utils/PID.java index 1a246f9..9847fa9 100644 --- a/src/org/montclairrobotics/sprocket/utils/PID.java +++ b/src/org/montclairrobotics/sprocket/utils/PID.java @@ -15,7 +15,7 @@ * */ -public class PID implements Updatable { +public class PID implements Updatable,Input{ private Input input; private double P,I,D; @@ -127,7 +127,7 @@ public PID setTarget(double t,boolean reset) * Get the output value * @return the output */ - public double get() + public Double get() { //out = calculate(input.get()); return out; From 78b72694154496ffc63dcc172cd858c913a555b8 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 21:55:15 -0400 Subject: [PATCH 25/71] Imports! --- src/org/montclairrobotics/sprocket/core/Sprocket.java | 3 --- src/org/montclairrobotics/sprocket/frc/FRCRobot.java | 6 ------ src/org/montclairrobotics/sprocket/frc/Motor.java | 2 -- src/org/montclairrobotics/sprocket/utils/Debug.java | 2 -- 4 files changed, 13 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index 8048557..8e89548 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -5,12 +5,9 @@ import org.montclairrobotics.sprocket.frc.DashboardDebug; import org.montclairrobotics.sprocket.loop.DisabledUpdater; import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.utils.Input; -import edu.wpi.first.wpilibj.IterativeRobot; - /** * @author MHS Robotics diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java index af47c3b..2794a8b 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -1,15 +1,9 @@ package org.montclairrobotics.sprocket.frc; -import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.IRobot; import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.core.Sprocket.MODE; -import org.montclairrobotics.sprocket.drive.DriveTrain; -import org.montclairrobotics.sprocket.loop.DisabledUpdater; -import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; -import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.IterativeRobot; diff --git a/src/org/montclairrobotics/sprocket/frc/Motor.java b/src/org/montclairrobotics/sprocket/frc/Motor.java index 84ffbcf..7370a09 100644 --- a/src/org/montclairrobotics/sprocket/frc/Motor.java +++ b/src/org/montclairrobotics/sprocket/frc/Motor.java @@ -15,8 +15,6 @@ public enum MotorType { TALON, UNKNOWN } - - private SpeedController motor; private MotorType motorType; diff --git a/src/org/montclairrobotics/sprocket/utils/Debug.java b/src/org/montclairrobotics/sprocket/utils/Debug.java index 1eeec0d..efd258f 100644 --- a/src/org/montclairrobotics/sprocket/utils/Debug.java +++ b/src/org/montclairrobotics/sprocket/utils/Debug.java @@ -1,8 +1,6 @@ package org.montclairrobotics.sprocket.utils; -import org.montclairrobotics.sprocket.core.IDebugger; import org.montclairrobotics.sprocket.core.Sprocket; -import org.montclairrobotics.sprocket.frc.DashboardDebug; public class Debug { From 101f794f18f8f6e606afd547091974cee1a5c9dd Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 22:00:14 -0400 Subject: [PATCH 26/71] We dont even have to do this stupid thing anymore! --- src/org/montclairrobotics/sprocket/core/Sprocket.java | 9 --------- src/org/montclairrobotics/sprocket/frc/FRCRobot.java | 5 ++++- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index 8e89548..c86700f 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -4,7 +4,6 @@ import org.montclairrobotics.sprocket.drive.DriveTrain; import org.montclairrobotics.sprocket.frc.DashboardDebug; import org.montclairrobotics.sprocket.loop.DisabledUpdater; -import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.utils.Input; @@ -18,8 +17,6 @@ public class Sprocket{ public static DriveTrain driveTrain; public static IDebugger debugger=new DashboardDebug(); - - private IRobot robot; public enum MODE {AUTO,TELEOP,TEST,DISABLED}; public MODE curMode; @@ -29,12 +26,6 @@ public enum MODE {AUTO,TELEOP,TEST,DISABLED}; testActionInput; private Action currentAction; - public Sprocket(IRobot robot) - { - this.robot=robot; - Updater.add(robot,Priority.NORMAL); - } - //OUR STUFF HERE public static DriveTrain getMainDriveTrain() { return driveTrain; } diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java index 2794a8b..f814f6c 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -3,7 +3,9 @@ import org.montclairrobotics.sprocket.core.IRobot; import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.core.Sprocket.MODE; +import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; import edu.wpi.first.wpilibj.IterativeRobot; @@ -19,7 +21,8 @@ public abstract class FRCRobot extends IterativeRobot implements IRobot,Updatabl public FRCRobot() { - sprocket=new Sprocket(this); + sprocket=new Sprocket(); + Updater.add(this,Priority.NORMAL); } @Override From 455eb2c12dce09649bd1b6fab5c2067c346ef2fd Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 22:01:09 -0400 Subject: [PATCH 27/71] Revert "We dont even have to do this stupid thing anymore!" This reverts commit 101f794f18f8f6e606afd547091974cee1a5c9dd. --- src/org/montclairrobotics/sprocket/core/Sprocket.java | 9 +++++++++ src/org/montclairrobotics/sprocket/frc/FRCRobot.java | 5 +---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index c86700f..8e89548 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -4,6 +4,7 @@ import org.montclairrobotics.sprocket.drive.DriveTrain; import org.montclairrobotics.sprocket.frc.DashboardDebug; import org.montclairrobotics.sprocket.loop.DisabledUpdater; +import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.utils.Input; @@ -17,6 +18,8 @@ public class Sprocket{ public static DriveTrain driveTrain; public static IDebugger debugger=new DashboardDebug(); + + private IRobot robot; public enum MODE {AUTO,TELEOP,TEST,DISABLED}; public MODE curMode; @@ -26,6 +29,12 @@ public enum MODE {AUTO,TELEOP,TEST,DISABLED}; testActionInput; private Action currentAction; + public Sprocket(IRobot robot) + { + this.robot=robot; + Updater.add(robot,Priority.NORMAL); + } + //OUR STUFF HERE public static DriveTrain getMainDriveTrain() { return driveTrain; } diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java index f814f6c..2794a8b 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -3,9 +3,7 @@ import org.montclairrobotics.sprocket.core.IRobot; import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.core.Sprocket.MODE; -import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; import edu.wpi.first.wpilibj.IterativeRobot; @@ -21,8 +19,7 @@ public abstract class FRCRobot extends IterativeRobot implements IRobot,Updatabl public FRCRobot() { - sprocket=new Sprocket(); - Updater.add(this,Priority.NORMAL); + sprocket=new Sprocket(this); } @Override From b507854a3f9d5a3965dbe7b292ecea0773e16843 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 22:26:27 -0400 Subject: [PATCH 28/71] Now we use it. --- .../sprocket/core/IRobot.java | 2 ++ .../sprocket/core/Sprocket.java | 19 +++++++++++++------ .../sprocket/frc/FRCRobot.java | 18 +++++++++--------- 3 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/core/IRobot.java b/src/org/montclairrobotics/sprocket/core/IRobot.java index 6dd6921..c6ed55f 100644 --- a/src/org/montclairrobotics/sprocket/core/IRobot.java +++ b/src/org/montclairrobotics/sprocket/core/IRobot.java @@ -9,4 +9,6 @@ public default void init(){} public default void start(Sprocket.MODE mode){} public default void stop(){} public default void update(){} + public default void disabledUpdate(){} + public default void debugs(){} } diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index 8e89548..d3a1d66 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -38,11 +38,15 @@ public Sprocket(IRobot robot) public static DriveTrain getMainDriveTrain() { return driveTrain; } - public static void setMainDriveTrain(DriveTrain dt) { driveTrain = dt; } - public final void start(MODE mode) + + public void initS() + { + robot.init(); + } + public final void startS(MODE mode) { switch(mode) { @@ -66,22 +70,25 @@ public final void start(MODE mode) { currentAction.start(); } - start(mode); + robot.start(mode); } - public final void update() + public final void updateS() { Updater.loop(); + robot.debugs(); } - public final void stop() + public final void stopS() { if(currentAction!=null) { currentAction.stop(); } curMode=MODE.DISABLED; + robot.stop(); } - public final void disabledUpdate() + public final void disabledUpdateS() { DisabledUpdater.loop(); + robot.disabledUpdate(); } } diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java index 2794a8b..d7b92fb 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -29,45 +29,45 @@ public void startCompetition() { @Override public void robotInit() { - init(); + sprocket.initS(); } @Override public final void autonomousInit() { - sprocket.start(MODE.AUTO); + sprocket.startS(MODE.AUTO); } @Override public final void teleopInit() { - sprocket.start(MODE.TELEOP); + sprocket.startS(MODE.TELEOP); } @Override public final void testInit() { - sprocket.start(MODE.TEST); + sprocket.startS(MODE.TEST); } @Override public final void disabledInit() { - sprocket.stop(); + sprocket.stopS(); } @Override public final void autonomousPeriodic() { - sprocket.update(); + sprocket.updateS(); } @Override public final void teleopPeriodic() { - sprocket.update(); + sprocket.updateS(); } @Override public final void testPeriodic() { - sprocket.update(); + sprocket.updateS(); } @Override public final void disabledPeriodic() { - sprocket.disabledUpdate(); + sprocket.disabledUpdateS(); } } From 181a483879327b06970ed51991bd296313777d79 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 22:28:11 -0400 Subject: [PATCH 29/71] And consolidating some classes to make less things on the left --- .../sprocket/auto/AutoDTInput.java | 31 ------------------- .../sprocket/auto/AutoState.java | 15 ++++++++- 2 files changed, 14 insertions(+), 32 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/auto/AutoDTInput.java diff --git a/src/org/montclairrobotics/sprocket/auto/AutoDTInput.java b/src/org/montclairrobotics/sprocket/auto/AutoDTInput.java deleted file mode 100644 index 48a17d3..0000000 --- a/src/org/montclairrobotics/sprocket/auto/AutoDTInput.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.montclairrobotics.sprocket.auto; - -import org.montclairrobotics.sprocket.drive.DTInput; -import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Vector; - -/** - * The bridge class which sends autonomous drive train commands to the drivetrain - * from an AutoMode - */ -public class AutoDTInput implements DTInput { - public Vector tgtDir = Vector.ZERO; - public Angle tgtTurn = Angle.ZERO; - //public DTInput.Type inputType = DTInput.Type.PERCENT; - - @Override - public Vector getDir() { - return tgtDir; - } - - @Override - public Angle getTurn() { - return tgtTurn; - } - - /*@Override - public Type getInputType() { - return inputType; - } -*/ -} diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index fe987fe..41bf925 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -2,6 +2,7 @@ import org.montclairrobotics.sprocket.actions.State; import org.montclairrobotics.sprocket.core.Sprocket; +import org.montclairrobotics.sprocket.drive.DTInput; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Updater; @@ -13,7 +14,9 @@ * Sprocket comes with some basic AutoStates built in, but you will need to make * at least some of them on your own by extending this class. */ -public abstract class AutoState extends AutoDTInput implements State { +public abstract class AutoState implements State,DTInput { + public Vector tgtDir = Vector.ZERO; + public Angle tgtTurn = Angle.ZERO; private double t;//time state was started public final void start() @@ -59,4 +62,14 @@ public void setTarget(Vector v, Angle a) { tgtDir = v; tgtTurn = a; } + + @Override + public Vector getDir() { + return tgtDir; + } + + @Override + public Angle getTurn() { + return tgtTurn; + } } From b21768d6c213bf073445f2ea1aea2ad93249b5f9 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 22:40:44 -0400 Subject: [PATCH 30/71] Here is a change I am less sure about the implementation, but am very sure that we need the functionality. --- .../montclairrobotics/sprocket/auto/AutoState.java | 14 ++++++++++++++ .../sprocket/auto/states/Delay.java | 5 +++-- .../sprocket/auto/states/DriveEncoders.java | 2 +- .../sprocket/auto/states/TurnEncoders.java | 2 +- .../sprocket/auto/states/TurnGyro.java | 2 +- 5 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index 41bf925..0f17343 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -18,6 +18,7 @@ public abstract class AutoState implements State,DTInput { public Vector tgtDir = Vector.ZERO; public Angle tgtTurn = Angle.ZERO; private double t;//time state was started + public double TIMEOUT=30; public final void start() { @@ -72,4 +73,17 @@ public Vector getDir() { public Angle getTurn() { return tgtTurn; } + + public abstract boolean userIsDone(); + + @Override + public final boolean isDone() + { + return timeInState() timeInput) { public void userStart() { super.userStart(); if(timeInput != null) time = timeInput.get(); + super.TIMEOUT=time; } @Override @@ -33,10 +34,10 @@ public void enabled() { } @Override - public boolean isDone() { + public boolean userIsDone() { Debug.num("Time", time); Debug.num("Time in State", timeInState()); - return timeInState()>time; + return false; } } diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index aefffd5..0e0163e 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -81,7 +81,7 @@ public void enabled() { } @Override - public boolean isDone() { + public boolean userIsDone() { Debug.msg("forwards", forwards?"YES":"NO"); Debug.msg("DISTANCE", dt.getDistance().getY()); Debug.msg("StopDistance", stopDist.get()); diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java index ada5cb3..73b96c8 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java @@ -35,7 +35,7 @@ public void enabled() { } @Override - public boolean isDone() { + public boolean userIsDone() { return Math.abs(module.getEnc().getDistance().get() - finalPos) < 1.0; } diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java index e7d6cf9..1fbb80c 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java @@ -80,7 +80,7 @@ public void userStop() } @Override - public boolean isDone() { + public boolean userIsDone() { return (Updater.getTime()-incorrectTime>timeAtTarget); } From 9d5375aa610465c2276ab9b4894d690f3d0f5519 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Jun 2017 22:48:07 -0400 Subject: [PATCH 31/71] AAAAAAAAAAAnd here is another class to make up for the one I removed. --- .../sprocket/control/JoystickXAxis.java | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/org/montclairrobotics/sprocket/control/JoystickXAxis.java diff --git a/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java b/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java new file mode 100644 index 0000000..d63267b --- /dev/null +++ b/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java @@ -0,0 +1,32 @@ +package org.montclairrobotics.sprocket.control; + +import org.montclairrobotics.sprocket.core.IJoystick; +import org.montclairrobotics.sprocket.utils.Input; + + +/** + * This class is a pretty simple wrapper around the Joystick Y-axis which just + * makes it into an Input{@literal <}Double{@literal >} for use with Sprocket + * classes such as ControlledMotor. + */ +public class JoystickXAxis implements Input { + + private IJoystick stick; + + + /** + * @param stick The Joystick + */ + public JoystickXAxis(IJoystick stick) { + this.stick = stick; + } + + /** + * @return The Joystick's Y-axis + */ + @Override + public Double get() { + return stick.getX(); + } + +} From db02ecbfe7ee826a7fef0a2fbf083e1ca63fb6d1 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 6 Sep 2017 10:30:22 -0400 Subject: [PATCH 32/71] Maybe this will help? Git isnt working --- .../sprocket/utils/SprocketRobot.java | 142 ++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100644 src/org/montclairrobotics/sprocket/utils/SprocketRobot.java diff --git a/src/org/montclairrobotics/sprocket/utils/SprocketRobot.java b/src/org/montclairrobotics/sprocket/utils/SprocketRobot.java new file mode 100644 index 0000000..08f4d10 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/utils/SprocketRobot.java @@ -0,0 +1,142 @@ +package org.montclairrobotics.sprocket; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.auto.AutoState; +import org.montclairrobotics.sprocket.drive.DriveTrain; +import org.montclairrobotics.sprocket.loop.Priority; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +/** + * @author MHS Robotics + * This class is basically just a wrapper around iterative robot which all Sprocket + * robots must extend. + */ +public abstract class SprocketRobot extends IterativeRobot implements Updatable{ + + private SendableChooser chooser; + private AutoMode[] autoModes; + private AutoMode selectedAutoMode; + private AutoState runState; + + public SprocketRobot() + { + Updater.add(this,Priority.NORMAL); + autoModes=new AutoMode[0]; + } + + private static DriveTrain driveTrain; + + public static DriveTrain getDriveTrain() { + return driveTrain; + } + + public static void setDriveTrain(DriveTrain dt) { + driveTrain = dt; + } + + + @Override + public void startCompetition() { + super.startCompetition(); + } + + @Override + public void robotInit(){} + public void userStart(){} + public void reset(){} + public void userTeleopInit(){} + public void userAutonomousInit(){} + public void userTestInit(){} + public void update(){} + + @Override + public final void disabledInit() { + if(selectedAutoMode!=null) + { + selectedAutoMode.stop(); + } + } + + @Override + public final void autonomousInit() { + selectedAutoMode = chooser.getSelected(); + selectedAutoMode.start(); + start(); + userAutonomousInit(); + } + + @Override + public final void teleopInit() { + start(); + userTeleopInit(); + } + + @Override + public final void testInit() { + start(); + userTestInit(); + } + + public final void start() + { + userStart(); + reset(); + sendAutoModes(); + } + public final void sprocketUpdate() + { + Updater.loop();//looooooooooooooooop + } + + @Override + public final void disabledPeriodic() { + super.disabledPeriodic(); + SmartDashboard.putData("AUTO:",chooser); + } + + @Override + public final void autonomousPeriodic() { + sprocketUpdate(); + } + + @Override + public final void teleopPeriodic() { + sprocketUpdate(); + } + + @Override + public final void testPeriodic() { + sprocketUpdate(); + } + + public void setAutoModes(AutoMode... modes) + { + this.autoModes=modes; + sendAutoModes(); + } + + public void addAutoMode(AutoMode mode) { + ArrayList modes = new ArrayList(Arrays.asList(autoModes)); + modes.add(mode); + autoModes = modes.toArray(autoModes); + } + + public void sendAutoModes() + { + chooser=new SendableChooser(); + for(AutoMode mode:autoModes) + { + chooser.addObject(mode+"", mode); + } + } +} From 1f02d6a26cee2a68de46e871fdae462093f1a8b7 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 6 Sep 2017 17:26:09 -0400 Subject: [PATCH 33/71] Buttons in FRC so that we can add FTC buttons --- .../sprocket/actions/SplitButton.java | 1 + .../sprocket/{actions => core}/Button.java | 4 +- .../sprocket/core/IButton.java | 7 - .../frc/{Button.java => FRCButton.java} | 7 +- .../sprocket/utils/PIDTuner.java | 2 +- .../sprocket/utils/SprocketRobot.java | 142 ------------------ 6 files changed, 9 insertions(+), 154 deletions(-) rename src/org/montclairrobotics/sprocket/{actions => core}/Button.java (91%) delete mode 100644 src/org/montclairrobotics/sprocket/core/IButton.java rename src/org/montclairrobotics/sprocket/frc/{Button.java => FRCButton.java} (81%) delete mode 100644 src/org/montclairrobotics/sprocket/utils/SprocketRobot.java diff --git a/src/org/montclairrobotics/sprocket/actions/SplitButton.java b/src/org/montclairrobotics/sprocket/actions/SplitButton.java index 04b5dbb..dd74c58 100644 --- a/src/org/montclairrobotics/sprocket/actions/SplitButton.java +++ b/src/org/montclairrobotics/sprocket/actions/SplitButton.java @@ -1,5 +1,6 @@ package org.montclairrobotics.sprocket.actions; +import org.montclairrobotics.sprocket.core.Button; import org.montclairrobotics.sprocket.utils.Input; public class SplitButton extends Button{ diff --git a/src/org/montclairrobotics/sprocket/actions/Button.java b/src/org/montclairrobotics/sprocket/core/Button.java similarity index 91% rename from src/org/montclairrobotics/sprocket/actions/Button.java rename to src/org/montclairrobotics/sprocket/core/Button.java index 5045e93..a8a8a76 100644 --- a/src/org/montclairrobotics/sprocket/actions/Button.java +++ b/src/org/montclairrobotics/sprocket/core/Button.java @@ -1,5 +1,7 @@ -package org.montclairrobotics.sprocket.actions; +package org.montclairrobotics.sprocket.core; +import org.montclairrobotics.sprocket.actions.Action; +import org.montclairrobotics.sprocket.actions.Togglable; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; diff --git a/src/org/montclairrobotics/sprocket/core/IButton.java b/src/org/montclairrobotics/sprocket/core/IButton.java deleted file mode 100644 index 0cf969f..0000000 --- a/src/org/montclairrobotics/sprocket/core/IButton.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.montclairrobotics.sprocket.core; - -import org.montclairrobotics.sprocket.utils.Input; - -public interface IButton extends Input{ - -} diff --git a/src/org/montclairrobotics/sprocket/frc/Button.java b/src/org/montclairrobotics/sprocket/frc/FRCButton.java similarity index 81% rename from src/org/montclairrobotics/sprocket/frc/Button.java rename to src/org/montclairrobotics/sprocket/frc/FRCButton.java index c2e12a8..c4f81b4 100644 --- a/src/org/montclairrobotics/sprocket/frc/Button.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCButton.java @@ -1,10 +1,11 @@ package org.montclairrobotics.sprocket.frc; +import org.montclairrobotics.sprocket.core.Button; import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.Joystick; -public class Button implements Input{ +public class FRCButton extends Button{ private Joystick stick; private int id; @@ -15,7 +16,7 @@ public class Button implements Input{ * @param buttonId The raw button ID of the button you're binding to. On most * Joysticks the ID is specified on the buttons themselves. */ - public Button(Joystick stick, int buttonId) { + public FRCButton(Joystick stick, int buttonId) { this.stick = stick; this.id = buttonId; } @@ -26,7 +27,7 @@ public Button(Joystick stick, int buttonId) { * @param buttonId The raw button ID of the button you're binding to. On most * Joysticks the ID is specified on the buttons themselves. */ - public Button(int stick, int buttonId) { + public FRCButton(int stick, int buttonId) { this(new Joystick(stick), buttonId); } @Override diff --git a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java index 961d6d7..3b87857 100644 --- a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java +++ b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.utils; import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.actions.Button; +import org.montclairrobotics.sprocket.core.Button; import org.montclairrobotics.sprocket.frc.DashboardButton; import org.montclairrobotics.sprocket.frc.DashboardInput; diff --git a/src/org/montclairrobotics/sprocket/utils/SprocketRobot.java b/src/org/montclairrobotics/sprocket/utils/SprocketRobot.java deleted file mode 100644 index 08f4d10..0000000 --- a/src/org/montclairrobotics/sprocket/utils/SprocketRobot.java +++ /dev/null @@ -1,142 +0,0 @@ -package org.montclairrobotics.sprocket; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; - -import org.montclairrobotics.sprocket.auto.AutoMode; -import org.montclairrobotics.sprocket.auto.AutoState; -import org.montclairrobotics.sprocket.drive.DriveTrain; -import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; - -import edu.wpi.first.wpilibj.IterativeRobot; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -/** - * @author MHS Robotics - * This class is basically just a wrapper around iterative robot which all Sprocket - * robots must extend. - */ -public abstract class SprocketRobot extends IterativeRobot implements Updatable{ - - private SendableChooser chooser; - private AutoMode[] autoModes; - private AutoMode selectedAutoMode; - private AutoState runState; - - public SprocketRobot() - { - Updater.add(this,Priority.NORMAL); - autoModes=new AutoMode[0]; - } - - private static DriveTrain driveTrain; - - public static DriveTrain getDriveTrain() { - return driveTrain; - } - - public static void setDriveTrain(DriveTrain dt) { - driveTrain = dt; - } - - - @Override - public void startCompetition() { - super.startCompetition(); - } - - @Override - public void robotInit(){} - public void userStart(){} - public void reset(){} - public void userTeleopInit(){} - public void userAutonomousInit(){} - public void userTestInit(){} - public void update(){} - - @Override - public final void disabledInit() { - if(selectedAutoMode!=null) - { - selectedAutoMode.stop(); - } - } - - @Override - public final void autonomousInit() { - selectedAutoMode = chooser.getSelected(); - selectedAutoMode.start(); - start(); - userAutonomousInit(); - } - - @Override - public final void teleopInit() { - start(); - userTeleopInit(); - } - - @Override - public final void testInit() { - start(); - userTestInit(); - } - - public final void start() - { - userStart(); - reset(); - sendAutoModes(); - } - public final void sprocketUpdate() - { - Updater.loop();//looooooooooooooooop - } - - @Override - public final void disabledPeriodic() { - super.disabledPeriodic(); - SmartDashboard.putData("AUTO:",chooser); - } - - @Override - public final void autonomousPeriodic() { - sprocketUpdate(); - } - - @Override - public final void teleopPeriodic() { - sprocketUpdate(); - } - - @Override - public final void testPeriodic() { - sprocketUpdate(); - } - - public void setAutoModes(AutoMode... modes) - { - this.autoModes=modes; - sendAutoModes(); - } - - public void addAutoMode(AutoMode mode) { - ArrayList modes = new ArrayList(Arrays.asList(autoModes)); - modes.add(mode); - autoModes = modes.toArray(autoModes); - } - - public void sendAutoModes() - { - chooser=new SendableChooser(); - for(AutoMode mode:autoModes) - { - chooser.addObject(mode+"", mode); - } - } -} From e2d6c052d56ab19a6ef426daf4f471660220921a Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 6 Sep 2017 18:00:39 -0400 Subject: [PATCH 34/71] FTC port! It 100% works, after no testing and no features even coded. --- .../sprocket/core/Sprocket.java | 2 +- .../sprocket/ftc/FTCMode.java | 59 +++++++++++++++++++ .../sprocket/ftc/FTCRobot.java | 18 ++++++ 3 files changed, 78 insertions(+), 1 deletion(-) create mode 100644 src/org/montclairrobotics/sprocket/ftc/FTCMode.java create mode 100644 src/org/montclairrobotics/sprocket/ftc/FTCRobot.java diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index d3a1d66..f10e83d 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -27,7 +27,7 @@ public enum MODE {AUTO,TELEOP,TEST,DISABLED}; autoActionInput, teleopActionInput, testActionInput; - private Action currentAction; + public Action currentAction; public Sprocket(IRobot robot) { diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java new file mode 100644 index 0000000..d753157 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java @@ -0,0 +1,59 @@ +package org.montclairrobotics.sprocket.ftc; + +import org.montclairrobotics.sprocket.actions.Action; +import org.montclairrobotics.sprocket.core.IRobot; +import org.montclairrobotics.sprocket.core.Sprocket; +import org.montclairrobotics.sprocket.core.Sprocket.MODE; +import org.montclairrobotics.sprocket.loop.Updatable; + +import edu.wpi.first.wpilibj.IterativeRobot; + + +/** + * @author MHS Robotics + * This class is basically just a wrapper around iterative robot which all Sprocket + * robots must extend. + */ +public abstract class FTCMode /*extends OpMode*/{ + + public Sprocket sprocket; + public MODE mode; + public Action action; + + public FTCMode() + { + } + public FTCMode(Sprocket sprocket,Action action) + { + this.sprocket=sprocket; + this.mode=MODE.AUTO; + this.action=action; + } + + //@Override + public void init() { + sprocket.initS(); + } + + //@Override + public void init_loop() { + sprocket.disabledUpdateS(); + } + + //@Override + public void start() { + sprocket.currentAction=action; + sprocket.startS(mode); + } + + //@Override + public void loop() { + sprocket.updateS(); + + } + + //@Override + public void stop() { + sprocket.stopS(); + } +} diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java new file mode 100644 index 0000000..065b29e --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -0,0 +1,18 @@ +package org.montclairrobotics.sprocket.ftc; + +import org.montclairrobotics.sprocket.core.IRobot; +import org.montclairrobotics.sprocket.core.Sprocket; +import org.montclairrobotics.sprocket.core.Sprocket.MODE; + +public class FTCRobot extends FTCMode implements IRobot { + +//public Sprocket sprocket; //Dont need this because FTCMode has it! + + public FTCRobot() + { + super(); + sprocket=new Sprocket(this); + super.sprocket=sprocket;//Doesnt do anything; better to be safe than sorry + super.mode=MODE.TELEOP; + } +} From bfb12d9855f768c8662f6109520916dc587cd6a7 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 6 Sep 2017 18:41:03 -0400 Subject: [PATCH 35/71] Implementing basic features: Motors, Joysticks, in the middle of buttons. This isn't actually that bad, although some of these classes make me very annoyed --- .../sprocket/drive/DriveTrainBuilder.java | 6 +- .../frc/{Motor.java => FRCMotor.java} | 4 +- .../sprocket/ftc/FTCButton.java | 49 +++++++++++++++ .../sprocket/ftc/FTCJoystick.java | 60 +++++++++++++++++++ .../sprocket/ftc/FTCMode.java | 3 + .../sprocket/ftc/FTCMotor.java | 21 +++++++ .../sprocket/ftc/FTCRobot.java | 14 +++++ .../sprocket/ftc/LightSensor.java | 20 +++++++ 8 files changed, 172 insertions(+), 5 deletions(-) rename src/org/montclairrobotics/sprocket/frc/{Motor.java => FRCMotor.java} (95%) create mode 100644 src/org/montclairrobotics/sprocket/ftc/FTCButton.java create mode 100644 src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java create mode 100644 src/org/montclairrobotics/sprocket/ftc/FTCMotor.java create mode 100644 src/org/montclairrobotics/sprocket/ftc/LightSensor.java diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java index 1096c69..54c4f7c 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java @@ -4,7 +4,7 @@ import org.montclairrobotics.sprocket.control.ArcadeDriveInput; import org.montclairrobotics.sprocket.core.IJoystick; -import org.montclairrobotics.sprocket.frc.Motor; +import org.montclairrobotics.sprocket.frc.FRCMotor; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Polar; import org.montclairrobotics.sprocket.geometry.Vector; @@ -45,7 +45,7 @@ public DriveTrainBuilder addDriveModule(DriveModule module) { } public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle force, SEncoder enc, PID pid) { - modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, new Motor(motor))); + modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, new FRCMotor(motor))); return this; } @@ -54,7 +54,7 @@ public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle fo } - public DriveTrainBuilder addWheels(Vector offset, Angle force, Motor... motors) { + public DriveTrainBuilder addWheels(Vector offset, Angle force, FRCMotor... motors) { modules.add(new DriveModule(offset, new Polar(1, force), motors)); return this; } diff --git a/src/org/montclairrobotics/sprocket/frc/Motor.java b/src/org/montclairrobotics/sprocket/frc/FRCMotor.java similarity index 95% rename from src/org/montclairrobotics/sprocket/frc/Motor.java rename to src/org/montclairrobotics/sprocket/frc/FRCMotor.java index 7370a09..af4a567 100644 --- a/src/org/montclairrobotics/sprocket/frc/Motor.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCMotor.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Talon; -public class Motor implements IMotor{ +public class FRCMotor implements IMotor{ public enum MotorType { CANTALON, @@ -24,7 +24,7 @@ public enum MotorType { private boolean brakeMode=true; - public Motor(SpeedController motor) { + public FRCMotor(SpeedController motor) { if(motor == null) { throw new IllegalArgumentException("SpeedController argument was null when instantiating Motor object"); } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java new file mode 100644 index 0000000..00fb4f9 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java @@ -0,0 +1,49 @@ +package org.montclairrobotics.sprocket.ftc; + +import org.montclairrobotics.sprocket.ftc.FTCJoystick.GAMEPAD; +import org.montclairrobotics.sprocket.ftc.FTCJoystick.STICK; +import org.montclairrobotics.sprocket.utils.Input; + +public class FTCButton implements Input{ + public enum GAMEPAD {A,B}; + enum BUTTON { + a, + b, + x, + y, + start, + back, + right_bumper, + left_bumper, + dpad_up, + dpad_down, + dpad_left, + dpad_right, + guide,//middle button, might not work + left_stick_button, + right_stick_button, + left_trigger,//a float + right_trigger;//a float + } + + private Gamepad gamepad; + private BUTTON button; + + public FTCButton(GAMEPAD gamepad,BUTTON button) + { + if(gamepad==GAMEPAD.A) + { + this.gamepad=FTCRobot.gamepad1; + } + else + { + this.gamepad=FTCRobot.gamepad2; + } + this.button=button; + } + public FTCButton(Gamepad gamepad,BUTTON button) + { + this.gamepad=gamepad; + this.button=button; + } +} diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java new file mode 100644 index 0000000..9596ed2 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java @@ -0,0 +1,60 @@ +package org.montclairrobotics.sprocket.ftc; + +import org.montclairrobotics.sprocket.core.IJoystick; + +//This class kills me +public class FTCJoystick implements IJoystick{ + public enum GAMEPAD {A,B}; + public enum STICK {LEFT,RIGHT,DPAD}; + private Gamepad gamepad; + private STICK stick; + + public FTCJoystick(GAMEPAD gamepad,STICK stick) + { + if(gamepad==GAMEPAD.A) + { + this.gamepad=FTCRobot.gamepad1; + } + else + { + this.gamepad=FTCRobot.gamepad2; + } + this.stick=stick; + } + public FTCJoystick(Gamepad gamepad,STICK stick) + { + this.gamepad=gamepad; + this.stick=stick; + } + + @Override + public double getX() { + if(stick==STICK.LEFT) + { + return gamepad.left_stick_x; + } + else if(stick==STICK.RIGHT) + { + return gamepad.right_stick_x; + } + else + { + return gamepad.dpad_right?1:0-gamepad.dpad_left?1:0; + } + } + @Override + public double getY() { + if(stick==STICK.LEFT) + { + return gamepad.left_stick_y; + } + else if(stick==STICK.RIGHT) + { + return gamepad.right_stick_y; + } + else + { + return gamepad.dpad_up?1:0-gamepad.dpad_down?1:0; + } + } +} diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java index d753157..038c4ce 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java @@ -17,6 +17,7 @@ public abstract class FTCMode /*extends OpMode*/{ public Sprocket sprocket; + public FTCRobot robot; public MODE mode; public Action action; @@ -33,6 +34,8 @@ public FTCMode(Sprocket sprocket,Action action) //@Override public void init() { sprocket.initS(); + if(robot!=null) + robot.ftcSetup(hardwareMap,gamepad1,gamepad2,telemetry); } //@Override diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java new file mode 100644 index 0000000..6e86cd4 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java @@ -0,0 +1,21 @@ +package org.montclairrobotics.sprocket.ftc; + +import org.montclairrobotics.sprocket.core.IMotor; + +public class FTCMotor implements IMotor{ + + private DcMotor motor; + + public FTCMotor(String motorID) + { + DcMotor motor=FTCRobot.hardwareMap.dcMotor.get(motorID); + motor.setPower(0); + motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + @Override + public void set(double power) { + motor.setPower(power); + } +} diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index 065b29e..e680864 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -7,12 +7,26 @@ public class FTCRobot extends FTCMode implements IRobot { //public Sprocket sprocket; //Dont need this because FTCMode has it! + + public static HardwareMap hardwareMap; + public static Gamepad gamepad1; + public static Gamepad gamepad2; + public static Telemetry telemetry; public FTCRobot() { super(); sprocket=new Sprocket(this); super.sprocket=sprocket;//Doesnt do anything; better to be safe than sorry + super.robot=this; super.mode=MODE.TELEOP; } + + public void ftcSetup(HardwareMap hardwareMap,Gamepad gamepad1,Gamepad gamepad2,Telemetry telemetry) + { + this.hardwareMap=hardwareMap; + this.gamepad1=gamepad1; + this.gamepad2=gamepad2; + this.telemetry=telemetry; + } } diff --git a/src/org/montclairrobotics/sprocket/ftc/LightSensor.java b/src/org/montclairrobotics/sprocket/ftc/LightSensor.java new file mode 100644 index 0000000..14e1267 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/LightSensor.java @@ -0,0 +1,20 @@ +package org.montclairrobotics.sprocket.ftc; + +import org.montclairrobotics.sprocket.utils.Input; + +public class LightSensor implements Input{ + private LightSensor sensor; + + public LightSensor(String sensorID) + { + sensor=FTCRobot.hardwareMap.lightSensor.get("lightGround"); + } + public void enableLed(boolean enable) + { + sensor.enableLed(enable); + } + public Double get() + { + return sensor.getRawLightDetected(); + } +} From 09bd6432027d21147f8b44795fbca55fd4cbf7d0 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 6 Sep 2017 19:50:20 -0400 Subject: [PATCH 36/71] Finished Buttons; I had to write some python code to write this. Not good --- .../sprocket/ftc/FTCButton.java | 65 ++++++++++++++++++- .../sprocket/ftc/FTCJoystick.java | 2 +- .../sprocket/ftc/FTCRobot.java | 1 + 3 files changed, 65 insertions(+), 3 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java index 00fb4f9..67a483c 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java @@ -1,11 +1,10 @@ package org.montclairrobotics.sprocket.ftc; -import org.montclairrobotics.sprocket.ftc.FTCJoystick.GAMEPAD; import org.montclairrobotics.sprocket.ftc.FTCJoystick.STICK; +import org.montclairrobotics.sprocket.ftc.FTCRobot.GAMEPAD; import org.montclairrobotics.sprocket.utils.Input; public class FTCButton implements Input{ - public enum GAMEPAD {A,B}; enum BUTTON { a, b, @@ -25,6 +24,8 @@ enum BUTTON { left_trigger,//a float right_trigger;//a float } + + private static final float THREASHOLD = 50; private Gamepad gamepad; private BUTTON button; @@ -46,4 +47,64 @@ public FTCButton(Gamepad gamepad,BUTTON button) this.gamepad=gamepad; this.button=button; } + + public Boolean get() + { + switch(button) + { + case a: + return gamepad.a; + + case b: + return gamepad.b; + + case x: + return gamepad.x; + + case y: + return gamepad.y; + + case start: + return gamepad.start; + + case back: + return gamepad.back; + + case right_bumper: + return gamepad.right_bumper; + + case left_bumper: + return gamepad.left_bumper; + + case dpad_up: + return gamepad.dpad_up; + + case dpad_down: + return gamepad.dpad_down; + + case dpad_left: + return gamepad.dpad_left; + + case dpad_right: + return gamepad.dpad_right; + + case guide: + return gamepad.guide; + + case left_stick_button: + return gamepad.left_stick_button; + + case right_stick_button: + return gamepad.right_stick_button; + + case left_trigger: + return gamepad.left_trigger>THREASHOLD; + + case right_trigger: + return gamepad.right_trigger>THREASHOLD; + + default: + return false; + } + } } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java index 9596ed2..015c5ac 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java @@ -1,10 +1,10 @@ package org.montclairrobotics.sprocket.ftc; import org.montclairrobotics.sprocket.core.IJoystick; +import org.montclairrobotics.sprocket.ftc.FTCRobot.GAMEPAD; //This class kills me public class FTCJoystick implements IJoystick{ - public enum GAMEPAD {A,B}; public enum STICK {LEFT,RIGHT,DPAD}; private Gamepad gamepad; private STICK stick; diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index e680864..cf8a64e 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -6,6 +6,7 @@ public class FTCRobot extends FTCMode implements IRobot { + public enum GAMEPAD {A,B}; //public Sprocket sprocket; //Dont need this because FTCMode has it! public static HardwareMap hardwareMap; From 649bd8d4c9ba5a5ef754a33c05ba8c3943e2aa2a Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 8 Sep 2017 19:59:37 -0400 Subject: [PATCH 37/71] At least it compiles now --- src/org/montclairrobotics/sprocket/ftc/FTCButton.java | 5 +++-- .../montclairrobotics/sprocket/ftc/FTCJoystick.java | 11 ++++++----- src/org/montclairrobotics/sprocket/ftc/FTCMode.java | 10 +++------- src/org/montclairrobotics/sprocket/ftc/FTCMotor.java | 2 ++ src/org/montclairrobotics/sprocket/ftc/FTCRobot.java | 4 ++++ .../montclairrobotics/sprocket/ftc/LightSensor.java | 2 +- 6 files changed, 19 insertions(+), 15 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java index 67a483c..c948a53 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java @@ -1,6 +1,7 @@ package org.montclairrobotics.sprocket.ftc; -import org.montclairrobotics.sprocket.ftc.FTCJoystick.STICK; +import com.qualcomm.robotcore.hardware.Gamepad; + import org.montclairrobotics.sprocket.ftc.FTCRobot.GAMEPAD; import org.montclairrobotics.sprocket.utils.Input; @@ -42,7 +43,7 @@ public FTCButton(GAMEPAD gamepad,BUTTON button) } this.button=button; } - public FTCButton(Gamepad gamepad,BUTTON button) + public FTCButton(Gamepad gamepad, BUTTON button) { this.gamepad=gamepad; this.button=button; diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java index 015c5ac..fe30d70 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java @@ -1,5 +1,6 @@ package org.montclairrobotics.sprocket.ftc; +import com.qualcomm.robotcore.hardware.Gamepad; import org.montclairrobotics.sprocket.core.IJoystick; import org.montclairrobotics.sprocket.ftc.FTCRobot.GAMEPAD; @@ -8,7 +9,7 @@ public class FTCJoystick implements IJoystick{ public enum STICK {LEFT,RIGHT,DPAD}; private Gamepad gamepad; private STICK stick; - + public FTCJoystick(GAMEPAD gamepad,STICK stick) { if(gamepad==GAMEPAD.A) @@ -18,7 +19,7 @@ public FTCJoystick(GAMEPAD gamepad,STICK stick) else { this.gamepad=FTCRobot.gamepad2; - } + } this.stick=stick; } public FTCJoystick(Gamepad gamepad,STICK stick) @@ -26,7 +27,7 @@ public FTCJoystick(Gamepad gamepad,STICK stick) this.gamepad=gamepad; this.stick=stick; } - + @Override public double getX() { if(stick==STICK.LEFT) @@ -39,7 +40,7 @@ else if(stick==STICK.RIGHT) } else { - return gamepad.dpad_right?1:0-gamepad.dpad_left?1:0; + return (gamepad.dpad_right?1:0)-(gamepad.dpad_left?1:0); } } @Override @@ -54,7 +55,7 @@ else if(stick==STICK.RIGHT) } else { - return gamepad.dpad_up?1:0-gamepad.dpad_down?1:0; + return (gamepad.dpad_up?1:0)-(gamepad.dpad_down?1:0); } } } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java index 038c4ce..05db6fe 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java @@ -1,20 +1,16 @@ package org.montclairrobotics.sprocket.ftc; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.core.IRobot; import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.core.Sprocket.MODE; -import org.montclairrobotics.sprocket.loop.Updatable; - -import edu.wpi.first.wpilibj.IterativeRobot; - - /** * @author MHS Robotics * This class is basically just a wrapper around iterative robot which all Sprocket * robots must extend. */ -public abstract class FTCMode /*extends OpMode*/{ +public abstract class FTCMode extends OpMode { public Sprocket sprocket; public FTCRobot robot; diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java index 6e86cd4..f48274d 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java @@ -1,5 +1,7 @@ package org.montclairrobotics.sprocket.ftc; +import com.qualcomm.robotcore.hardware.DcMotor; + import org.montclairrobotics.sprocket.core.IMotor; public class FTCMotor implements IMotor{ diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index cf8a64e..3651411 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -1,5 +1,9 @@ package org.montclairrobotics.sprocket.ftc; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.montclairrobotics.sprocket.core.IRobot; import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.core.Sprocket.MODE; diff --git a/src/org/montclairrobotics/sprocket/ftc/LightSensor.java b/src/org/montclairrobotics/sprocket/ftc/LightSensor.java index 14e1267..2846fb7 100644 --- a/src/org/montclairrobotics/sprocket/ftc/LightSensor.java +++ b/src/org/montclairrobotics/sprocket/ftc/LightSensor.java @@ -3,7 +3,7 @@ import org.montclairrobotics.sprocket.utils.Input; public class LightSensor implements Input{ - private LightSensor sensor; + private com.qualcomm.robotcore.hardware.LightSensor sensor; public LightSensor(String sensorID) { From 5ef4f7bf98abf074ab9c4e72710b08e5ebb7b8a0 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 8 Sep 2017 21:37:53 -0400 Subject: [PATCH 38/71] makes drivetrain builder compile with ftc --- .../sprocket/drive/DriveTrainBuilder.java | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java index 54c4f7c..f53b316 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java @@ -1,20 +1,15 @@ package org.montclairrobotics.sprocket.drive; -import java.util.ArrayList; - import org.montclairrobotics.sprocket.control.ArcadeDriveInput; import org.montclairrobotics.sprocket.core.IJoystick; -import org.montclairrobotics.sprocket.frc.FRCMotor; +import org.montclairrobotics.sprocket.core.IMotor; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Polar; import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.motors.Module; -import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.pipeline.Pipeline; import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.PID; -import edu.wpi.first.wpilibj.SpeedController; +import java.util.ArrayList; /** * DriveTrainBuilder is a helper class which makes constructing DriveTrains more @@ -43,7 +38,7 @@ public DriveTrainBuilder addDriveModule(DriveModule module) { modules.add(module); return this; } - + /* public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle force, SEncoder enc, PID pid) { modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, new FRCMotor(motor))); return this; @@ -51,10 +46,10 @@ public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle fo public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle force) { return addWheel(motor, offset, force, null, null); - } + }*/ - public DriveTrainBuilder addWheels(Vector offset, Angle force, FRCMotor... motors) { + public DriveTrainBuilder addWheels(Vector offset, Angle force, IMotor... motors) { modules.add(new DriveModule(offset, new Polar(1, force), motors)); return this; } From 95047a8e25ded74fe5ef6bb53e34266969dbc3c2 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 8 Sep 2017 21:54:48 -0400 Subject: [PATCH 39/71] That wasn't too bad. Everything but DriveEncodersGyro is done. That one will take a bit more thought. --- .../sprocket/actions/Action.java | 8 ++++---- .../sprocket/actions/MultiState.java | 6 ++++-- .../sprocket/auto/states/DriveEncoders.java | 7 ++++++- .../sprocket/core/IRobot.java | 12 +++++------ .../sprocket/loop/Updater.java | 9 ++++++--- .../sprocket/utils/Input.java | 9 --------- .../sprocket/utils/OppositeInput.java | 20 +++++++++++++++++++ .../sprocket/utils/SmoothVectorInput.java | 5 +++-- 8 files changed, 49 insertions(+), 27 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/utils/OppositeInput.java diff --git a/src/org/montclairrobotics/sprocket/actions/Action.java b/src/org/montclairrobotics/sprocket/actions/Action.java index b50db73..9b2d69b 100644 --- a/src/org/montclairrobotics/sprocket/actions/Action.java +++ b/src/org/montclairrobotics/sprocket/actions/Action.java @@ -14,18 +14,18 @@ public interface Action { /** * Called once when the object is activated */ - public default void start(){} + public void start(); /** * Called every loop the object is active, after it is activated */ - public default void enabled(){} + public void enabled(); /** * Called once when the object is deactivated */ - public default void stop(){} + public void stop(); /** * Called every loop when the object is deactivated * Note: this may not be called by every implementation */ - public default void disabled(){} + public void disabled(); } diff --git a/src/org/montclairrobotics/sprocket/actions/MultiState.java b/src/org/montclairrobotics/sprocket/actions/MultiState.java index b99618c..f603f48 100644 --- a/src/org/montclairrobotics/sprocket/actions/MultiState.java +++ b/src/org/montclairrobotics/sprocket/actions/MultiState.java @@ -18,11 +18,13 @@ public MultiState(Input done, State... states) { public MultiState(int stateToStopAt, State... states) { super(states); + final State[] statesFinal=states; + final int stateToStopAtFinal=stateToStopAt; if(stateToStopAt < 0 || stateToStopAt > states.length) { this.done = new Input() { @Override public Boolean get() { - for(State s : states) { + for(State s : statesFinal) { if(!s.isDone()) { return false; } @@ -34,7 +36,7 @@ public Boolean get() { this.done = new Input() { @Override public Boolean get() { - return states[stateToStopAt].isDone(); + return statesFinal[stateToStopAtFinal].isDone(); } }; } diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index 0e0163e..8b29248 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -79,7 +79,12 @@ public void enabled() { //tgtDir = new XY(0, speed); } - + + @Override + public void disabled() { + + } + @Override public boolean userIsDone() { Debug.msg("forwards", forwards?"YES":"NO"); diff --git a/src/org/montclairrobotics/sprocket/core/IRobot.java b/src/org/montclairrobotics/sprocket/core/IRobot.java index c6ed55f..bf8a17f 100644 --- a/src/org/montclairrobotics/sprocket/core/IRobot.java +++ b/src/org/montclairrobotics/sprocket/core/IRobot.java @@ -5,10 +5,10 @@ public interface IRobot extends Updatable{ //Stuff user overrides - public default void init(){} - public default void start(Sprocket.MODE mode){} - public default void stop(){} - public default void update(){} - public default void disabledUpdate(){} - public default void debugs(){} + public void init(); + public void start(Sprocket.MODE mode); + public void stop(); + public void update(); + public void disabledUpdate(); + public void debugs(); } diff --git a/src/org/montclairrobotics/sprocket/loop/Updater.java b/src/org/montclairrobotics/sprocket/loop/Updater.java index e0f53e2..42e0f86 100644 --- a/src/org/montclairrobotics/sprocket/loop/Updater.java +++ b/src/org/montclairrobotics/sprocket/loop/Updater.java @@ -1,6 +1,7 @@ package org.montclairrobotics.sprocket.loop; import java.util.ArrayList; +import java.util.Comparator; import java.util.TreeMap; public class Updater { @@ -8,7 +9,9 @@ public class Updater { private static double lastLoop=getTime(); private static double loopTime=1.0/50; - private static TreeMap> updatables = new TreeMap<>((o1, o2) -> { + private static TreeMap> updatables = new TreeMap<>(new Comparator() { + @Override + public int compare(Priority o1, Priority o2) { if(o1.getPriority() > o2.getPriority()) { return -1; } else if(o1.getPriority() == o2.getPriority()) { @@ -16,12 +19,12 @@ public class Updater { } return 1; } - ); + }); public static void add(Updatable updatable, Priority priority) { - if(!updatables.containsKey(priority)) updatables.put(priority, new ArrayList<>()); + if(!updatables.containsKey(priority)) updatables.put(priority, new ArrayList()); ArrayList priorities = updatables.get(priority); priorities.add(updatable); } diff --git a/src/org/montclairrobotics/sprocket/utils/Input.java b/src/org/montclairrobotics/sprocket/utils/Input.java index 232fbeb..584efb4 100644 --- a/src/org/montclairrobotics/sprocket/utils/Input.java +++ b/src/org/montclairrobotics/sprocket/utils/Input.java @@ -3,14 +3,5 @@ public interface Input { public T get(); - - public static Input opposite(Input a) - { - return new Input(){ - @Override - public Double get() { - return -a.get(); - }}; - } } diff --git a/src/org/montclairrobotics/sprocket/utils/OppositeInput.java b/src/org/montclairrobotics/sprocket/utils/OppositeInput.java new file mode 100644 index 0000000..b674242 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/utils/OppositeInput.java @@ -0,0 +1,20 @@ +package org.montclairrobotics.sprocket.utils; + +/** + * Created by Hymowitz on 9/8/2017. + */ + +public class OppositeInput implements Input{ + + private Input inp; + + public OppositeInput(Input inp) + { + this.inp=inp; + } + + public Double get() + { + return -inp.get(); + } +} diff --git a/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java b/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java index 24f2978..b689c48 100644 --- a/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java +++ b/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java @@ -9,19 +9,20 @@ public class SmoothVectorInput implements Input{ public SmoothVectorInput(int len,Input inp) { + final Input inpFinal=inp; x=new SmoothInput(len,new Input(){ @Override public Double get() { // TODO Auto-generated method stub - return inp.get().getX(); + return inpFinal.get().getX(); }}); y=new SmoothInput(len,new Input(){ @Override public Double get() { // TODO Auto-generated method stub - return inp.get().getY(); + return inpFinal.get().getY(); }}); } From 27777fdc73e307e810c83aa16f688915248e2b57 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 8 Sep 2017 22:21:29 -0400 Subject: [PATCH 40/71] Added a lot of methods that no longer default to empty. --- .../sprocket/actions/StateMachine.java | 5 +++ .../sprocket/actions/Toggle.java | 12 ++++++ .../sprocket/auto/AutoMode.java | 8 +++- .../sprocket/auto/AutoState.java | 9 ++++ .../sprocket/auto/states/Delay.java | 6 +++ .../auto/states/DriveEncoderGyro.java | 43 ++++++++++++------- .../control/FieldCentricDriveInput.java | 10 +++++ .../sprocket/drive/ControlledMotor.java | 12 ++++++ .../sprocket/drive/steps/AccelLimit.java | 10 +++++ .../sprocket/drive/steps/GyroCorrection.java | 10 +++++ .../sprocket/drive/steps/SpeedLimiter.java | 12 ++++++ .../sprocket/drive/steps/TurnLimiter.java | 12 ++++++ .../sprocket/drive/utils/GyroLock.java | 12 ++++++ .../sprocket/utils/PIDTuner.java | 18 ++++++++ 14 files changed, 162 insertions(+), 17 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/actions/StateMachine.java b/src/org/montclairrobotics/sprocket/actions/StateMachine.java index 052eeea..86850a1 100644 --- a/src/org/montclairrobotics/sprocket/actions/StateMachine.java +++ b/src/org/montclairrobotics/sprocket/actions/StateMachine.java @@ -65,4 +65,9 @@ public void update() { enabled(); } } + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/actions/Toggle.java b/src/org/montclairrobotics/sprocket/actions/Toggle.java index 33d20fd..3fe697b 100644 --- a/src/org/montclairrobotics/sprocket/actions/Toggle.java +++ b/src/org/montclairrobotics/sprocket/actions/Toggle.java @@ -20,5 +20,17 @@ public void stop() { obj.disable(); } + + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index 39523f2..a7c4d39 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -41,5 +41,11 @@ public String toString() { return name; } - + public void enabled() + { + } + public void disabled() + { + + } } diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index 0f17343..f5f5f7e 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -86,4 +86,13 @@ public void setTimeout(double t) { this.TIMEOUT=t; } + + public void enabled() + { + + } + public void disabled() + { + + } } diff --git a/src/org/montclairrobotics/sprocket/auto/states/Delay.java b/src/org/montclairrobotics/sprocket/auto/states/Delay.java index 9ccdb8f..a9694e9 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/Delay.java +++ b/src/org/montclairrobotics/sprocket/auto/states/Delay.java @@ -39,5 +39,11 @@ public boolean userIsDone() { Debug.num("Time in State", timeInState()); return false; } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java index 858915e..bdcb4be 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java @@ -63,43 +63,55 @@ public boolean isDone() { }}) ); } - - public DriveEncoderGyro(Input dInput, Input a, boolean relative, Input speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) { - super( + + //Lets make it to States! + public static State[] makeStatesA(Distance d,Angle a,boolean relative,double speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) + { + return { new TurnGyro(a,driveGyro,relative), new MultiState(0, - new DriveEncoders(dInput,speed, maxEncAccel, maxEncTicksPerSec), + new DriveEncoders(d,speed, maxEncAccel, maxEncTicksPerSec), new State(){ @Override public void start() { - driveGyro.setTargetAngle(new Degrees(a.get()),relative); + driveGyro.setTargetAngle(a,relative); } @Override public void stop() {} + + @Override + public void disabled() { + } + @Override public void enabled() { driveGyro.use(); } + @Override public boolean isDone() { // TODO Auto-generated method stub return false; }} - ) - ); + )}; } - - public DriveEncoderGyro(Input d, Input speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) { - super( - new MultiState(0, - new DriveEncoders(d, speed, maxEncAccel, maxEncTicksPerSec), - new State(){ + public static State[] makeStatesB(Distance d, double speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) + { + return {new MultiState(0, + new DriveEncoders(d, speed, maxEncAccel, maxEncTicksPerSec), + new State(){ @Override public void start() { driveGyro.setTargetAngleRelative(); } @Override public void stop() {} + + @Override + public void disabled() { + + } + @Override public void enabled() { driveGyro.use(); @@ -109,8 +121,7 @@ public void enabled() { public boolean isDone() { // TODO Auto-generated method stub return false; - }}) - ); + }})}; + ); } - } diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index bb1f477..fc7edbb 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -92,4 +92,14 @@ public void start() { public void stop() { Sprocket.getMainDriveTrain().useDefaultInput(); } + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java index d153f4e..406fdff 100644 --- a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java +++ b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java @@ -78,6 +78,18 @@ public void stop() { enabled=false; } + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } + } class ButtonPairInput implements Input { diff --git a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java index a4334aa..a440f59 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java @@ -78,5 +78,15 @@ public void start() { public void stop() { enabled=false; } + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java index 17f1f1e..c310719 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/GyroCorrection.java @@ -165,5 +165,15 @@ public void setTargetAngle(Angle a,boolean relative) { setTargetAngleReset(a); } } + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java index 156e444..7b108aa 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java @@ -35,4 +35,16 @@ public DTTarget get(DTTarget in) { } return new DTTarget(out,in.getTurn()); } + + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java index 1d8c014..f8dd356 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java @@ -36,4 +36,16 @@ public DTTarget get(DTTarget in) { } return new DTTarget(in.getDirection(),out); } + + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java index eed213c..098d674 100644 --- a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java +++ b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java @@ -51,6 +51,18 @@ public Angle getTargetAngle() { public void setTargetAngle(Angle a) { gyro.setTargetAngleRaw(a); } + + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java index 3b87857..88f5bee 100644 --- a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java +++ b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java @@ -31,6 +31,24 @@ public PIDTuner(Input TempP,Input cyclesPer10Sec,Input @Override public void start() { recalculatePIDs(); + } + + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void stop() { + // TODO Auto-generated method stub + + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + }}); From 2738a7a1820f17ce2a71bded428ecbf19e30e5b7 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 8 Sep 2017 22:48:37 -0400 Subject: [PATCH 41/71] Much better, and I even used the Number inputs! I wonder if they work. --- .../auto/states/DriveEncoderGyro.java | 148 +++++------------- .../sprocket/auto/states/DriveEncoders.java | 76 ++++----- .../sprocket/auto/states/TurnGyro.java | 40 ++--- 3 files changed, 91 insertions(+), 173 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java index bdcb4be..88ff93c 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java @@ -11,117 +11,49 @@ public class DriveEncoderGyro extends StateMachine{ - - - public DriveEncoderGyro(Distance d,Angle a,boolean relative,double speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) + public DriveEncoderGyro(Number distance,Number speed,Number angle,Comparable relative,GyroCorrection gyroCorrection) { - super( - new TurnGyro(a,driveGyro,relative), - new MultiState(0, - new DriveEncoders(d,speed, maxEncAccel, maxEncTicksPerSec), - new State(){ - @Override - public void start() { - driveGyro.setTargetAngle(a,relative); - } - @Override - public void stop() {} - @Override - public void enabled() { - driveGyro.use(); - } - - @Override - public boolean isDone() { - // TODO Auto-generated method stub - return false; - }} - ) - ); + super(makeStates(distance,speed,angle,relative,gyroCorrection)); } - public DriveEncoderGyro(Distance d, double speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) { - super( - new MultiState(0, - new DriveEncoders(d, speed, maxEncAccel, maxEncTicksPerSec), - new State(){ - @Override - public void start() { - driveGyro.setTargetAngleRelative(); - } - @Override - public void stop() {} - @Override - public void enabled() { - driveGyro.use(); - } - - @Override - public boolean isDone() { - // TODO Auto-generated method stub - return false; - }}) - ); - } - - //Lets make it to States! - public static State[] makeStatesA(Distance d,Angle a,boolean relative,double speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) + public static State[] makeStates(Number distance,Number speed,Number angle,Comparable relative,GyroCorrection gyroCorrection) { - return { - new TurnGyro(a,driveGyro,relative), - new MultiState(0, - new DriveEncoders(d,speed, maxEncAccel, maxEncTicksPerSec), - new State(){ - @Override - public void start() { - driveGyro.setTargetAngle(a,relative); - } - @Override - public void stop() {} - - @Override - public void disabled() { - } - - @Override - public void enabled() { - driveGyro.use(); - } - - @Override - public boolean isDone() { - // TODO Auto-generated method stub - return false; - }} - )}; - } - public static State[] makeStatesB(Distance d, double speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) - { - return {new MultiState(0, - new DriveEncoders(d, speed, maxEncAccel, maxEncTicksPerSec), - new State(){ - @Override - public void start() { - driveGyro.setTargetAngleRelative(); - } - @Override - public void stop() {} - - @Override - public void disabled() { - - } - - @Override - public void enabled() { - driveGyro.use(); - } - - @Override - public boolean isDone() { - // TODO Auto-generated method stub - return false; - }})}; - ); + State[] r={ + new TurnGyro(angle,gyroCorrection,relative), + new State() + { + + @Override + public void start() { + // TODO Auto-generated method stub + gyroCorrection.setTargetAngle(new Degrees(angle.doubleValue()),relative.compareTo(true)==0); + } + + @Override + public void enabled() { + // TODO Auto-generated method stub + gyroCorrection.use(); + } + + @Override + public void stop() { + // TODO Auto-generated method stub + + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } + + @Override + public boolean isDone() { + // TODO Auto-generated method stub + return false; + } + } + }; + return r; } } diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java index 8b29248..8d3dd27 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -11,47 +11,33 @@ public class DriveEncoders extends AutoState { + public static double MAX_ENC_ACCEL=0; + public static double MAX_ENC_TICKS_PER_SEC=0; + private DriveTrain dt; - private Distance tgtDistance; - private Distance stopDist; - private double maxEncAccel; - private double maxEncTicksPerSec; + private double distance; + private double stopDist; private double speed; - private boolean forwards; + //private boolean forwards; - private Input dashInput; - private Input speedDashInput; + private Number inDistance; + private Number inSpeed; - public DriveEncoders(Distance tgtDistance, double speed, double maxEncAccel, double maxEncTicksPerSec) { - this.tgtDistance=tgtDistance; - this.speed = speed; - this.maxEncAccel=maxEncAccel; - this.maxEncTicksPerSec = maxEncTicksPerSec; - } - public DriveEncoders(double tgtDistance, double speed, double maxEncAccel, double maxEncTicksPerSec) { - this(new Distance(tgtDistance), speed, maxEncAccel, maxEncTicksPerSec); + public DriveEncoders(Number distance, Number speed) { + this.inDistance=distance; + this.inSpeed=speed; } - public DriveEncoders(Input dashInput, Input speed, double maxEncAccel, double maxEncTicksPerSec) { - this.dashInput = dashInput; - this.speedDashInput = speed; - this.maxEncAccel = maxEncAccel; - this.maxEncTicksPerSec = maxEncTicksPerSec; - } @Override public void userStart() { - if(dashInput != null) { - this.tgtDistance = new Distance(dashInput.get()); - } - if(speedDashInput != null) { - this.speed = speedDashInput.get(); - } - this.dt = Sprocket.getMainDriveTrain(); - stopDist = new Distance(dt.getDistance().getY()+tgtDistance.get()); - forwards=tgtDistance.get()>0; + distance=inDistance.doubleValue(); + speed=inSpeed.doubleValue(); + dt = Sprocket.getMainDriveTrain(); + stopDist = dt.getDistance().getY()+distance; + //forwards=distance>0; } @Override @@ -68,13 +54,19 @@ public void enabled() { *(stopDist.get()-dt.getDistance().getY()>0?1:-1) ,-speed,speed)); */ - - - double tgtV2inTicks=2*maxEncAccel*(stopDist.get()-dt.get().get()); - double tgtV=Math.sqrt(Math.abs(tgtV2inTicks))/maxEncTicksPerSec*(stopDist.get()-dt.get().get()>0?1:-1); - Debug.num("dt-get-get", dt.get().get()); - Debug.num("stopDist", stopDist.get()); - tgtV=Utils.constrain(tgtV, -speed, speed); + double tgtV; + if(MAX_ENC_ACCEL!=0||MAX_ENC_TICKS_PER_SEC!=0) + { + double tgtV2inTicks=2*MAX_ENC_ACCEL*(stopDist-dt.get().get()); + tgtV=Math.sqrt(Math.abs(tgtV2inTicks))/MAX_ENC_TICKS_PER_SEC*(stopDist-dt.get().get()>0?1:-1); + Debug.num("dt-get-get", dt.get().get()); + Debug.num("stopDist", stopDist); + tgtV=Utils.constrain(tgtV, -speed, speed); + } + else + { + tgtV=speed*(distance>0?1:0); + } tgtDir = new XY(0,tgtV); //tgtDir = new XY(0, speed); @@ -87,13 +79,13 @@ public void disabled() { @Override public boolean userIsDone() { - Debug.msg("forwards", forwards?"YES":"NO"); + Debug.msg("forwards", distance>0?"YES":"NO"); Debug.msg("DISTANCE", dt.getDistance().getY()); - Debug.msg("StopDistance", stopDist.get()); - if(forwards) - return dt.getDistance().getY()>stopDist.get()-1; + Debug.msg("StopDistance", stopDist); + if(distance>0) + return dt.getDistance().getY()>stopDist-1; else - return dt.getDistance().getY() dashInput; + private Number tgtInDeg; - public Angle getTgt() { - return tgt; - } - - public void setTgt(Angle tgt) { - this.tgt = tgt; - } private GyroCorrection gyro; private double incorrectTime; - private boolean relative; + private Comparable relative; - private static final Angle tolerance=new Degrees(4); - private static final double timeAtTarget=0.5; + public static Angle tolerance=new Degrees(4); + public static double timeAtTarget=0.5; - public TurnGyro(Angle tgt,GyroCorrection gyro,boolean relative) + public TurnGyro(Number tgtDeg,GyroCorrection gyro,Comparable relative) { - this.tgt=tgt; + this.tgtInDeg=tgtDeg; this.gyro=gyro; this.relative=relative; } - public TurnGyro(Input dashInput, GyroCorrection gyro, boolean relative) { - this.dashInput = dashInput; - this.gyro = gyro; - this.relative = relative; - } - @Override public void userStart() { - if(dashInput != null) { - tgt = new Degrees(dashInput.get()); - } - if(relative) + tgt = new Degrees(tgtInDeg.doubleValue()); + if(relative.compareTo(true)==0) { gyro.setTargetAngleRelative(tgt); } @@ -83,5 +68,14 @@ public void userStop() public boolean userIsDone() { return (Updater.getTime()-incorrectTime>timeAtTarget); } + + + public Angle getTgt() { + return tgt; + } + + public void setTgt(Angle tgt) { + this.tgt = tgt; + } } From 8f9fd8d5d828f479b1c1aba712b6590a117902ba Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 8 Sep 2017 22:58:17 -0400 Subject: [PATCH 42/71] OK, now I am done. --- .../sprocket/auto/states/DriveEncoderGyro.java | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java index 88ff93c..822ec8c 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java @@ -1,13 +1,9 @@ package org.montclairrobotics.sprocket.auto.states; -import org.montclairrobotics.sprocket.actions.MultiState; import org.montclairrobotics.sprocket.actions.State; import org.montclairrobotics.sprocket.actions.StateMachine; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; -import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.Distance; -import org.montclairrobotics.sprocket.utils.Input; public class DriveEncoderGyro extends StateMachine{ @@ -18,6 +14,9 @@ public DriveEncoderGyro(Number distance,Number speed,Number angle,Comparable relative,GyroCorrection gyroCorrection) { + final GyroCorrection gcFinal=gyroCorrection; + final Number angleFinal=angle; + final Comparable relativeFinal=relative; State[] r={ new TurnGyro(angle,gyroCorrection,relative), new State() @@ -26,13 +25,13 @@ public static State[] makeStates(Number distance,Number speed,Number angle,Compa @Override public void start() { // TODO Auto-generated method stub - gyroCorrection.setTargetAngle(new Degrees(angle.doubleValue()),relative.compareTo(true)==0); + gcFinal.setTargetAngle(new Degrees(angleFinal.doubleValue()),relativeFinal.compareTo(true)==0); } @Override public void enabled() { // TODO Auto-generated method stub - gyroCorrection.use(); + gcFinal.use(); } @Override From 6fd6a4c44bf1c3167c1bc47c4c8e09b1ba7c6b36 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 9 Sep 2017 11:10:25 -0400 Subject: [PATCH 43/71] Making this closer to working --- .../sprocket/core/IRobot.java | 2 +- .../sprocket/core/Sprocket.java | 2 +- .../sprocket/ftc/FTCMode.java | 21 ++++++++++++------ .../sprocket/ftc/FTCRobot.java | 22 +++++++++---------- 4 files changed, 27 insertions(+), 20 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/core/IRobot.java b/src/org/montclairrobotics/sprocket/core/IRobot.java index bf8a17f..0a20f8e 100644 --- a/src/org/montclairrobotics/sprocket/core/IRobot.java +++ b/src/org/montclairrobotics/sprocket/core/IRobot.java @@ -5,7 +5,7 @@ public interface IRobot extends Updatable{ //Stuff user overrides - public void init(); + public void setup(); public void start(Sprocket.MODE mode); public void stop(); public void update(); diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index f10e83d..982e09f 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -44,7 +44,7 @@ public static void setMainDriveTrain(DriveTrain dt) { public void initS() { - robot.init(); + robot.setup(); } public final void startS(MODE mode) { diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java index 05db6fe..6cda500 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java @@ -12,26 +12,33 @@ */ public abstract class FTCMode extends OpMode { - public Sprocket sprocket; - public FTCRobot robot; + private static boolean FIRST=true; + public static Sprocket sprocket; + public static FTCRobot robot; + public MODE mode; public Action action; public FTCMode() { } - public FTCMode(Sprocket sprocket,Action action) + public FTCMode(Action action) { - this.sprocket=sprocket; this.mode=MODE.AUTO; this.action=action; } //@Override public void init() { - sprocket.initS(); - if(robot!=null) - robot.ftcSetup(hardwareMap,gamepad1,gamepad2,telemetry); + if(robot!=null) { + robot.ftcSetup(hardwareMap, gamepad1, gamepad2, telemetry); + if (FIRST) { + sprocket = new Sprocket(robot); + sprocket.initS(); + FIRST = false; + } + sprocket.startS(MODE.DISABLED); + } } //@Override diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index 3651411..daa30a4 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -5,10 +5,10 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.montclairrobotics.sprocket.core.IRobot; -import org.montclairrobotics.sprocket.core.Sprocket; -import org.montclairrobotics.sprocket.core.Sprocket.MODE; -public class FTCRobot extends FTCMode implements IRobot { +public abstract class FTCRobot extends FTCMode implements IRobot { + + public enum GAMEPAD {A,B}; //public Sprocket sprocket; //Dont need this because FTCMode has it! @@ -17,15 +17,15 @@ public enum GAMEPAD {A,B}; public static Gamepad gamepad1; public static Gamepad gamepad2; public static Telemetry telemetry; - - public FTCRobot() + + /* + public static void setRobot() { - super(); - sprocket=new Sprocket(this); - super.sprocket=sprocket;//Doesnt do anything; better to be safe than sorry - super.robot=this; - super.mode=MODE.TELEOP; - } + if(robot==null) + { + robot=new MyTeleopRobotClass(); + } + }*/ public void ftcSetup(HardwareMap hardwareMap,Gamepad gamepad1,Gamepad gamepad2,Telemetry telemetry) { From f439fecbb3e7625ee8b183b9228ca41da1f75f62 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 8 Sep 2017 21:54:48 -0400 Subject: [PATCH 44/71] Revert some of this --- src/org/montclairrobotics/sprocket/actions/MultiState.java | 6 ++---- .../montclairrobotics/sprocket/utils/SmoothVectorInput.java | 5 ++--- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/actions/MultiState.java b/src/org/montclairrobotics/sprocket/actions/MultiState.java index f603f48..b99618c 100644 --- a/src/org/montclairrobotics/sprocket/actions/MultiState.java +++ b/src/org/montclairrobotics/sprocket/actions/MultiState.java @@ -18,13 +18,11 @@ public MultiState(Input done, State... states) { public MultiState(int stateToStopAt, State... states) { super(states); - final State[] statesFinal=states; - final int stateToStopAtFinal=stateToStopAt; if(stateToStopAt < 0 || stateToStopAt > states.length) { this.done = new Input() { @Override public Boolean get() { - for(State s : statesFinal) { + for(State s : states) { if(!s.isDone()) { return false; } @@ -36,7 +34,7 @@ public Boolean get() { this.done = new Input() { @Override public Boolean get() { - return statesFinal[stateToStopAtFinal].isDone(); + return states[stateToStopAt].isDone(); } }; } diff --git a/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java b/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java index b689c48..24f2978 100644 --- a/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java +++ b/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java @@ -9,20 +9,19 @@ public class SmoothVectorInput implements Input{ public SmoothVectorInput(int len,Input inp) { - final Input inpFinal=inp; x=new SmoothInput(len,new Input(){ @Override public Double get() { // TODO Auto-generated method stub - return inpFinal.get().getX(); + return inp.get().getX(); }}); y=new SmoothInput(len,new Input(){ @Override public Double get() { // TODO Auto-generated method stub - return inpFinal.get().getY(); + return inp.get().getY(); }}); } From 9b113d92f81e95baab6a6ececa104a6df0771210 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 15 Sep 2017 17:27:40 -0400 Subject: [PATCH 45/71] Java 7 compliant for Android Studio --- src/org/montclairrobotics/sprocket/actions/MultiState.java | 2 +- src/org/montclairrobotics/sprocket/auto/AutoMode.java | 2 +- src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/actions/MultiState.java b/src/org/montclairrobotics/sprocket/actions/MultiState.java index b99618c..8ae5d86 100644 --- a/src/org/montclairrobotics/sprocket/actions/MultiState.java +++ b/src/org/montclairrobotics/sprocket/actions/MultiState.java @@ -16,7 +16,7 @@ public MultiState(Input done, State... states) { this.done = done; } - public MultiState(int stateToStopAt, State... states) { + public MultiState(final int stateToStopAt, final State... states) { super(states); if(stateToStopAt < 0 || stateToStopAt > states.length) { this.done = new Input() { diff --git a/src/org/montclairrobotics/sprocket/auto/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index a7c4d39..5ae9982 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -20,7 +20,7 @@ public class AutoMode extends StateMachine{ /** * Creates an AutoMode based off of a pre-configured StateMachine * @param name The name of the AutoMode (for SmartDashboard) - * @param m The StateMachine which contains AutoModes + * @param states The StateMachine which contains AutoModes */ public AutoMode(String name, State... states) { diff --git a/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java b/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java index 24f2978..514f96d 100644 --- a/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java +++ b/src/org/montclairrobotics/sprocket/utils/SmoothVectorInput.java @@ -7,7 +7,7 @@ public class SmoothVectorInput implements Input{ private SmoothInput x,y; - public SmoothVectorInput(int len,Input inp) + public SmoothVectorInput(int len,final Input inp) { x=new SmoothInput(len,new Input(){ From 1526b21654cbae0ca4bffb96a9aeb7660ccfd7b8 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 15 Sep 2017 17:28:59 -0400 Subject: [PATCH 46/71] UniversalMapper created; should work for symmetric drivetrains, including mecanum, --- .../sprocket/drive/UniversalMapper.java | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 src/org/montclairrobotics/sprocket/drive/UniversalMapper.java diff --git a/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java new file mode 100644 index 0000000..95b843a --- /dev/null +++ b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java @@ -0,0 +1,34 @@ +package org.montclairrobotics.sprocket.drive; + +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.geometry.Radians; +import org.montclairrobotics.sprocket.geometry.Vector; + +public class UniversalMapper implements DTMapper{ + + @Override + public void map(DTTarget driveTarget, DriveModule[] driveModules) { + if(driveModules.length==0) return; + Vector dir=driveTarget.getDirection().setMag(1); + double maxForce=0; + double torque=0; + for(DriveModule module:driveModules) + { + double f=module.getForce().dotProduct(dir); + Vector fVec=module.getForce().setMag(f); + torque+=module.getOffset().rotate(Angle.QUARTER).setMag(1).dotProduct(fVec); + if(f>maxForce) + maxForce=f; + } + Angle tgtTurn=driveTarget.getTurn().subtract(new Radians(torque)); + for(DriveModule module:driveModules) + { + double f=module.getForce().dotProduct(dir); + Vector fVec=module.getForce().setMag(f/maxForce); + Vector tVec=module.getOffset().rotate(Angle.QUARTER).setMag(torque); + Vector vec=fVec.add(tVec); + module.set(vec.dotProduct(module.getForce())/module.getForce().getMagnitude()); + } + } + +} From f9e4c91ef669279e4b2925c3a37552b3f7c7b387 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 15 Sep 2017 18:19:06 -0400 Subject: [PATCH 47/71] Power (translation and turn) now a double from -1 to 1, for consistency, usability, and performance --- .../sprocket/auto/AutoState.java | 6 +-- .../sprocket/auto/states/DriveTime.java | 6 +-- .../sprocket/auto/states/TurnEncoders.java | 6 +-- .../sprocket/control/ArcadeDriveInput.java | 6 +-- .../control/FieldCentricDriveInput.java | 4 +- .../sprocket/drive/DTInput.java | 9 ++++- .../sprocket/drive/DTTarget.java | 8 ++-- .../sprocket/drive/DriveTrain.java | 2 +- .../sprocket/drive/GenericMapper.java | 12 +++--- .../sprocket/drive/MecanumMapper.java | 2 +- .../sprocket/drive/TankMapper.java | 2 +- .../sprocket/drive/UniversalMapper.java | 2 +- .../sprocket/drive/ZeroDTInput.java | 4 +- .../sprocket/drive/steps/AccelLimit.java | 26 ++++++------- .../sprocket/drive/steps/Deadzone.java | 37 +++++-------------- .../sprocket/drive/steps/GyroCorrection.java | 3 +- .../sprocket/drive/steps/TurnLimiter.java | 6 +-- 17 files changed, 61 insertions(+), 80 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/auto/AutoState.java b/src/org/montclairrobotics/sprocket/auto/AutoState.java index f5f5f7e..53fd874 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -16,7 +16,7 @@ */ public abstract class AutoState implements State,DTInput { public Vector tgtDir = Vector.ZERO; - public Angle tgtTurn = Angle.ZERO; + public double tgtTurn = 0; private double t;//time state was started public double TIMEOUT=30; @@ -59,7 +59,7 @@ public double timeInState() * @param v The direction * @param a The turning speed (units/sec) */ - public void setTarget(Vector v, Angle a) { + public void setTarget(Vector v, double a) { tgtDir = v; tgtTurn = a; } @@ -70,7 +70,7 @@ public Vector getDir() { } @Override - public Angle getTurn() { + public double getTurn() { return tgtTurn; } diff --git a/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java b/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java index c0befda..d18a66e 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveTime.java @@ -20,9 +20,9 @@ public DriveTime(double time,double power) } public DriveTime(double time,Vector tgtDir) { - this(time,tgtDir,Angle.ZERO); + this(time,tgtDir,0); } - public DriveTime(double time, Vector tgtDir, Angle tgtTurn) + public DriveTime(double time, Vector tgtDir, double tgtTurn) { super(time); this.tgtDir = tgtDir; @@ -31,7 +31,7 @@ public DriveTime(double time, Vector tgtDir, Angle tgtTurn) public DriveTime(Input timeInput, double power) { super(timeInput); this.tgtDir = new XY(0, power); - this.tgtTurn = Angle.ZERO; + this.tgtTurn = 0; } @Override diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java index 73b96c8..6cf39dd 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java @@ -8,11 +8,11 @@ public class TurnEncoders extends AutoState { private Angle turn; - private Angle turnSpeed; + private double turnSpeed; private double finalPos; private DriveModule module; - public TurnEncoders(Angle turn, Angle turnSpeed) { + public TurnEncoders(Angle turn, double turnSpeed) { this.turn = turn; this.turnSpeed = turnSpeed; } @@ -41,7 +41,7 @@ public boolean userIsDone() { @Override public void userStop() { - tgtTurn = Angle.ZERO; + tgtTurn = 0.0; } diff --git a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java index 2fbd997..c45bc71 100644 --- a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java @@ -22,7 +22,7 @@ public class ArcadeDriveInput implements DTInput, Updatable { private IJoystick stick; private Vector dir; - private Angle turn; + private double turn; private Vector raw; private double sensitivity = 1; @@ -67,7 +67,7 @@ public ArcadeDriveInput setSensitivity(double dir, double turn) } public void update() { - turn = new Radians(getX()*turnSensitivity); + turn = getX()*turnSensitivity; dir = new XY(0, getY()*sensitivity*-1); raw=new XY(getX(),-getY()); } @@ -104,7 +104,7 @@ public Vector getRaw() /** * @return The calculated turning speed for the DriveTrain */ - public Angle getTurn() { + public double getTurn() { return turn; } } diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index fc7edbb..bb40c3d 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -67,7 +67,7 @@ public Vector getDir() { * @return The calculated turning speed for the DriveTrain */ @Override - public Angle getTurn() { + public double getTurn() { if(rotToVector&&field.getMagnitude()>0.1) { @@ -81,7 +81,7 @@ public Angle getTurn() { } gyro.use(); } - return Angle.ZERO; + return 0; } @Override diff --git a/src/org/montclairrobotics/sprocket/drive/DTInput.java b/src/org/montclairrobotics/sprocket/drive/DTInput.java index 7314257..3836c64 100644 --- a/src/org/montclairrobotics/sprocket/drive/DTInput.java +++ b/src/org/montclairrobotics/sprocket/drive/DTInput.java @@ -4,7 +4,12 @@ import org.montclairrobotics.sprocket.geometry.Vector; public interface DTInput { - + /** + * X and Y from -1 to 1 inclusive; the power for x and y translation + */ public Vector getDir(); - public Angle getTurn(); + /** + * The rotation value, from -1 to 1, with 1 being a rotation to the right at full power + */ + public double getTurn(); } diff --git a/src/org/montclairrobotics/sprocket/drive/DTTarget.java b/src/org/montclairrobotics/sprocket/drive/DTTarget.java index 861ef16..8e18b3f 100644 --- a/src/org/montclairrobotics/sprocket/drive/DTTarget.java +++ b/src/org/montclairrobotics/sprocket/drive/DTTarget.java @@ -5,12 +5,12 @@ public class DTTarget { - public static final DTTarget ZERO = new DTTarget(Vector.ZERO,Angle.ZERO); + public static final DTTarget ZERO = new DTTarget(Vector.ZERO,0); private final Vector direction; - private final Angle turn; + private final double turn; //private MotorInputType inputType; - public DTTarget(Vector vector, Angle turn) { + public DTTarget(Vector vector, double turn) { this.direction = vector; this.turn = turn; //this.inputType = inputType; @@ -24,7 +24,7 @@ public Vector getDirection() { this.direction = direction; }*/ - public Angle getTurn() { + public double getTurn() { return turn; } diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java index 94d2ee0..4e65c83 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java @@ -33,7 +33,7 @@ public void update() { //DTInput input = auto ? this.autoInput : this.input; Vector tgtDir=input.getDir(); - Angle tgtTurn=input.getTurn(); + double tgtTurn=input.getTurn(); DTTarget target = new DTTarget(tgtDir,tgtTurn); Debug.string("DriveTrain INPUT:",target.toString()); target=pipeline.get(target); diff --git a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java index b6548bd..2bbbaa2 100644 --- a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java @@ -10,23 +10,23 @@ public class GenericMapper implements DTMapper{ @Override public void map(DTTarget driveTarget, DriveModule[] driveModules) { Vector tgtDir=driveTarget.getDirection(); - Angle tgtTurn=driveTarget.getTurn(); + double tgtTurn=driveTarget.getTurn(); for(DriveModule module:driveModules) { - tgtTurn=tgtTurn.subtract(getTorque(module.getOffset(),module.getForce(),tgtDir)); + tgtTurn-=getTorque(module.getOffset(),module.getForce(),tgtDir); } for(DriveModule module:driveModules) { module.set(inverseDot(module.getForce(),tgtDir.add( - new Polar(tgtTurn.toRadians()*module.getOffset().getMagnitude(),module.getOffset().getAngle().add(Angle.QUARTER))))); + new Polar(tgtTurn*module.getOffset().getMagnitude(),module.getOffset().getAngle().add(Angle.QUARTER))))); } } - public static Angle getTorque(Vector offset,Vector force,Vector target) + public static double getTorque(Vector offset,Vector force,Vector target) { if(offset.getMagnitude()==0) - return Angle.ZERO; - return new Radians(inverseDot(force,target)/offset.getMagnitude()); + return 0; + return inverseDot(force,target)/offset.getMagnitude(); } /* * If a dot b = |c| and c is || to b, given a and c returns b diff --git a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java index 99a6f34..cb054ac 100644 --- a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java @@ -10,7 +10,7 @@ public class MecanumMapper implements DTMapper { @Override public void map(DTTarget driveTarget, DriveModule[] driveModules) { - double turn = driveTarget.getTurn().toDegrees(); + double turn = driveTarget.getTurn(); for(DriveModule m : driveModules) { double power = getPower(driveTarget.getDirection(), m.getForceAngle()); diff --git a/src/org/montclairrobotics/sprocket/drive/TankMapper.java b/src/org/montclairrobotics/sprocket/drive/TankMapper.java index 65ce832..4e5fffb 100644 --- a/src/org/montclairrobotics/sprocket/drive/TankMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/TankMapper.java @@ -9,7 +9,7 @@ public class TankMapper implements DTMapper { @Override public void map(DTTarget driveTarget, DriveModule[] driveModules) { double power = driveTarget.getDirection().getY(); - double turn = driveTarget.getTurn().toRadians(); + double turn = driveTarget.getTurn(); double max = 0; diff --git a/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java index 95b843a..2ba5582 100644 --- a/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java @@ -20,7 +20,7 @@ public void map(DTTarget driveTarget, DriveModule[] driveModules) { if(f>maxForce) maxForce=f; } - Angle tgtTurn=driveTarget.getTurn().subtract(new Radians(torque)); + double tgtTurn=driveTarget.getTurn()-torque; for(DriveModule module:driveModules) { double f=module.getForce().dotProduct(dir); diff --git a/src/org/montclairrobotics/sprocket/drive/ZeroDTInput.java b/src/org/montclairrobotics/sprocket/drive/ZeroDTInput.java index 021cc1e..4277167 100644 --- a/src/org/montclairrobotics/sprocket/drive/ZeroDTInput.java +++ b/src/org/montclairrobotics/sprocket/drive/ZeroDTInput.java @@ -11,8 +11,8 @@ public Vector getDir() { } @Override - public Angle getTurn() { - return Angle.ZERO; + public double getTurn() { + return 0; } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java index a440f59..898fbe6 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java @@ -14,28 +14,24 @@ public class AccelLimit implements Step,Action{ - private Distance maxAccel; - private Angle maxTurn; + private double maxAccel; + private double maxTurn; private Vector lastDir; - private Angle lastTurn; + private double lastTurn; private boolean enabled=true; public AccelLimit() { - this(new Distance(4),new Degrees(180)); + this(4,4); } - public AccelLimit(double maxAccel,double maxTurn) - { - this(new Distance(maxAccel),new Radians(maxTurn)); - } - public AccelLimit(Distance maxAccel,Angle maxTurn) + public AccelLimit(double maxAccel,double maxTurn) { this.maxAccel=maxAccel; this.maxTurn=maxTurn; lastDir=Vector.ZERO; - lastTurn=Angle.ZERO; + lastTurn=0; } @Override @@ -47,19 +43,19 @@ public DTTarget get(DTTarget in) { //Debug.num("maxAccel", maxAccel.get()*Updater.getLoopTime()); //Debug.string("dDirBefore", dDir.toString()); - if(dDir.getMagnitude()>maxAccel.get()*Updater.getLoopTime()) + if(dDir.getMagnitude()>maxAccel*Updater.getLoopTime()) { - dDir=dDir.setMag(maxAccel.get()*Updater.getLoopTime()); + dDir=dDir.setMag(maxAccel*Updater.getLoopTime()); } //Debug.string("dDirAfter", dDir.toString()); //Debug.string("lastDir", lastDir.toString()); //Debug.string("newDir", (lastDir.add(dDir)).toString()); - Angle dAng=in.getTurn().subtract(lastTurn); - dAng=new Degrees(Utils.constrain(dAng.toDegrees(),-maxTurn.toDegrees()*Updater.getLoopTime(),maxTurn.toDegrees()*Updater.getLoopTime())); + double dAng=in.getTurn()-lastTurn; + dAng=Utils.constrain(dAng,-maxTurn*Updater.getLoopTime(),maxTurn*Updater.getLoopTime()); - DTTarget tgt= new DTTarget(lastDir.add(dDir),lastTurn.add(dAng)); + DTTarget tgt= new DTTarget(lastDir.add(dDir),lastTurn+dAng); lastDir=tgt.getDirection(); lastTurn=tgt.getTurn(); diff --git a/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java b/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java index dd3cce7..e99fadb 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java @@ -9,52 +9,33 @@ public class Deadzone implements Step{ - private Vector deadZone; - private Angle turnDeadZone; - - private boolean relative; + private double dirDeadZone; + private double turnDeadZone; public Deadzone() { this(.1,.1); } - public Deadzone(double x,double y) - { - this(x,y,true); - } - public Deadzone(double x,double y,boolean relative) - { - this(new XY(0,y),new Radians(x),relative); - } - public Deadzone(Vector dz,Angle turnDZ) + public Deadzone(double dirDZ,double turnDZ) { - this(dz,turnDZ,false); - } - public Deadzone(Vector dz,Angle turnDZ,boolean relative) - { - this.deadZone=dz; + this.dirDeadZone=dirDZ; this.turnDeadZone=turnDZ; - this.relative=relative; } @Override public DTTarget get(DTTarget in) { - if(relative) - { - relative=false; - } Vector tgtDir=in.getDirection(); - if(Math.abs(tgtDir.getX())maxSpeed) + double out=in.getTurn(); + if(Math.abs(out)>maxSpeed) { - out=new Radians(maxSpeed*(out.toRadians()>0?1:-1)); + out=maxSpeed*(out>0?1:-1); } return new DTTarget(in.getDirection(),out); } From 89d979c97c722cfb12d24cc083896dfbefa6bcaa Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 16 Sep 2017 22:34:30 -0400 Subject: [PATCH 48/71] Implement another method of registering modes, where auto modes extend the teleop mode. --- .../sprocket/core/Sprocket.java | 3 +- .../sprocket/frc/FRCRobot.java | 1 + .../sprocket/ftc/FTCDebug.java | 21 ++++++ .../sprocket/ftc/FTCMode.java | 65 ------------------- .../sprocket/ftc/FTCRobot.java | 48 +++++++++++++- .../sprocket/geometry/Degrees.java | 2 +- .../sprocket/geometry/Radians.java | 2 +- 7 files changed, 71 insertions(+), 71 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/ftc/FTCDebug.java delete mode 100644 src/org/montclairrobotics/sprocket/ftc/FTCMode.java diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index 982e09f..d1cf127 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -2,7 +2,6 @@ import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.DriveTrain; -import org.montclairrobotics.sprocket.frc.DashboardDebug; import org.montclairrobotics.sprocket.loop.DisabledUpdater; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updater; @@ -17,7 +16,7 @@ public class Sprocket{ public static DriveTrain driveTrain; - public static IDebugger debugger=new DashboardDebug(); + public static IDebugger debugger; private IRobot robot; diff --git a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java index d7b92fb..0d6c78f 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCRobot.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -20,6 +20,7 @@ public abstract class FRCRobot extends IterativeRobot implements IRobot,Updatabl public FRCRobot() { sprocket=new Sprocket(this); + sprocket.debugger=new DashboardDebug(); } @Override diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java new file mode 100644 index 0000000..7b8b616 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java @@ -0,0 +1,21 @@ +package org.montclairrobotics.sprocket.ftc; + +import org.montclairrobotics.sprocket.core.IDebugger; + +/** + * Created by Hymowitz on 9/16/2017. + */ + +public class FTCDebug implements IDebugger{ + + + @Override + public void debugStr(String key, String val) { + FTCRobot.telemetry.addData(key,val); + } + + @Override + public void debugNum(String key, double val) { + FTCRobot.telemetry.addData(key,val); + } +} diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java b/src/org/montclairrobotics/sprocket/ftc/FTCMode.java deleted file mode 100644 index 6cda500..0000000 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMode.java +++ /dev/null @@ -1,65 +0,0 @@ -package org.montclairrobotics.sprocket.ftc; - -import com.qualcomm.robotcore.eventloop.opmode.OpMode; - -import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.core.Sprocket; -import org.montclairrobotics.sprocket.core.Sprocket.MODE; -/** - * @author MHS Robotics - * This class is basically just a wrapper around iterative robot which all Sprocket - * robots must extend. - */ -public abstract class FTCMode extends OpMode { - - private static boolean FIRST=true; - public static Sprocket sprocket; - public static FTCRobot robot; - - public MODE mode; - public Action action; - - public FTCMode() - { - } - public FTCMode(Action action) - { - this.mode=MODE.AUTO; - this.action=action; - } - - //@Override - public void init() { - if(robot!=null) { - robot.ftcSetup(hardwareMap, gamepad1, gamepad2, telemetry); - if (FIRST) { - sprocket = new Sprocket(robot); - sprocket.initS(); - FIRST = false; - } - sprocket.startS(MODE.DISABLED); - } - } - - //@Override - public void init_loop() { - sprocket.disabledUpdateS(); - } - - //@Override - public void start() { - sprocket.currentAction=action; - sprocket.startS(mode); - } - - //@Override - public void loop() { - sprocket.updateS(); - - } - - //@Override - public void stop() { - sprocket.stopS(); - } -} diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index daa30a4..aa4629e 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -1,14 +1,22 @@ package org.montclairrobotics.sprocket.ftc; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.IRobot; +import org.montclairrobotics.sprocket.core.Sprocket; +import org.montclairrobotics.sprocket.core.Sprocket.MODE; -public abstract class FTCRobot extends FTCMode implements IRobot { - +public abstract class FTCRobot extends OpMode implements IRobot { + public static Sprocket sprocket; + public static FTCRobot robot; + + public MODE mode; + public Action action; public enum GAMEPAD {A,B}; //public Sprocket sprocket; //Dont need this because FTCMode has it! @@ -27,6 +35,42 @@ public static void setRobot() } }*/ + //@Override + public void init() { + if(robot==null) { + robot=this; + } + robot.ftcSetup(hardwareMap, gamepad1, gamepad2, telemetry); + if (sprocket==null) { + sprocket = new Sprocket(robot); + sprocket.initS(); + sprocket.debugger=new FTCDebug(); + } + sprocket.startS(MODE.DISABLED); + this.mode=mode.TELEOP; + } + //@Override + public void init_loop() { + sprocket.disabledUpdateS(); + } + + //@Override + public void start() { + sprocket.currentAction=action; + sprocket.startS(mode); + } + + //@Override + public void loop() { + sprocket.updateS(); + + } + + //@Override + public void stop() { + sprocket.stopS(); + } + public void ftcSetup(HardwareMap hardwareMap,Gamepad gamepad1,Gamepad gamepad2,Telemetry telemetry) { this.hardwareMap=hardwareMap; diff --git a/src/org/montclairrobotics/sprocket/geometry/Degrees.java b/src/org/montclairrobotics/sprocket/geometry/Degrees.java index 01cbafd..a6340c3 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Degrees.java +++ b/src/org/montclairrobotics/sprocket/geometry/Degrees.java @@ -66,7 +66,7 @@ public double divide(Angle x) { public String toString() { - return degrees+"°"; + return degrees+"deg"; } @Override diff --git a/src/org/montclairrobotics/sprocket/geometry/Radians.java b/src/org/montclairrobotics/sprocket/geometry/Radians.java index cab63fb..f7e816b 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Radians.java +++ b/src/org/montclairrobotics/sprocket/geometry/Radians.java @@ -67,7 +67,7 @@ public double divide(Angle x) { } public String toString() { - return toDegrees()+"°"; + return toDegrees()+"deg"; } @Override From c14cb74c0fd611268c27f64624f50dd574b765ab Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sun, 17 Sep 2017 10:23:51 -0400 Subject: [PATCH 49/71] Added @Override annotations to FTCRobot --- src/org/montclairrobotics/sprocket/ftc/FTCRobot.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index aa4629e..d177aa7 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -35,7 +35,7 @@ public static void setRobot() } }*/ - //@Override + @Override public void init() { if(robot==null) { robot=this; @@ -49,24 +49,24 @@ public void init() { sprocket.startS(MODE.DISABLED); this.mode=mode.TELEOP; } - //@Override + @Override public void init_loop() { sprocket.disabledUpdateS(); } - //@Override + @Override public void start() { sprocket.currentAction=action; sprocket.startS(mode); } - //@Override + @Override public void loop() { sprocket.updateS(); } - //@Override + @Override public void stop() { sprocket.stopS(); } From 78a9616c25ece0dbfa2ac693af7f1f1a627a4e8f Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 20 Sep 2017 20:05:55 -0400 Subject: [PATCH 50/71] Little cleanup to remove all of FRC from Sprocket --- .../sprocket/utils/PIDTuner.java | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java index 88f5bee..1828bc2 100644 --- a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java +++ b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java @@ -2,8 +2,6 @@ import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.Button; -import org.montclairrobotics.sprocket.frc.DashboardButton; -import org.montclairrobotics.sprocket.frc.DashboardInput; public class PIDTuner extends PID { @@ -11,7 +9,6 @@ public class PIDTuner extends PID private Input test; private Input apply; private Input run; - private DashboardInput realP,realI,realD; private Input cyclesPer10Sec; public PIDTuner(Input TempP,Input cyclesPer10Sec,Input test,Input apply,Input run) @@ -23,9 +20,9 @@ public PIDTuner(Input TempP,Input cyclesPer10Sec,Input this.cyclesPer10Sec=cyclesPer10Sec; this.run=run; - realP=new DashboardInput("PID Tuner P"); + /*realP=new DashboardInput("PID Tuner P"); realI=new DashboardInput("PID Tuner I"); - realD=new DashboardInput("PID Tuner D"); + realD=new DashboardInput("PID Tuner D");*/ new Button(apply).setAction(new Action(){ @Override @@ -54,9 +51,9 @@ public void disabled() { } - public PIDTuner(Input runButton) { + /*public PIDTuner(Input runButton) { this(new DashboardInput("Temp P"), new DashboardInput("Cycles/10sec"), new DashboardButton("Test"), new DashboardButton("Apply"), runButton); - } + }*/ public void recalculatePIDs() { if(cyclesPer10Sec.get()==0.0) @@ -64,10 +61,10 @@ public void recalculatePIDs() { return; } double period=0.1/cyclesPer10Sec.get(); - +/* realP.set(0.6*TempP.get()); realI.set(2/period); - realD.set(period/8); + realD.set(period/8);*/ } public PIDTuner copy() { @@ -82,7 +79,7 @@ public void update() } else if(run.get()) { - super.setPID(realP.get(), realI.get(), realD.get()); + //super.setPID(realP.get(), realI.get(), realD.get()); } else { From d3b4289afe4121f0e8dca7b7d73661d264292ded Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 20 Sep 2017 21:13:22 -0400 Subject: [PATCH 51/71] Added a Test package to complement the FRC and FTC packages, to enable testing of basic functionality on a computer rather than on a robot. This can be expanded in the future, eventually simulating part or all of the robot's functions. --- .../core/{IDebugger.java => IDebug.java} | 2 +- .../sprocket/core/Sprocket.java | 2 +- .../sprocket/frc/DashboardDebug.java | 4 +- .../sprocket/ftc/FTCDebug.java | 4 +- .../sprocket/ftc/FTCRobot.java | 2 +- .../sprocket/test/TestButton.java | 43 +++++++++++++++++++ .../sprocket/test/TestDebug.java | 17 ++++++++ .../sprocket/test/TestJoystick.java | 43 +++++++++++++++++++ .../sprocket/test/TestMotor.java | 18 ++++++++ .../sprocket/test/TestRobot.java | 37 ++++++++++++++++ 10 files changed, 165 insertions(+), 7 deletions(-) rename src/org/montclairrobotics/sprocket/core/{IDebugger.java => IDebug.java} (82%) create mode 100644 src/org/montclairrobotics/sprocket/test/TestButton.java create mode 100644 src/org/montclairrobotics/sprocket/test/TestDebug.java create mode 100644 src/org/montclairrobotics/sprocket/test/TestJoystick.java create mode 100644 src/org/montclairrobotics/sprocket/test/TestMotor.java create mode 100644 src/org/montclairrobotics/sprocket/test/TestRobot.java diff --git a/src/org/montclairrobotics/sprocket/core/IDebugger.java b/src/org/montclairrobotics/sprocket/core/IDebug.java similarity index 82% rename from src/org/montclairrobotics/sprocket/core/IDebugger.java rename to src/org/montclairrobotics/sprocket/core/IDebug.java index c055cc3..139cbbb 100644 --- a/src/org/montclairrobotics/sprocket/core/IDebugger.java +++ b/src/org/montclairrobotics/sprocket/core/IDebug.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.core; -public interface IDebugger { +public interface IDebug { public void debugStr(String key,String val); public void debugNum(String key,double val); } diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index d1cf127..c40a9f8 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -16,7 +16,7 @@ public class Sprocket{ public static DriveTrain driveTrain; - public static IDebugger debugger; + public static IDebug debugger; private IRobot robot; diff --git a/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java index 3978c20..c661758 100644 --- a/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java +++ b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java @@ -1,10 +1,10 @@ package org.montclairrobotics.sprocket.frc; -import org.montclairrobotics.sprocket.core.IDebugger; +import org.montclairrobotics.sprocket.core.IDebug; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class DashboardDebug implements IDebugger{ +public class DashboardDebug implements IDebug{ @Override public void debugStr(String key, String val) { diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java index 7b8b616..9294cb8 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java @@ -1,12 +1,12 @@ package org.montclairrobotics.sprocket.ftc; -import org.montclairrobotics.sprocket.core.IDebugger; +import org.montclairrobotics.sprocket.core.IDebug; /** * Created by Hymowitz on 9/16/2017. */ -public class FTCDebug implements IDebugger{ +public class FTCDebug implements IDebug{ @Override diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index d177aa7..9731d08 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -43,8 +43,8 @@ public void init() { robot.ftcSetup(hardwareMap, gamepad1, gamepad2, telemetry); if (sprocket==null) { sprocket = new Sprocket(robot); - sprocket.initS(); sprocket.debugger=new FTCDebug(); + sprocket.initS(); } sprocket.startS(MODE.DISABLED); this.mode=mode.TELEOP; diff --git a/src/org/montclairrobotics/sprocket/test/TestButton.java b/src/org/montclairrobotics/sprocket/test/TestButton.java new file mode 100644 index 0000000..614fd6d --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/TestButton.java @@ -0,0 +1,43 @@ +package org.montclairrobotics.sprocket.test; + +import org.montclairrobotics.sprocket.core.Button; +import org.montclairrobotics.sprocket.utils.Debug; + +public class TestButton extends Button{ + enum TEST {OFF,ON,PULSE,RANDOM}; + int i=0; + + String name; + boolean last=false; + private TEST test; + + public TestButton(String name,TEST test) + { + this.name=name; + this.test=test; + } + + public Boolean get() + { + boolean res=getNext(); + Debug.msg("Button "+name, res); + return res; + } + private boolean getNext() + { + i++; + switch(test) + { + case ON: + return true; + case PULSE: + return i/5%2>0; + case RANDOM: + if(Math.random()>0.8) + last=!last; + return last; + default: + return false; + } + } +} diff --git a/src/org/montclairrobotics/sprocket/test/TestDebug.java b/src/org/montclairrobotics/sprocket/test/TestDebug.java new file mode 100644 index 0000000..7de9f0b --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/TestDebug.java @@ -0,0 +1,17 @@ +package org.montclairrobotics.sprocket.test; + +import org.montclairrobotics.sprocket.core.IDebug; + +public class TestDebug implements IDebug{ + + @Override + public void debugStr(String key, String val) { + System.out.println(key+":"+val); + } + + @Override + public void debugNum(String key, double val) { + debugStr(key,""+val); + } + +} diff --git a/src/org/montclairrobotics/sprocket/test/TestJoystick.java b/src/org/montclairrobotics/sprocket/test/TestJoystick.java new file mode 100644 index 0000000..30e8360 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/TestJoystick.java @@ -0,0 +1,43 @@ +package org.montclairrobotics.sprocket.test; + +import org.montclairrobotics.sprocket.core.IJoystick; + +public class TestJoystick implements IJoystick{ + enum TEST {ZERO,CIRCLE,RANDOM}; + int i=0; + + private TEST test; + + public TestJoystick(TEST test) + { + this.test=test; + } + + @Override + public double getX() { + i++; + switch(test) + { + case CIRCLE: + return Math.cos(i*Math.PI/4); + case RANDOM: + return Math.random()*2-1; + default: + return 0; + } + } + + @Override + public double getY() { + switch(test) + { + case CIRCLE: + return Math.sin(i*Math.PI/4); + case RANDOM: + return Math.random()*2-1; + default: + return 0; + } + } + +} diff --git a/src/org/montclairrobotics/sprocket/test/TestMotor.java b/src/org/montclairrobotics/sprocket/test/TestMotor.java new file mode 100644 index 0000000..a0f63a6 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/TestMotor.java @@ -0,0 +1,18 @@ +package org.montclairrobotics.sprocket.test; + +import org.montclairrobotics.sprocket.core.IMotor; +import org.montclairrobotics.sprocket.utils.Debug; + +public class TestMotor implements IMotor{ + + private String name; + public TestMotor(String name) + { + this.name=name; + } + @Override + public void set(double power) { + Debug.msg("Motor "+name, power); + } + +} diff --git a/src/org/montclairrobotics/sprocket/test/TestRobot.java b/src/org/montclairrobotics/sprocket/test/TestRobot.java new file mode 100644 index 0000000..8ce96a5 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/TestRobot.java @@ -0,0 +1,37 @@ +package org.montclairrobotics.sprocket.test; + +import org.montclairrobotics.sprocket.core.IRobot; +import org.montclairrobotics.sprocket.core.Sprocket; +import org.montclairrobotics.sprocket.core.Sprocket.MODE; +import org.montclairrobotics.sprocket.utils.Debug; + +public abstract class TestRobot implements IRobot{ + + Sprocket sprocket; + + public TestRobot(MODE mode, int time) + { + sprocket=new Sprocket(this); + sprocket.debugger=new TestDebug(); + sprocket.initS(); + sprocket.startS(MODE.DISABLED); + for(int i=0;i Date: Wed, 20 Sep 2017 22:00:01 -0400 Subject: [PATCH 52/71] Added a test bot, and actually used it to fix a bug (!!!wow!!11!!) --- .../sprocket/core/Sprocket.java | 2 + .../sprocket/drive/DriveTrain.java | 2 +- .../sprocket/loop/DisabledUpdater.java | 2 +- .../sprocket/motors/Module.java | 2 +- .../sprocket/test/SimpleTest.java | 63 +++++++++++++++++++ .../sprocket/test/TestRobot.java | 6 +- 6 files changed, 71 insertions(+), 6 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/test/SimpleTest.java diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index c40a9f8..f58a643 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -5,6 +5,7 @@ import org.montclairrobotics.sprocket.loop.DisabledUpdater; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.Input; @@ -64,6 +65,7 @@ public final void startS(MODE mode) default: break; } + Debug.msg("MODE", mode); curMode=mode; if(currentAction!=null) { diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java index 4e65c83..3fa323f 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java @@ -37,7 +37,7 @@ public void update() { DTTarget target = new DTTarget(tgtDir,tgtTurn); Debug.string("DriveTrain INPUT:",target.toString()); target=pipeline.get(target); - Debug.string("DriveTrain PIPELINE OUTPUT:",target.toString()); + Debug.string("DriveTrain OUTPUT:",target.toString()); mapper.map(target, modules); } diff --git a/src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java b/src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java index 12ed2c8..6a6774e 100644 --- a/src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java +++ b/src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java @@ -3,7 +3,7 @@ import java.util.ArrayList; public class DisabledUpdater { - public static ArrayList updatables; + public static ArrayList updatables=new ArrayList(); public static void add(Updatable u) { updatables.add(u); diff --git a/src/org/montclairrobotics/sprocket/motors/Module.java b/src/org/montclairrobotics/sprocket/motors/Module.java index 5b5f248..3834d3a 100644 --- a/src/org/montclairrobotics/sprocket/motors/Module.java +++ b/src/org/montclairrobotics/sprocket/motors/Module.java @@ -74,7 +74,7 @@ public Module(IMotor... motors) public void set(double val) { power=val; - Debug.msg("Modules running", "yay"); + //Debug.msg("Modules running", "yay"); if(inputType == MotorInputType.SPEED) { Debug.msg("motordebug", "Using encoders"); diff --git a/src/org/montclairrobotics/sprocket/test/SimpleTest.java b/src/org/montclairrobotics/sprocket/test/SimpleTest.java new file mode 100644 index 0000000..b3cc3ca --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/SimpleTest.java @@ -0,0 +1,63 @@ +package org.montclairrobotics.sprocket.test; + +import org.montclairrobotics.sprocket.control.SquaredDriveInput; +import org.montclairrobotics.sprocket.core.Sprocket.MODE; +import org.montclairrobotics.sprocket.drive.DriveModule; +import org.montclairrobotics.sprocket.drive.DriveTrain; +import org.montclairrobotics.sprocket.drive.TankMapper; +import org.montclairrobotics.sprocket.geometry.XY; +import org.montclairrobotics.sprocket.test.TestJoystick.TEST; + +public class SimpleTest extends TestRobot{ + + public SimpleTest() + { + super(MODE.TELEOP, 8); + } + + @Override + public void setup() { + DriveTrain dt=new DriveTrain( + new DriveModule(new XY(-1,0),new XY(0,-1),new TestMotor("Left")), + new DriveModule(new XY( 1,0),new XY(0, 1),new TestMotor("Right")) + ); + dt.setDefaultInput(new SquaredDriveInput(new TestJoystick(TEST.CIRCLE))); + dt.setMapper(new TankMapper()); + } + + @Override + public void start(MODE mode) { + // TODO Auto-generated method stub + + } + + @Override + public void stop() { + // TODO Auto-generated method stub + + } + + @Override + public void update() { + // TODO Auto-generated method stub + + } + + @Override + public void disabledUpdate() { + // TODO Auto-generated method stub + + } + + @Override + public void debugs() { + // TODO Auto-generated method stub + + } + + + public static void main(String[] args) + { + new SimpleTest(); + } +} diff --git a/src/org/montclairrobotics/sprocket/test/TestRobot.java b/src/org/montclairrobotics/sprocket/test/TestRobot.java index 8ce96a5..de38ed5 100644 --- a/src/org/montclairrobotics/sprocket/test/TestRobot.java +++ b/src/org/montclairrobotics/sprocket/test/TestRobot.java @@ -17,19 +17,19 @@ public TestRobot(MODE mode, int time) sprocket.startS(MODE.DISABLED); for(int i=0;i Date: Wed, 20 Sep 2017 22:44:06 -0400 Subject: [PATCH 53/71] Fixing some inheritance --- src/org/montclairrobotics/sprocket/ftc/FTCButton.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java index c948a53..02d04b1 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java @@ -2,10 +2,10 @@ import com.qualcomm.robotcore.hardware.Gamepad; +import org.montclairrobotics.sprocket.core.Button; import org.montclairrobotics.sprocket.ftc.FTCRobot.GAMEPAD; -import org.montclairrobotics.sprocket.utils.Input; -public class FTCButton implements Input{ +public class FTCButton extends Button { enum BUTTON { a, b, From 0d4b1002822ab54c119f3c4fafcc1a49e57a54a3 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 20 Sep 2017 22:55:57 -0400 Subject: [PATCH 54/71] Small bugfix; a variable was being overwritten when it shouldn't have been. Also; SPROCKET RUNS ON FTC!!! --- .../sprocket/ftc/FTCButton.java | 6 +++--- .../montclairrobotics/sprocket/ftc/FTCDebug.java | 4 ++-- .../sprocket/ftc/FTCJoystick.java | 4 ++-- .../montclairrobotics/sprocket/ftc/FTCMotor.java | 2 +- .../montclairrobotics/sprocket/ftc/FTCRobot.java | 16 ++++++++-------- .../sprocket/ftc/LightSensor.java | 2 +- 6 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java index 02d04b1..a9b64e0 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCButton.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java @@ -6,7 +6,7 @@ import org.montclairrobotics.sprocket.ftc.FTCRobot.GAMEPAD; public class FTCButton extends Button { - enum BUTTON { + public enum BUTTON { a, b, x, @@ -35,11 +35,11 @@ public FTCButton(GAMEPAD gamepad,BUTTON button) { if(gamepad==GAMEPAD.A) { - this.gamepad=FTCRobot.gamepad1; + this.gamepad=FTCRobot.ftcGamepad1; } else { - this.gamepad=FTCRobot.gamepad2; + this.gamepad=FTCRobot.ftcGamepad2; } this.button=button; } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java index 9294cb8..4a52050 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java @@ -11,11 +11,11 @@ public class FTCDebug implements IDebug{ @Override public void debugStr(String key, String val) { - FTCRobot.telemetry.addData(key,val); + FTCRobot.ftcTelemetry.addData(key,val); } @Override public void debugNum(String key, double val) { - FTCRobot.telemetry.addData(key,val); + FTCRobot.ftcTelemetry.addData(key,val); } } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java index fe30d70..d943e41 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java @@ -14,11 +14,11 @@ public FTCJoystick(GAMEPAD gamepad,STICK stick) { if(gamepad==GAMEPAD.A) { - this.gamepad=FTCRobot.gamepad1; + this.gamepad=FTCRobot.ftcGamepad1; } else { - this.gamepad=FTCRobot.gamepad2; + this.gamepad=FTCRobot.ftcGamepad2; } this.stick=stick; } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java index f48274d..b5cedf1 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java @@ -10,7 +10,7 @@ public class FTCMotor implements IMotor{ public FTCMotor(String motorID) { - DcMotor motor=FTCRobot.hardwareMap.dcMotor.get(motorID); + DcMotor motor=FTCRobot.ftcHardwareMap.dcMotor.get(motorID); motor.setPower(0); motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index 9731d08..f872126 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -21,10 +21,10 @@ public abstract class FTCRobot extends OpMode implements IRobot { public enum GAMEPAD {A,B}; //public Sprocket sprocket; //Dont need this because FTCMode has it! - public static HardwareMap hardwareMap; - public static Gamepad gamepad1; - public static Gamepad gamepad2; - public static Telemetry telemetry; + public static HardwareMap ftcHardwareMap; + public static Gamepad ftcGamepad1; + public static Gamepad ftcGamepad2; + public static Telemetry ftcTelemetry; /* public static void setRobot() @@ -73,9 +73,9 @@ public void stop() { public void ftcSetup(HardwareMap hardwareMap,Gamepad gamepad1,Gamepad gamepad2,Telemetry telemetry) { - this.hardwareMap=hardwareMap; - this.gamepad1=gamepad1; - this.gamepad2=gamepad2; - this.telemetry=telemetry; + this.ftcHardwareMap=hardwareMap; + this.ftcGamepad1=gamepad1; + this.ftcGamepad2=gamepad2; + this.ftcTelemetry=telemetry; } } diff --git a/src/org/montclairrobotics/sprocket/ftc/LightSensor.java b/src/org/montclairrobotics/sprocket/ftc/LightSensor.java index 2846fb7..99ec268 100644 --- a/src/org/montclairrobotics/sprocket/ftc/LightSensor.java +++ b/src/org/montclairrobotics/sprocket/ftc/LightSensor.java @@ -7,7 +7,7 @@ public class LightSensor implements Input{ public LightSensor(String sensorID) { - sensor=FTCRobot.hardwareMap.lightSensor.get("lightGround"); + sensor=FTCRobot.ftcHardwareMap.lightSensor.get("lightGround"); } public void enableLed(boolean enable) { From 8024eb33f1499cc981498276f14efeac98e46956 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 20 Sep 2017 23:02:19 -0400 Subject: [PATCH 55/71] One small infinite loop actually when it is disabled; now it should be good. --- src/org/montclairrobotics/sprocket/core/IRobot.java | 4 ++-- src/org/montclairrobotics/sprocket/core/Sprocket.java | 4 ++-- src/org/montclairrobotics/sprocket/test/SimpleTest.java | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/core/IRobot.java b/src/org/montclairrobotics/sprocket/core/IRobot.java index 0a20f8e..fc4ff4f 100644 --- a/src/org/montclairrobotics/sprocket/core/IRobot.java +++ b/src/org/montclairrobotics/sprocket/core/IRobot.java @@ -6,8 +6,8 @@ public interface IRobot extends Updatable{ //Stuff user overrides public void setup(); - public void start(Sprocket.MODE mode); - public void stop(); + public void enable(Sprocket.MODE mode); + public void disable(); public void update(); public void disabledUpdate(); public void debugs(); diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index f58a643..4f5f9a2 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -71,7 +71,7 @@ public final void startS(MODE mode) { currentAction.start(); } - robot.start(mode); + robot.enable(mode); } public final void updateS() { @@ -85,7 +85,7 @@ public final void stopS() currentAction.stop(); } curMode=MODE.DISABLED; - robot.stop(); + robot.disable(); } public final void disabledUpdateS() { diff --git a/src/org/montclairrobotics/sprocket/test/SimpleTest.java b/src/org/montclairrobotics/sprocket/test/SimpleTest.java index b3cc3ca..480b9eb 100644 --- a/src/org/montclairrobotics/sprocket/test/SimpleTest.java +++ b/src/org/montclairrobotics/sprocket/test/SimpleTest.java @@ -26,13 +26,13 @@ public void setup() { } @Override - public void start(MODE mode) { + public void enable(MODE mode) { // TODO Auto-generated method stub } @Override - public void stop() { + public void disable() { // TODO Auto-generated method stub } From 026fecc82a40fb0e84702bb2beb9cc0682cf06ac Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Wed, 20 Sep 2017 23:17:16 -0400 Subject: [PATCH 56/71] Added run to position methods to FTCMotor --- .../sprocket/ftc/FTCMotor.java | 40 +++++++++++++++++-- 1 file changed, 37 insertions(+), 3 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java index b5cedf1..e90b4cc 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java @@ -7,17 +7,51 @@ public class FTCMotor implements IMotor{ private DcMotor motor; + private double zeroPos=0; public FTCMotor(String motorID) { DcMotor motor=FTCRobot.ftcHardwareMap.dcMotor.get(motorID); + setMode(DcMotor.RunMode.RUN_USING_ENCODER); + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + public FTCMotor setMode(DcMotor.RunMode mode) + { + motor.setMode(mode); motor.setPower(0); - motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - + return this; + } + public FTCMotor setZeroPowerBehavior(DcMotor.ZeroPowerBehavior b) + { + motor.setZeroPowerBehavior(b); + return this; } @Override public void set(double power) { motor.setPower(power); } + public void setTargetPosition(double pos) + { + motor.setTargetPosition((int)(pos+0.5-zeroPos)); + } + public void resetZeroPos() + { + zeroPos=getCurrentPos(); + } + public void resetZeroPos(double currentVal) + { + zeroPos=getCurrentPosRaw()-currentVal; + } + public int getCurrentPosRaw() + { + return motor.getCurrentPosition(); + } + public double getCurrentPos() + { + return motor.getCurrentPosition()-zeroPos; + } + public void setZeroPos(double zeroPos) + { + this.zeroPos=zeroPos; + } } From 80d0911ab8c11a76c1c14eeb339382eef56331e5 Mon Sep 17 00:00:00 2001 From: Garrett Burroughs Date: Mon, 25 Sep 2017 15:36:24 -0400 Subject: [PATCH 57/71] Fixed NPE in FTCMotor --- src/org/montclairrobotics/sprocket/ftc/FTCMotor.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java index e90b4cc..fcd745c 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java @@ -11,7 +11,7 @@ public class FTCMotor implements IMotor{ public FTCMotor(String motorID) { - DcMotor motor=FTCRobot.ftcHardwareMap.dcMotor.get(motorID); + motor=FTCRobot.ftcHardwareMap.dcMotor.get(motorID); setMode(DcMotor.RunMode.RUN_USING_ENCODER); setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } From 3e88e4a9fef467052377f42ee6ce80aa1e874161 Mon Sep 17 00:00:00 2001 From: Jack Hymowitz Date: Thu, 28 Sep 2017 22:00:21 -0400 Subject: [PATCH 58/71] Auto modes will not work the way they were coded; this idea should work. I am using the online editor, so this might not be the exact syntax. --- .../sprocket/ftc/FTCRobot.java | 35 ++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index f872126..954d9c7 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -10,6 +10,33 @@ import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.core.Sprocket.MODE; +/** + +The main issue with this class is the way it has to interact with the FTC library. + +The correct usage should be to extend this class with a Robot class, which has an @Teleop annotation +Then, auto modes should extend that, ovveride only the autoInit function, and have @Autonomous annotations +In this method, "action" variable should be set with the proper AutoMode + +Potential issues arise because when different automodes are started, different objects are created. +This is solved by running sprocket functions through the Sprocket class. +In addition, only the first Robot that is created is used for user code. + +The net result: +The FTC Library calls: +The current running opmode object, which calls: +The sprocket class, which calles sprocket functionality and: +The first opmode object which has been created, and any user functions that have been created there (this is the robot). +The init functions will only be called once (probably); they are equivalent to robot init funcions + +To make this work properly: +Leave all constructors blank; use the provided init functions +Make all instance variables static (that are used by autonomous functions, at least) +Do not override anything in auto modes besided autoInit +Basically dont mess with much, it is very delicate. + +*/ + public abstract class FTCRobot extends OpMode implements IRobot { public static Sprocket sprocket; @@ -47,8 +74,14 @@ public void init() { sprocket.initS(); } sprocket.startS(MODE.DISABLED); - this.mode=mode.TELEOP; + this.mode=mode.AUTONOMOUS; + autoInit(); } + + public void autoInit() + { + this.mode=mode.TELEOP; + } @Override public void init_loop() { sprocket.disabledUpdateS(); From 042e47ce0cc89685bed73c2a988c5ff0ff83ef34 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Wed, 4 Oct 2017 15:20:59 -0400 Subject: [PATCH 59/71] Fixed a typo with the robot mode --- src/org/montclairrobotics/sprocket/ftc/FTCRobot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index 954d9c7..cc2066d 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -74,7 +74,7 @@ public void init() { sprocket.initS(); } sprocket.startS(MODE.DISABLED); - this.mode=mode.AUTONOMOUS; + this.mode=mode.AUTO; autoInit(); } From c7cf37ad4e79b1a384beb4367a46cee26b487300 Mon Sep 17 00:00:00 2001 From: Garrett Burroughs Date: Wed, 4 Oct 2017 16:17:02 -0400 Subject: [PATCH 60/71] Added 'isCloseTo' methods for FTCMotor --- src/org/montclairrobotics/sprocket/ftc/FTCMotor.java | 6 ++++++ src/org/montclairrobotics/sprocket/ftc/FTCRobot.java | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java index fcd745c..08d7060 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java @@ -54,4 +54,10 @@ public void setZeroPos(double zeroPos) { this.zeroPos=zeroPos; } + public boolean isCloseTo(double pos, double tolerance){ + return Math.abs(motor.getCurrentPosition() - pos) < tolerance; + } + public boolean isCloseTo(double pos){ + return isCloseTo(pos, 30); + } } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index 954d9c7..cc2066d 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -74,7 +74,7 @@ public void init() { sprocket.initS(); } sprocket.startS(MODE.DISABLED); - this.mode=mode.AUTONOMOUS; + this.mode=mode.AUTO; autoInit(); } From 329f21956e174df1816077108947e3d4a0e834b7 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Wed, 4 Oct 2017 16:17:23 -0400 Subject: [PATCH 61/71] Updated DriveTrainBuilder for 0.3 --- .../sprocket/drive/DriveTrainBuilder.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java index f53b316..0aee6b4 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java @@ -6,8 +6,11 @@ import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Polar; import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.motors.Module; +import org.montclairrobotics.sprocket.motors.SEncoder; import org.montclairrobotics.sprocket.pipeline.Pipeline; import org.montclairrobotics.sprocket.pipeline.Step; +import org.montclairrobotics.sprocket.utils.PID; import java.util.ArrayList; @@ -38,15 +41,15 @@ public DriveTrainBuilder addDriveModule(DriveModule module) { modules.add(module); return this; } - /* - public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle force, SEncoder enc, PID pid) { - modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, new FRCMotor(motor))); + + public DriveTrainBuilder addWheel(IMotor motor, Vector offset, Angle force, SEncoder enc, PID pid) { + modules.add(new DriveModule(offset, new Polar(1, force),enc,pid,Module.MotorInputType.SPEED, motor)); return this; } - public DriveTrainBuilder addWheel(SpeedController motor, Vector offset, Angle force) { + public DriveTrainBuilder addWheel(IMotor motor, Vector offset, Angle force) { return addWheel(motor, offset, force, null, null); - }*/ + } public DriveTrainBuilder addWheels(Vector offset, Angle force, IMotor... motors) { From aa0038d4d88a5cf63aaa42ad57c9c8d4ce397af7 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Sat, 14 Oct 2017 15:41:50 -0400 Subject: [PATCH 62/71] Created my own generic mapper for fun! --- .../sprocket/drive/GenericMapper.java | 74 ++++++++++--------- 1 file changed, 41 insertions(+), 33 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java index 2bbbaa2..9206448 100644 --- a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java @@ -1,44 +1,52 @@ package org.montclairrobotics.sprocket.drive; + import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Polar; -import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; -public class GenericMapper implements DTMapper{ +public class GenericMapper implements DTMapper { @Override public void map(DTTarget driveTarget, DriveModule[] driveModules) { - Vector tgtDir=driveTarget.getDirection(); - double tgtTurn=driveTarget.getTurn(); - for(DriveModule module:driveModules) - { - tgtTurn-=getTorque(module.getOffset(),module.getForce(),tgtDir); - } - for(DriveModule module:driveModules) - { - module.set(inverseDot(module.getForce(),tgtDir.add( - new Polar(tgtTurn*module.getOffset().getMagnitude(),module.getOffset().getAngle().add(Angle.QUARTER))))); + //Stores parts of target in local variables for convenience + Vector dir = driveTarget.getDirection(); + double turn = driveTarget.getTurn(); + + //The motor power will be scaled to the highest of either the directional input or the turn input + double power = dir.getMagnitude() > Math.abs(turn) ? dir.getMagnitude() : Math.abs(turn); + + //Calculates how much each component should be weighted in the final vector + double totalInput = dir.getMagnitude() + Math.abs(turn); + double dirWeight = dir.getMagnitude()/totalInput; + double turnWeight = Math.abs(turn)/totalInput; + + dir = dir.normalize().scale(dirWeight); //Scales direction to appropriate weighting + + //Creates variables for scaling the motor powers + double[] powers = new double[driveModules.length]; + double maxPower = 0.0; + //Calculating torque factor for each drive module, projecting that onto the motor + for(int i = 0; i < driveModules.length; i++) { + DriveModule module = driveModules[i]; //Gets drive module from array + Vector torqueVector = module.getOffset().rotate(Angle.QUARTER); //Finds the vector where force applied creates a CW turn + torqueVector = torqueVector.normalize().scale(turnWeight); //Scales it to the appropriate weighting + + Vector moduleVec = dir.add(torqueVector); //Adds the torque vector to the directional vector + moduleVec = moduleVec.normalize(); //Normalises the vector, magnitude of resultant vector cannot affect the dot product + double dot = moduleVec.dotProduct(module.getForce()); //Projects target vector onto the motor + //Searches for largest dot product + if(Math.abs(dot) > maxPower) { + maxPower = Math.abs(dot); + powers[i] = dot; + } } - } - - public static double getTorque(Vector offset,Vector force,Vector target) - { - if(offset.getMagnitude()==0) - return 0; - return inverseDot(force,target)/offset.getMagnitude(); - } - /* - * If a dot b = |c| and c is || to b, given a and c returns b - */ - public static double inverseDot(Vector force,Vector target) - { - Angle diff=force.angleBetween(target); - double degTo90=Math.abs(Math.abs(diff.toDegrees())-90); - if(degTo90<30) - { - return target.getMagnitude()*degTo90/15; + + //Scaling + for(int i = 0; i < driveModules.length; i++) { + powers[i] = (powers[i]/maxPower) * power; //Scales each power relative to the maximum power, and then to the desired maximum motor power + driveModules[i].set(powers[i]); } - return target.getMagnitude()/diff.cos(); + } -} + +} \ No newline at end of file From 396f1d96e3796018c60420ee3900044aeeaf4489 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Sat, 14 Oct 2017 16:51:39 -0400 Subject: [PATCH 63/71] Added code to handle motors of different powers --- .../sprocket/drive/GenericMapper.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java index 9206448..9bf4d3c 100644 --- a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java @@ -25,6 +25,10 @@ public void map(DTTarget driveTarget, DriveModule[] driveModules) { //Creates variables for scaling the motor powers double[] powers = new double[driveModules.length]; double maxPower = 0.0; + + //Storing the lowest motor force so the motor can scale appropriately + double lowestForce = Double.MAX_VALUE; + //Calculating torque factor for each drive module, projecting that onto the motor for(int i = 0; i < driveModules.length; i++) { DriveModule module = driveModules[i]; //Gets drive module from array @@ -34,16 +38,23 @@ public void map(DTTarget driveTarget, DriveModule[] driveModules) { Vector moduleVec = dir.add(torqueVector); //Adds the torque vector to the directional vector moduleVec = moduleVec.normalize(); //Normalises the vector, magnitude of resultant vector cannot affect the dot product double dot = moduleVec.dotProduct(module.getForce()); //Projects target vector onto the motor + //Searches for largest dot product if(Math.abs(dot) > maxPower) { maxPower = Math.abs(dot); powers[i] = dot; } + + //Searches for least powerful motor for scaling + if(driveModules[i].getForce().getMagnitude() < lowestForce) { + lowestForce = driveModules[i].getForce().getMagnitude(); + } } //Scaling for(int i = 0; i < driveModules.length; i++) { powers[i] = (powers[i]/maxPower) * power; //Scales each power relative to the maximum power, and then to the desired maximum motor power + powers[i] = powers[i] * (lowestForce/driveModules[i].getForce().getMagnitude()); //Scales to account for imbalanced motor powers driveModules[i].set(powers[i]); } From 0c8b8fe42058c45d1555bdf03de9636f6be258be Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Sat, 14 Oct 2017 16:54:11 -0400 Subject: [PATCH 64/71] Motor force needed to be normalised for previous commit. --- src/org/montclairrobotics/sprocket/drive/GenericMapper.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java index 9bf4d3c..d352635 100644 --- a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java @@ -37,7 +37,7 @@ public void map(DTTarget driveTarget, DriveModule[] driveModules) { Vector moduleVec = dir.add(torqueVector); //Adds the torque vector to the directional vector moduleVec = moduleVec.normalize(); //Normalises the vector, magnitude of resultant vector cannot affect the dot product - double dot = moduleVec.dotProduct(module.getForce()); //Projects target vector onto the motor + double dot = moduleVec.dotProduct(module.getForce().normalize()); //Projects target vector onto the motor //Searches for largest dot product if(Math.abs(dot) > maxPower) { From b26cc9c802af04a58da47bb4c36187fd3980575c Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Sat, 14 Oct 2017 17:12:41 -0400 Subject: [PATCH 65/71] Added a torque correction for wheels with different offsets --- .../sprocket/drive/GenericMapper.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java index d352635..9fa2503 100644 --- a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java @@ -29,11 +29,21 @@ public void map(DTTarget driveTarget, DriveModule[] driveModules) { //Storing the lowest motor force so the motor can scale appropriately double lowestForce = Double.MAX_VALUE; + //Storing lowest offset for torque correction + double lowestOffset = Double.MAX_VALUE; + + //Searching for lowest offset + for(int i = 0; i < driveModules.length; i++) { + if(driveModules[i].getOffset().getMagnitude() < lowestOffset) { + lowestOffset = driveModules[i].getOffset().getMagnitude(); + } + } + //Calculating torque factor for each drive module, projecting that onto the motor for(int i = 0; i < driveModules.length; i++) { DriveModule module = driveModules[i]; //Gets drive module from array Vector torqueVector = module.getOffset().rotate(Angle.QUARTER); //Finds the vector where force applied creates a CW turn - torqueVector = torqueVector.normalize().scale(turnWeight); //Scales it to the appropriate weighting + torqueVector = torqueVector.normalize().scale(turnWeight * (lowestOffset/driveModules[i].getOffset().getMagnitude())); //Scales it to the appropriate weighting and corrects for wheel torque Vector moduleVec = dir.add(torqueVector); //Adds the torque vector to the directional vector moduleVec = moduleVec.normalize(); //Normalises the vector, magnitude of resultant vector cannot affect the dot product From 7c16e3815dc82563160f90542458543bc9109134 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Thu, 2 Nov 2017 09:29:17 -0400 Subject: [PATCH 66/71] Mecanum mapper that should work --- .../sprocket/drive/MecanumMapper.java | 42 ++++--------------- 1 file changed, 8 insertions(+), 34 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java index cb054ac..b001e88 100644 --- a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java @@ -1,49 +1,23 @@ package org.montclairrobotics.sprocket.drive; -import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Position; +import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.geometry.XY; public class MecanumMapper implements DTMapper { @Override public void map(DTTarget driveTarget, DriveModule[] driveModules) { + Vector dir = driveTarget.getDirection(); + double dirPower = dir.getMagnitude(); double turn = driveTarget.getTurn(); - for(DriveModule m : driveModules) { - double power = getPower(driveTarget.getDirection(), m.getForceAngle()); - - //Get vector for rotating - Vector turnVector; - if(m.getOffset() == Position.FL) { - turnVector = new XY(1, 1); - } else if(m.getOffset() == Position.FR) { - turnVector = new XY(1, -1); - } else if(m.getOffset() == Position.BL) { - turnVector = new XY(-1, 1); - } else if(m.getOffset() == Position.BR) { - turnVector = new XY(-1, -1); - } else { - turnVector = new XY(0, 0); - } - - //Scale it to the appropriate turn speed - turnVector.scale(turn); - double turnPower = getPower(turnVector, m.getForceAngle()); - - power += turnPower; - - //TODO: SCALING!!!!!!! - m.set(power); + for (DriveModule driveModule : driveModules) { + Vector offset = driveModule.getOffset(); + Vector turnVec = offset.rotate(new Degrees(90)).normalize().scale(turn / (dirPower + Math.abs(turn))); + Vector finalVec = dir.normalize().scale(dirPower / (dirPower + Math.abs(turn))).add(turnVec); + driveModule.set(finalVec.dotProduct(driveModule.getForce())); } } - - private static double getPower(Vector vector, Angle forceAngle) { - // Power = tY*csc(angle) + tX*sec(angle) - return vector.getY() * (1/Math.sin(forceAngle.toRadians())) + vector.getX() * (1/Math.sin(forceAngle.toRadians())); - } - } From 3bf4c815ac352de1121a421496217036ca6a1959 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Thu, 2 Nov 2017 09:50:04 -0400 Subject: [PATCH 67/71] Added scaling --- .../sprocket/drive/MecanumMapper.java | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java index b001e88..96fcc39 100644 --- a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java @@ -11,12 +11,26 @@ public void map(DTTarget driveTarget, DriveModule[] driveModules) { Vector dir = driveTarget.getDirection(); double dirPower = dir.getMagnitude(); double turn = driveTarget.getTurn(); + double motorPowers[] = new double[driveModules.length]; - for (DriveModule driveModule : driveModules) { + for (int i = 0; i < driveModules.length; i++) { + DriveModule driveModule = driveModules[i]; Vector offset = driveModule.getOffset(); Vector turnVec = offset.rotate(new Degrees(90)).normalize().scale(turn / (dirPower + Math.abs(turn))); Vector finalVec = dir.normalize().scale(dirPower / (dirPower + Math.abs(turn))).add(turnVec); - driveModule.set(finalVec.dotProduct(driveModule.getForce())); + motorPowers[i] = finalVec.dotProduct(driveModule.getForce()); + } + + double highestPower = Double.MIN_VALUE; + for(double p : motorPowers) { + if(Math.abs(p) > highestPower) { + highestPower = p; + } + } + + for(int i = 0; i < driveModules.length; i++) { + motorPowers[i] = motorPowers[i] / highestPower; + driveModules[i].set(motorPowers[i]); } } From 8b540793ca3f8aaf941c3bcc3b411b299007edb8 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Thu, 2 Nov 2017 10:08:38 -0400 Subject: [PATCH 68/71] Added comments and fixed a bug with scaling --- .../sprocket/drive/MecanumMapper.java | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java index 96fcc39..687c718 100644 --- a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java @@ -4,32 +4,49 @@ import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Vector; +/* +Second attempt to implement a mecanum mappper. This is essentially a simple generic +mapper which works only on symmetrical drivetrains as coding something specifically +for mecanum would probably end up being more complicated (since exact implementations +of a mecanum drivetrain can vary). + */ public class MecanumMapper implements DTMapper { @Override public void map(DTTarget driveTarget, DriveModule[] driveModules) { + //Setting up variables Vector dir = driveTarget.getDirection(); double dirPower = dir.getMagnitude(); double turn = driveTarget.getTurn(); double motorPowers[] = new double[driveModules.length]; + //Finding initial motor powers for (int i = 0; i < driveModules.length; i++) { DriveModule driveModule = driveModules[i]; - Vector offset = driveModule.getOffset(); + Vector offset = driveModule.getOffset(); //Storing the motor offset + //Rotating the offset 90 degrees to find the vector along which torque should be applied, + //then scaling the turn vector appropriately Vector turnVec = offset.rotate(new Degrees(90)).normalize().scale(turn / (dirPower + Math.abs(turn))); + //Scaling the directional vector, then adding it to the turn vector calculated Vector finalVec = dir.normalize().scale(dirPower / (dirPower + Math.abs(turn))).add(turnVec); + //Dotting it with the force vector of the motor to get the final power motorPowers[i] = finalVec.dotProduct(driveModule.getForce()); } + //Finding the highest power calculated double highestPower = Double.MIN_VALUE; for(double p : motorPowers) { if(Math.abs(p) > highestPower) { - highestPower = p; + highestPower = Math.abs(p); } } + //Finding the higher of either the directional vector or the turn vector + double maxPower = dirPower > Math.abs(turn) ? dirPower : Math.abs(turn); + for(int i = 0; i < driveModules.length; i++) { - motorPowers[i] = motorPowers[i] / highestPower; + //Scaling all motors first relative to that with the highest power, and then down to the max desired power + motorPowers[i] = (motorPowers[i] / highestPower)*maxPower; driveModules[i].set(motorPowers[i]); } } From 171d50f9b0d312c858efafbf559b0c31685f60f6 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Thu, 2 Nov 2017 23:35:53 -0400 Subject: [PATCH 69/71] Redid input system, and simplified UniversalMapper. Right now, turning is acting weird. --- .../sprocket/control/ArcadeDriveInput.java | 93 ++----------------- .../sprocket/control/BasicInput.java | 39 ++++++++ .../sprocket/control/ExponentDriveInput.java | 30 ------ .../control/FieldCentricDriveInput.java | 31 ++++--- .../sprocket/control/JoystickXAxis.java | 2 +- .../sprocket/control/JoystickYAxis.java | 2 +- .../sprocket/control/SquaredDriveInput.java | 35 ------- .../sprocket/core/IJoystick.java | 9 +- .../sprocket/drive/UniversalMapper.java | 35 +++++-- .../sprocket/drive/steps/PowerCurve.java | 48 ++++++++++ .../sprocket/drive/steps/Sensitivity.java | 47 ++++++++++ .../sprocket/drive/steps/Squared.java | 41 ++++++++ .../sprocket/frc/FRCJoystick.java | 7 ++ .../sprocket/geometry/Polar.java | 4 + .../sprocket/geometry/Vector.java | 1 + .../sprocket/geometry/VectorInputX.java | 20 ++++ .../sprocket/geometry/VectorInputY.java | 20 ++++ .../sprocket/geometry/XY.java | 4 + .../sprocket/test/MecanumTest.java | 73 +++++++++++++++ .../sprocket/test/SimpleTest.java | 4 +- .../sprocket/test/TestJoystick.java | 31 +++---- .../sprocket/utils/ZeroInput.java | 2 +- 22 files changed, 373 insertions(+), 205 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/control/BasicInput.java delete mode 100644 src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java delete mode 100644 src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java create mode 100644 src/org/montclairrobotics/sprocket/drive/steps/PowerCurve.java create mode 100644 src/org/montclairrobotics/sprocket/drive/steps/Sensitivity.java create mode 100644 src/org/montclairrobotics/sprocket/drive/steps/Squared.java create mode 100644 src/org/montclairrobotics/sprocket/geometry/VectorInputX.java create mode 100644 src/org/montclairrobotics/sprocket/geometry/VectorInputY.java create mode 100644 src/org/montclairrobotics/sprocket/test/MecanumTest.java diff --git a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java index c45bc71..05c6988 100644 --- a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java @@ -5,10 +5,13 @@ import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.geometry.VectorInputX; +import org.montclairrobotics.sprocket.geometry.VectorInputY; import org.montclairrobotics.sprocket.geometry.XY; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.utils.ZeroInput; /** @@ -17,94 +20,10 @@ * to determine turning speed and the Y axis for translation along the Y axis, * making this mapper unsuitable for any robot which translates on the X axis. */ -public class ArcadeDriveInput implements DTInput, Updatable { +public class ArcadeDriveInput extends BasicInput { - private IJoystick stick; - - private Vector dir; - private double turn; - private Vector raw; - - private double sensitivity = 1; - private double turnSensitivity = 1; - - /** - * Instantiates an ArcadeDriveInput with default scaling values - * @param stick The joystick that will be used for DriveTrain control - */ - public ArcadeDriveInput(IJoystick stick) { - this.stick = stick; - Updater.add(this, Priority.INPUT); - } - - /* - * Instantiates an ArcadeDriveInput which is scaled based off of manually - * inputted values. - * @param stick The joystick that will be used for DriveTrain control - * @param maxSpeed The maximum speed that the Joystick can tell the DriveTrain to go in units/sec - * @param maxTurnSpeed The maximum turning speed that the Joystick can tell the DriveTrain to go in units/sec - */ - /* - public ArcadeDriveInput(Joystick stick, Angle maxTurnSpeed) { - this.stick = stick; - this.maxTurn = maxTurnSpeed; - Updater.add(this, Priority.INPUT); - }*/ - - /** - * Sets the sensitivity of each Joystick axis. For example, if the direction - * sensitivity and turn sensitivity are both 0.5, both axises will have their - * raw inputs halved when they are sent to the DriveTrain. - * @param dir The sensitivity of the translation axis (Y-axis) - * @param turn The sensitvity of the turning axis (X-axis) - * @return itself for currying - */ - public ArcadeDriveInput setSensitivity(double dir, double turn) - { - turnSensitivity = turn; - sensitivity = dir; - return this; - } - - public void update() { - turn = getX()*turnSensitivity; - dir = new XY(0, getY()*sensitivity*-1); - raw=new XY(getX(),-getY()); - } - - public double getX() + public ArcadeDriveInput(IJoystick joystick) { - return stick.getX(); - } - public double getY() - { - return stick.getY(); - } - - /** - * @return The calculated direction for the DriveTrain - */ - public Vector getDirection() { - return dir; - } - - /** - * @return The calculated direction for the DriveTrain (shortcut for getDirection() ) - */ - public Vector getDir() - { - return getDirection();//I'm very lazy - } - - public Vector getRaw() - { - return raw; - } - - /** - * @return The calculated turning speed for the DriveTrain - */ - public double getTurn() { - return turn; + super(ZeroInput.ZERO_INPUT,new VectorInputY(joystick),new VectorInputX(joystick)); } } diff --git a/src/org/montclairrobotics/sprocket/control/BasicInput.java b/src/org/montclairrobotics/sprocket/control/BasicInput.java new file mode 100644 index 0000000..ebbf8e9 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/control/BasicInput.java @@ -0,0 +1,39 @@ +package org.montclairrobotics.sprocket.control; + +import org.montclairrobotics.sprocket.drive.DTInput; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.geometry.XY; +import org.montclairrobotics.sprocket.utils.Input; + +public class BasicInput implements DTInput{ + + private Input dir; + private Input turn; + + public BasicInput(Input dir,Input turn) + { + this.dir=dir; + this.turn=turn; + } + public BasicInput(Input dirX,Input dirY,Input turn) + { + this.dir=new Input(){ + + @Override + public Vector get() { + return new XY(dirX.get(),dirY.get()); + }}; + this.turn=turn; + } + + @Override + public Vector getDir() { + return dir.get(); + } + + @Override + public double getTurn() { + return turn.get(); + } + +} diff --git a/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java b/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java deleted file mode 100644 index 706e9ba..0000000 --- a/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.montclairrobotics.sprocket.control; - -import org.montclairrobotics.sprocket.core.IJoystick; - - -public class ExponentDriveInput extends ArcadeDriveInput -{ - - private double p; - - public ExponentDriveInput(IJoystick stick) { - this(stick,0.5); - // TODO Auto-generated constructor stub - } - public ExponentDriveInput(IJoystick stick,double p) { - super(stick); - this.p=p; - // TODO Auto-generated constructor stub - } - public double getX() - { - double x=super.getX(); - return p*Math.pow(x, 3)+(1-p)*x; - } - public double getY() - { - double y=super.getY(); - return p*Math.pow(y, 3)+(1-p)*y; - } -} diff --git a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java index bb40c3d..145871b 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -3,14 +3,18 @@ import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.core.IJoystick; import org.montclairrobotics.sprocket.core.Sprocket; +import org.montclairrobotics.sprocket.drive.DTInput; import org.montclairrobotics.sprocket.drive.steps.GyroCorrection; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.loop.Priority; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; import org.montclairrobotics.sprocket.utils.Input; import org.montclairrobotics.sprocket.utils.SmoothVectorInput; -public class FieldCentricDriveInput extends ArcadeDriveInput implements Action{ +public class FieldCentricDriveInput implements DTInput,Action,Updatable{ private GyroCorrection gyro; @@ -19,30 +23,25 @@ public class FieldCentricDriveInput extends ArcadeDriveInput implements Action{ private boolean forwards; private boolean rotToVector; + + private boolean enabled=false; private static final int SMOOTH_LEN=10; - public FieldCentricDriveInput(IJoystick stick,GyroCorrection gyro) + public FieldCentricDriveInput(Input stick,GyroCorrection gyro) { - this(stick,gyro,true); + this(stick,gyro,false); } - public FieldCentricDriveInput(IJoystick stick,GyroCorrection gyro,boolean rotToVector) + public FieldCentricDriveInput(Input stick,GyroCorrection gyro,boolean rotToVector) { - super(stick); this.gyro=gyro; this.rotToVector=rotToVector; - fieldInput=new SmoothVectorInput(SMOOTH_LEN,new Input(){ - - @Override - public Vector get() { - // TODO Auto-generated method stub - return getRaw(); - }}); + fieldInput=new SmoothVectorInput(SMOOTH_LEN,stick); + Updater.add(this, Priority.CONTROL); } @Override public void update() { - super.update(); field=fieldInput.get(); if(field.getMagnitude()>0.1) { @@ -60,7 +59,7 @@ public void update() */ @Override public Vector getDir() { - return robot.square(); + return robot; } /** @@ -69,7 +68,7 @@ public Vector getDir() { @Override public double getTurn() { - if(rotToVector&&field.getMagnitude()>0.1) + if(enabled&&rotToVector&&field.getMagnitude()>0.1) { if(forwards) { @@ -87,10 +86,12 @@ public double getTurn() { @Override public void start() { Sprocket.getMainDriveTrain().setTempInput(this); + enabled=true; } @Override public void stop() { Sprocket.getMainDriveTrain().useDefaultInput(); + enabled=false; } @Override public void enabled() { diff --git a/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java b/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java index d63267b..97d6eae 100644 --- a/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java +++ b/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java @@ -26,7 +26,7 @@ public JoystickXAxis(IJoystick stick) { */ @Override public Double get() { - return stick.getX(); + return stick.get().getX(); } } diff --git a/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java b/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java index 4e305a5..2e0a28d 100644 --- a/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java +++ b/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java @@ -26,7 +26,7 @@ public JoystickYAxis(IJoystick stick) { */ @Override public Double get() { - return -stick.getY(); + return -stick.get().getY(); } } diff --git a/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java b/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java deleted file mode 100644 index 115499c..0000000 --- a/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.montclairrobotics.sprocket.control; - -import org.montclairrobotics.sprocket.core.IJoystick; - - -public class SquaredDriveInput extends ArcadeDriveInput{ - - private double xSensitivity = 1.0; - private double ySensitivity = 1.0; - - public SquaredDriveInput(IJoystick stick) { - super(stick); - // TODO Auto-generated constructor stub - } - - public double getX() - { - double x=super.getX(); - return x*/*Math.abs(x)**/xSensitivity; - } - public double getY() - { - double y=super.getY(); - return y*Math.abs(y)*ySensitivity; - } - - public void setXSensitivity(double s) { - xSensitivity = s; - } - - public void setYSensitivity(double s) { - ySensitivity = s; - } - -} diff --git a/src/org/montclairrobotics/sprocket/core/IJoystick.java b/src/org/montclairrobotics/sprocket/core/IJoystick.java index c4901ca..d25fb0b 100644 --- a/src/org/montclairrobotics/sprocket/core/IJoystick.java +++ b/src/org/montclairrobotics/sprocket/core/IJoystick.java @@ -1,6 +1,9 @@ package org.montclairrobotics.sprocket.core; -public interface IJoystick { - public double getX(); - public double getY(); +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.utils.Input; + +public interface IJoystick extends Input{ + //public double getX(); + //public double getY(); } diff --git a/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java index 2ba5582..d3c7a60 100644 --- a/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java @@ -9,25 +9,40 @@ public class UniversalMapper implements DTMapper{ @Override public void map(DTTarget driveTarget, DriveModule[] driveModules) { if(driveModules.length==0) return; - Vector dir=driveTarget.getDirection().setMag(1); - double maxForce=0; - double torque=0; + Vector dir=driveTarget.getDirection(); + Vector normDir=dir.normalize(); + double turn=driveTarget.getTurn(); + double maxForce=0.1; + double maxTorque=0.1; + //double torque=0; for(DriveModule module:driveModules) { - double f=module.getForce().dotProduct(dir); - Vector fVec=module.getForce().setMag(f); - torque+=module.getOffset().rotate(Angle.QUARTER).setMag(1).dotProduct(fVec); + double f=module.getForce().dotProduct(normDir); if(f>maxForce) maxForce=f; + Vector offset=module.getOffset(); + if(offset.getMagnitude()>0) + { + double t=offset.setMag(1/offset.getMagnitude()).crossProduct(module.getForce()); + if (t>maxTorque) + maxTorque=t; + } } - double tgtTurn=driveTarget.getTurn()-torque; for(DriveModule module:driveModules) { double f=module.getForce().dotProduct(dir); Vector fVec=module.getForce().setMag(f/maxForce); - Vector tVec=module.getOffset().rotate(Angle.QUARTER).setMag(torque); - Vector vec=fVec.add(tVec); - module.set(vec.dotProduct(module.getForce())/module.getForce().getMagnitude()); + Vector offset=module.getOffset(); + if(offset.getMagnitude()>0) + { + double t=offset.setMag(1/offset.getMagnitude()).crossProduct(module.getForce()); + if(t>0) + { + Vector tVec=module.getOffset().rotate(Angle.QUARTER).setMag(t*turn/maxTorque); + fVec=fVec.add(tVec); + } + } + module.set(fVec.dotProduct(module.getForce())/module.getForce().getMagnitude()); } } diff --git a/src/org/montclairrobotics/sprocket/drive/steps/PowerCurve.java b/src/org/montclairrobotics/sprocket/drive/steps/PowerCurve.java new file mode 100644 index 0000000..575450e --- /dev/null +++ b/src/org/montclairrobotics/sprocket/drive/steps/PowerCurve.java @@ -0,0 +1,48 @@ +package org.montclairrobotics.sprocket.drive.steps; + +import org.montclairrobotics.sprocket.actions.Action; +import org.montclairrobotics.sprocket.drive.DTTarget; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.pipeline.Step; + +public class PowerCurve implements Step, Action{ + private double p; + private boolean enabled; + + public PowerCurve(double p) + { + this.p=p; + } + + @Override + public void start() { + // TODO Auto-generated method stub + enabled=true; + } + + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void stop() { + // TODO Auto-generated method stub + enabled=false; + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } + + @Override + public DTTarget get(DTTarget in) { + if(!enabled) return in; + Vector dir=in.getDirection(); + return new DTTarget(dir.scale(p*Math.pow(dir.getMagnitude(),2)+1-p),in.getTurn()); + } + +} diff --git a/src/org/montclairrobotics/sprocket/drive/steps/Sensitivity.java b/src/org/montclairrobotics/sprocket/drive/steps/Sensitivity.java new file mode 100644 index 0000000..2864dca --- /dev/null +++ b/src/org/montclairrobotics/sprocket/drive/steps/Sensitivity.java @@ -0,0 +1,47 @@ +package org.montclairrobotics.sprocket.drive.steps; + +import org.montclairrobotics.sprocket.actions.Action; +import org.montclairrobotics.sprocket.drive.DTTarget; +import org.montclairrobotics.sprocket.pipeline.Step; + +public class Sensitivity implements Step, Action{ + + public double dirScale,turnScale; + private boolean enabled=true; + + public Sensitivity(double dir,double turn) + { + this.dirScale=dir; + this.turnScale=turn; + } + @Override + public void start() { + // TODO Auto-generated method stub + enabled=true; + } + + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void stop() { + // TODO Auto-generated method stub + enabled=false; + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } + + @Override + public DTTarget get(DTTarget in) { + if(!enabled)return in; + return new DTTarget(in.getDirection().scale(dirScale),in.getTurn()*turnScale); + } + +} diff --git a/src/org/montclairrobotics/sprocket/drive/steps/Squared.java b/src/org/montclairrobotics/sprocket/drive/steps/Squared.java new file mode 100644 index 0000000..1c5356c --- /dev/null +++ b/src/org/montclairrobotics/sprocket/drive/steps/Squared.java @@ -0,0 +1,41 @@ +package org.montclairrobotics.sprocket.drive.steps; + +import org.montclairrobotics.sprocket.actions.Action; +import org.montclairrobotics.sprocket.drive.DTTarget; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.pipeline.Step; + +public class Squared implements Step, Action{ + private boolean enabled=true; + + @Override + public void start() { + // TODO Auto-generated method stub + enabled=true; + } + + @Override + public void enabled() { + // TODO Auto-generated method stub + + } + + @Override + public void stop() { + // TODO Auto-generated method stub + enabled=false; + } + + @Override + public void disabled() { + // TODO Auto-generated method stub + + } + + @Override + public DTTarget get(DTTarget in) { + if(!enabled)return in; + Vector dir=in.getDirection(); + return new DTTarget(dir.scale(dir.getMagnitude()),in.getTurn()); + } +} diff --git a/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java b/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java index b73c7a6..a3585cb 100644 --- a/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java @@ -1,7 +1,14 @@ package org.montclairrobotics.sprocket.frc; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.geometry.XY; + public class FRCJoystick extends edu.wpi.first.wpilibj.Joystick implements org.montclairrobotics.sprocket.core.IJoystick{ public FRCJoystick(int port) { super(port); } + public Vector get() + { + return new XY(getX(),getY()); + } } diff --git a/src/org/montclairrobotics/sprocket/geometry/Polar.java b/src/org/montclairrobotics/sprocket/geometry/Polar.java index 89e48bc..d78af55 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Polar.java +++ b/src/org/montclairrobotics/sprocket/geometry/Polar.java @@ -52,6 +52,10 @@ public Vector scale(double s) { public double dotProduct(Vector v) { return getX() * v.getX() + getY() * v.getY(); } + public double crossProduct(Vector v) + { + return getY() * v.getX() - getX() * v.getY(); + } @Override public Vector rotate(Angle a) { diff --git a/src/org/montclairrobotics/sprocket/geometry/Vector.java b/src/org/montclairrobotics/sprocket/geometry/Vector.java index 926d271..2312e97 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Vector.java +++ b/src/org/montclairrobotics/sprocket/geometry/Vector.java @@ -13,6 +13,7 @@ public interface Vector { Vector setMag(double mag); Vector normalize(); double dotProduct(Vector v); + double crossProduct(Vector v);//NOTE THAT THIS IS LEFT HANDED Vector rotate(Angle angle); Angle angleBetween(Vector c); Vector square(); diff --git a/src/org/montclairrobotics/sprocket/geometry/VectorInputX.java b/src/org/montclairrobotics/sprocket/geometry/VectorInputX.java new file mode 100644 index 0000000..c1517bf --- /dev/null +++ b/src/org/montclairrobotics/sprocket/geometry/VectorInputX.java @@ -0,0 +1,20 @@ +package org.montclairrobotics.sprocket.geometry; + +import org.montclairrobotics.sprocket.utils.Input; +import org.montclairrobotics.sprocket.utils.InputDouble; + +public class VectorInputX extends InputDouble{ + + private Input vec; + + public VectorInputX(Input vec) + { + this.vec=vec; + } + + @Override + public Double get() { + return vec.get().getX(); + } + +} diff --git a/src/org/montclairrobotics/sprocket/geometry/VectorInputY.java b/src/org/montclairrobotics/sprocket/geometry/VectorInputY.java new file mode 100644 index 0000000..c211723 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/geometry/VectorInputY.java @@ -0,0 +1,20 @@ +package org.montclairrobotics.sprocket.geometry; + +import org.montclairrobotics.sprocket.utils.Input; +import org.montclairrobotics.sprocket.utils.InputDouble; + +public class VectorInputY extends InputDouble{ + + private Input vec; + + public VectorInputY(Input vec) + { + this.vec=vec; + } + + @Override + public Double get() { + return vec.get().getY(); + } + +} diff --git a/src/org/montclairrobotics/sprocket/geometry/XY.java b/src/org/montclairrobotics/sprocket/geometry/XY.java index 4714bbd..c24871c 100644 --- a/src/org/montclairrobotics/sprocket/geometry/XY.java +++ b/src/org/montclairrobotics/sprocket/geometry/XY.java @@ -52,6 +52,10 @@ public Vector scale(double s) { public double dotProduct(Vector v) { return x * v.getX() + y * v.getY(); } + public double crossProduct(Vector v) + { + return y * v.getX() - x * v.getY();//LEFT HANDED + } @Override public Vector rotate(Angle a) { diff --git a/src/org/montclairrobotics/sprocket/test/MecanumTest.java b/src/org/montclairrobotics/sprocket/test/MecanumTest.java new file mode 100644 index 0000000..45e7dcc --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/MecanumTest.java @@ -0,0 +1,73 @@ +package org.montclairrobotics.sprocket.test; + +import org.montclairrobotics.sprocket.control.BasicInput; +import org.montclairrobotics.sprocket.core.Sprocket.MODE; +import org.montclairrobotics.sprocket.drive.DriveModule; +import org.montclairrobotics.sprocket.drive.DriveTrain; +import org.montclairrobotics.sprocket.drive.TankMapper; +import org.montclairrobotics.sprocket.drive.UniversalMapper; +import org.montclairrobotics.sprocket.geometry.XY; +import org.montclairrobotics.sprocket.test.TestJoystick.TEST; +import org.montclairrobotics.sprocket.utils.Input; +import org.montclairrobotics.sprocket.utils.ZeroInput; + +public class MecanumTest extends TestRobot{ + public MecanumTest() + { + super(MODE.TELEOP, 8); + } + + @Override + public void setup() { + DriveTrain dt=new DriveTrain( + new DriveModule(new XY(-1,-1),new XY(-1,1),new TestMotor("BackLeft")), + new DriveModule(new XY(1,-1),new XY(1,1),new TestMotor("BackRight")), + new DriveModule(new XY(-1,1),new XY(1,1),new TestMotor("FrontLeft")), + new DriveModule(new XY(1,1),new XY(-1,1),new TestMotor("FrontRight")) + ); + dt.setDefaultInput(new BasicInput(new TestJoystick(TEST.ZERO),new Input(){ + + @Override + public Double get() { + // TODO Auto-generated method stub + return -0.5; + }})); + dt.setMapper(new UniversalMapper()); + } + + @Override + public void enable(MODE mode) { + // TODO Auto-generated method stub + + } + + @Override + public void disable() { + // TODO Auto-generated method stub + + } + + @Override + public void update() { + // TODO Auto-generated method stub + + } + + @Override + public void disabledUpdate() { + // TODO Auto-generated method stub + + } + + @Override + public void debugs() { + // TODO Auto-generated method stub + + } + + + public static void main(String[] args) + { + new MecanumTest(); + } +} diff --git a/src/org/montclairrobotics/sprocket/test/SimpleTest.java b/src/org/montclairrobotics/sprocket/test/SimpleTest.java index 480b9eb..cb1bae2 100644 --- a/src/org/montclairrobotics/sprocket/test/SimpleTest.java +++ b/src/org/montclairrobotics/sprocket/test/SimpleTest.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.test; -import org.montclairrobotics.sprocket.control.SquaredDriveInput; +import org.montclairrobotics.sprocket.control.ArcadeDriveInput; import org.montclairrobotics.sprocket.core.Sprocket.MODE; import org.montclairrobotics.sprocket.drive.DriveModule; import org.montclairrobotics.sprocket.drive.DriveTrain; @@ -21,7 +21,7 @@ public void setup() { new DriveModule(new XY(-1,0),new XY(0,-1),new TestMotor("Left")), new DriveModule(new XY( 1,0),new XY(0, 1),new TestMotor("Right")) ); - dt.setDefaultInput(new SquaredDriveInput(new TestJoystick(TEST.CIRCLE))); + dt.setDefaultInput(new ArcadeDriveInput(new TestJoystick(TEST.CIRCLE))); dt.setMapper(new TankMapper()); } diff --git a/src/org/montclairrobotics/sprocket/test/TestJoystick.java b/src/org/montclairrobotics/sprocket/test/TestJoystick.java index 30e8360..0cd660e 100644 --- a/src/org/montclairrobotics/sprocket/test/TestJoystick.java +++ b/src/org/montclairrobotics/sprocket/test/TestJoystick.java @@ -1,10 +1,12 @@ package org.montclairrobotics.sprocket.test; import org.montclairrobotics.sprocket.core.IJoystick; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.geometry.XY; public class TestJoystick implements IJoystick{ - enum TEST {ZERO,CIRCLE,RANDOM}; - int i=0; + enum TEST {ZERO,CIRCLE,RANDOM, TINY}; + int i=-1; private TEST test; @@ -12,31 +14,20 @@ public TestJoystick(TEST test) { this.test=test; } - - @Override - public double getX() { - i++; - switch(test) - { - case CIRCLE: - return Math.cos(i*Math.PI/4); - case RANDOM: - return Math.random()*2-1; - default: - return 0; - } - } @Override - public double getY() { + public Vector get() { + i++; switch(test) { case CIRCLE: - return Math.sin(i*Math.PI/4); + return new XY(Math.sin(i*Math.PI/4),Math.cos(i*Math.PI/4)); case RANDOM: - return Math.random()*2-1; + return new XY(Math.random()*2-1,Math.random()*2-1); + case TINY: + return new XY(0.01,0.01); default: - return 0; + return Vector.ZERO; } } diff --git a/src/org/montclairrobotics/sprocket/utils/ZeroInput.java b/src/org/montclairrobotics/sprocket/utils/ZeroInput.java index 31ae1eb..05b3148 100644 --- a/src/org/montclairrobotics/sprocket/utils/ZeroInput.java +++ b/src/org/montclairrobotics/sprocket/utils/ZeroInput.java @@ -1,7 +1,7 @@ package org.montclairrobotics.sprocket.utils; public class ZeroInput implements Input{ - + public static final ZeroInput ZERO_INPUT=new ZeroInput(); @Override public Double get() { // TODO Auto-generated method stub From 8dbf1e52a9c5650bccbc5f665ba0ab763db66645 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Fri, 3 Nov 2017 22:15:32 -0400 Subject: [PATCH 70/71] Mecanum Driving! Using Universal Drivetrain. --- .../sprocket/control/BasicInput.java | 2 +- .../sprocket/drive/DriveModule.java | 5 +++++ .../sprocket/drive/UniversalMapper.java | 22 +++++++++++-------- .../sprocket/ftc/FTCJoystick.java | 14 ++++++++---- .../sprocket/ftc/FTCMotor.java | 6 +++++ .../sprocket/motors/Module.java | 8 ++++++- .../sprocket/test/MecanumTest.java | 2 +- 7 files changed, 43 insertions(+), 16 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/control/BasicInput.java b/src/org/montclairrobotics/sprocket/control/BasicInput.java index ebbf8e9..b885138 100644 --- a/src/org/montclairrobotics/sprocket/control/BasicInput.java +++ b/src/org/montclairrobotics/sprocket/control/BasicInput.java @@ -15,7 +15,7 @@ public BasicInput(Input dir,Input turn) this.dir=dir; this.turn=turn; } - public BasicInput(Input dirX,Input dirY,Input turn) + public BasicInput(final Input dirX,final Input dirY,Input turn) { this.dir=new Input(){ diff --git a/src/org/montclairrobotics/sprocket/drive/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index e104218..48c4028 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -7,6 +7,7 @@ import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.motors.Module; import org.montclairrobotics.sprocket.motors.SEncoder; +import org.montclairrobotics.sprocket.utils.Debug; import org.montclairrobotics.sprocket.utils.PID; /** @@ -109,4 +110,8 @@ public String toString() { return "("+offset.getX()+","+offset.getY()+"): "+(power*100)+"%"; } + public void debug() + { + Debug.msg("("+offset.getX()+","+offset.getY()+")",(power*100)+"%"); + } } diff --git a/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java index d3c7a60..08135b7 100644 --- a/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java @@ -1,7 +1,5 @@ package org.montclairrobotics.sprocket.drive; -import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; public class UniversalMapper implements DTMapper{ @@ -23,11 +21,12 @@ public void map(DTTarget driveTarget, DriveModule[] driveModules) { Vector offset=module.getOffset(); if(offset.getMagnitude()>0) { - double t=offset.setMag(1/offset.getMagnitude()).crossProduct(module.getForce()); + double t=Math.abs(offset.setMag(1/offset.getMagnitude()).crossProduct(module.getForce())); if (t>maxTorque) maxTorque=t; } } + //int i=0; for(DriveModule module:driveModules) { double f=module.getForce().dotProduct(dir); @@ -36,13 +35,18 @@ public void map(DTTarget driveTarget, DriveModule[] driveModules) { if(offset.getMagnitude()>0) { double t=offset.setMag(1/offset.getMagnitude()).crossProduct(module.getForce()); - if(t>0) - { - Vector tVec=module.getOffset().rotate(Angle.QUARTER).setMag(t*turn/maxTorque); - fVec=fVec.add(tVec); - } + //if(Math.abs(t)>0) + //{ + //Debug.msg("Module "+i+": offset",module.getOffset()); + //Debug.msg("Module "+i+": t",t); + //Vector tVec=module.getOffset().rotate(Angle.QUARTER).setMag(turn*t/maxTorque); + //Debug.msg("Module "+i+": tVec",tVec); + fVec=fVec.setMag(fVec.getMagnitude()+turn*t/maxTorque); + //Debug.msg("Module "+i+": fVec",fVec); + //} } - module.set(fVec.dotProduct(module.getForce())/module.getForce().getMagnitude()); + module.set(fVec.dotProduct(module.getForce().normalize())); + //i++; } } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java index d943e41..67e790b 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java @@ -1,8 +1,11 @@ package org.montclairrobotics.sprocket.ftc; import com.qualcomm.robotcore.hardware.Gamepad; + import org.montclairrobotics.sprocket.core.IJoystick; import org.montclairrobotics.sprocket.ftc.FTCRobot.GAMEPAD; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.geometry.XY; //This class kills me public class FTCJoystick implements IJoystick{ @@ -28,7 +31,6 @@ public FTCJoystick(Gamepad gamepad,STICK stick) this.stick=stick; } - @Override public double getX() { if(stick==STICK.LEFT) { @@ -43,19 +45,23 @@ else if(stick==STICK.RIGHT) return (gamepad.dpad_right?1:0)-(gamepad.dpad_left?1:0); } } - @Override public double getY() { if(stick==STICK.LEFT) { - return gamepad.left_stick_y; + return -gamepad.left_stick_y; } else if(stick==STICK.RIGHT) { - return gamepad.right_stick_y; + return -gamepad.right_stick_y; } else { return (gamepad.dpad_up?1:0)-(gamepad.dpad_down?1:0); } } + + public Vector get() + { + return new XY(getX(),getY()); + } } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java index fcd745c..90c04db 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java @@ -1,6 +1,7 @@ package org.montclairrobotics.sprocket.ftc; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import org.montclairrobotics.sprocket.core.IMotor; @@ -54,4 +55,9 @@ public void setZeroPos(double zeroPos) { this.zeroPos=zeroPos; } + + public void forward(boolean forward) + { + motor.setDirection(forward? DcMotorSimple.Direction.FORWARD: DcMotorSimple.Direction.REVERSE); + } } diff --git a/src/org/montclairrobotics/sprocket/motors/Module.java b/src/org/montclairrobotics/sprocket/motors/Module.java index 3834d3a..5992f18 100644 --- a/src/org/montclairrobotics/sprocket/motors/Module.java +++ b/src/org/montclairrobotics/sprocket/motors/Module.java @@ -85,7 +85,13 @@ public void set(double val) { motor.set(power); } - Debug.num("module-" + moduleId, power); + //Debug.num("module-" + moduleId, power); + debug(); + } + + public void debug() + { + Debug.num("module-" + moduleId, power); } /** diff --git a/src/org/montclairrobotics/sprocket/test/MecanumTest.java b/src/org/montclairrobotics/sprocket/test/MecanumTest.java index 45e7dcc..7eaaae2 100644 --- a/src/org/montclairrobotics/sprocket/test/MecanumTest.java +++ b/src/org/montclairrobotics/sprocket/test/MecanumTest.java @@ -25,7 +25,7 @@ public void setup() { new DriveModule(new XY(-1,1),new XY(1,1),new TestMotor("FrontLeft")), new DriveModule(new XY(1,1),new XY(-1,1),new TestMotor("FrontRight")) ); - dt.setDefaultInput(new BasicInput(new TestJoystick(TEST.ZERO),new Input(){ + dt.setDefaultInput(new BasicInput(new TestJoystick(TEST.CIRCLE),new Input(){ @Override public Double get() { From ad3daf0be793a22fd53e0e95637c114fc57628c3 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Sat, 4 Nov 2017 10:26:00 -0400 Subject: [PATCH 71/71] Added some easier init methods --- .../sprocket/core/IDebug.java | 2 + .../sprocket/core/IRobot.java | 15 +++--- .../sprocket/core/Sprocket.java | 21 ++++++-- .../sprocket/frc/DashboardDebug.java | 2 + .../sprocket/ftc/FTCDebug.java | 13 ++++- .../sprocket/ftc/FTCRobot.java | 48 ++++++++++--------- .../sprocket/test/MecanumTest.java | 20 +++++++- .../sprocket/test/SimpleTest.java | 20 +++++++- .../sprocket/test/TestDebug.java | 3 +- 9 files changed, 109 insertions(+), 35 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/core/IDebug.java b/src/org/montclairrobotics/sprocket/core/IDebug.java index 139cbbb..9885040 100644 --- a/src/org/montclairrobotics/sprocket/core/IDebug.java +++ b/src/org/montclairrobotics/sprocket/core/IDebug.java @@ -3,4 +3,6 @@ public interface IDebug { public void debugStr(String key,String val); public void debugNum(String key,double val); + + public void update(); } diff --git a/src/org/montclairrobotics/sprocket/core/IRobot.java b/src/org/montclairrobotics/sprocket/core/IRobot.java index fc4ff4f..d809390 100644 --- a/src/org/montclairrobotics/sprocket/core/IRobot.java +++ b/src/org/montclairrobotics/sprocket/core/IRobot.java @@ -5,10 +5,13 @@ public interface IRobot extends Updatable{ //Stuff user overrides - public void setup(); - public void enable(Sprocket.MODE mode); - public void disable(); - public void update(); - public void disabledUpdate(); - public void debugs(); + public void setup();//Called ONCE when object is first created + public void enableMode(Sprocket.MODE mode);//Called at any init, not including Disabled + public void teleopInit();//Called when Teleop is enabled + public void autoInit();//Called when Auto is enabled + public void testInit();//Called when Test is enabled + public void disable();//Called when Disabled (right after setup is called, and every time you disable + public void update();//Called every enabled update loop + public void disabledUpdate();//Called every disabled update loop + public void debugs();//Called at the end of every loop } diff --git a/src/org/montclairrobotics/sprocket/core/Sprocket.java b/src/org/montclairrobotics/sprocket/core/Sprocket.java index 4f5f9a2..fcabfed 100644 --- a/src/org/montclairrobotics/sprocket/core/Sprocket.java +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -48,35 +48,46 @@ public void initS() } public final void startS(MODE mode) { + if(mode==MODE.DISABLED) + { + stopS(); + return; + } + + Debug.msg("MODE", mode); + curMode=mode; + robot.enableMode(mode); + switch(mode) { case AUTO: if(autoActionInput!=null) currentAction=autoActionInput.get(); + robot.autoInit(); break; case TELEOP: if(teleopActionInput!=null) currentAction=teleopActionInput.get(); + robot.teleopInit(); break; case TEST: if(testActionInput!=null) currentAction=testActionInput.get(); + robot.testInit(); break; default: break; } - Debug.msg("MODE", mode); - curMode=mode; if(currentAction!=null) { currentAction.start(); } - robot.enable(mode); } public final void updateS() { Updater.loop(); robot.debugs(); + debugger.update(); } public final void stopS() { @@ -85,11 +96,15 @@ public final void stopS() currentAction.stop(); } curMode=MODE.DISABLED; + Debug.msg("MODE", MODE.DISABLED); + robot.enableMode(MODE.DISABLED); robot.disable(); } public final void disabledUpdateS() { DisabledUpdater.loop(); robot.disabledUpdate(); + robot.debugs(); + debugger.update(); } } diff --git a/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java index c661758..4ea0aee 100644 --- a/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java +++ b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java @@ -17,4 +17,6 @@ public void debugNum(String key, double val) { SmartDashboard.putNumber(key, val); } + + public void update() {} } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java index 4a52050..20f2afc 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java @@ -8,7 +8,13 @@ public class FTCDebug implements IDebug{ - + private FTCRobot robot; + + public FTCDebug(FTCRobot r) + { + robot=r; + } + @Override public void debugStr(String key, String val) { FTCRobot.ftcTelemetry.addData(key,val); @@ -18,4 +24,9 @@ public void debugStr(String key, String val) { public void debugNum(String key, double val) { FTCRobot.ftcTelemetry.addData(key,val); } + + public void update() + { + robot.sendTelemetry(); + } } diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java index cc2066d..8f6a1e9 100644 --- a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -70,7 +70,7 @@ public void init() { robot.ftcSetup(hardwareMap, gamepad1, gamepad2, telemetry); if (sprocket==null) { sprocket = new Sprocket(robot); - sprocket.debugger=new FTCDebug(); + sprocket.debugger=new FTCDebug(robot); sprocket.initS(); } sprocket.startS(MODE.DISABLED); @@ -82,27 +82,31 @@ public void autoInit() { this.mode=mode.TELEOP; } - @Override - public void init_loop() { - sprocket.disabledUpdateS(); - } - - @Override - public void start() { - sprocket.currentAction=action; - sprocket.startS(mode); - } - - @Override - public void loop() { - sprocket.updateS(); - - } - - @Override - public void stop() { - sprocket.stopS(); - } + @Override + public void init_loop() { + sprocket.disabledUpdateS(); + } + + @Override + public void start() { + sprocket.currentAction=action; + sprocket.startS(mode); + } + + @Override + public void loop() { + sprocket.updateS(); + + } + + @Override + public void stop() { + sprocket.stopS(); + } + public void sendTelemetry() + { + updateTelemetry(ftcTelemetry); + } public void ftcSetup(HardwareMap hardwareMap,Gamepad gamepad1,Gamepad gamepad2,Telemetry telemetry) { diff --git a/src/org/montclairrobotics/sprocket/test/MecanumTest.java b/src/org/montclairrobotics/sprocket/test/MecanumTest.java index 7eaaae2..acbafe5 100644 --- a/src/org/montclairrobotics/sprocket/test/MecanumTest.java +++ b/src/org/montclairrobotics/sprocket/test/MecanumTest.java @@ -36,7 +36,7 @@ public Double get() { } @Override - public void enable(MODE mode) { + public void enableMode(MODE mode) { // TODO Auto-generated method stub } @@ -70,4 +70,22 @@ public static void main(String[] args) { new MecanumTest(); } + + @Override + public void teleopInit() { + // TODO Auto-generated method stub + + } + + @Override + public void autoInit() { + // TODO Auto-generated method stub + + } + + @Override + public void testInit() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/test/SimpleTest.java b/src/org/montclairrobotics/sprocket/test/SimpleTest.java index cb1bae2..99b8256 100644 --- a/src/org/montclairrobotics/sprocket/test/SimpleTest.java +++ b/src/org/montclairrobotics/sprocket/test/SimpleTest.java @@ -26,7 +26,7 @@ public void setup() { } @Override - public void enable(MODE mode) { + public void enableMode(MODE mode) { // TODO Auto-generated method stub } @@ -60,4 +60,22 @@ public static void main(String[] args) { new SimpleTest(); } + + @Override + public void teleopInit() { + // TODO Auto-generated method stub + + } + + @Override + public void autoInit() { + // TODO Auto-generated method stub + + } + + @Override + public void testInit() { + // TODO Auto-generated method stub + + } } diff --git a/src/org/montclairrobotics/sprocket/test/TestDebug.java b/src/org/montclairrobotics/sprocket/test/TestDebug.java index 7de9f0b..36f11df 100644 --- a/src/org/montclairrobotics/sprocket/test/TestDebug.java +++ b/src/org/montclairrobotics/sprocket/test/TestDebug.java @@ -13,5 +13,6 @@ public void debugStr(String key, String val) { public void debugNum(String key, double val) { debugStr(key,""+val); } - + + public void update(){} }