diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/SprocketRobot.java deleted file mode 100644 index c00e6e8..0000000 --- a/src/org/montclairrobotics/sprocket/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(); - } - - @Override - public final void disabledPeriodic() { - super.disabledPeriodic(); - } - - @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); - } - SmartDashboard.putData("AUTO:",chooser); - } -} diff --git a/src/org/montclairrobotics/sprocket/actions/Action.java b/src/org/montclairrobotics/sprocket/actions/Action.java new file mode 100644 index 0000000..9b2d69b --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/Action.java @@ -0,0 +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 void start(); + /** + * Called every loop the object is active, after it is activated + */ + public void enabled(); + /** + * Called once when the object is deactivated + */ + public void stop(); + /** + * Called every loop when the object is deactivated + * Note: this may not be called by every implementation + */ + public 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 59% rename from src/org/montclairrobotics/sprocket/states/MultiState.java rename to src/org/montclairrobotics/sprocket/actions/MultiState.java index ed8c84f..8ae5d86 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) { + public MultiState(final int stateToStopAt, final 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 stateUpdate() { - for(State s : states) { - s.stateUpdate(); - } - } @Override public boolean isDone() { 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/actions/SplitButton.java b/src/org/montclairrobotics/sprocket/actions/SplitButton.java new file mode 100644 index 0000000..dd74c58 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/SplitButton.java @@ -0,0 +1,55 @@ +package org.montclairrobotics.sprocket.actions; + +import org.montclairrobotics.sprocket.core.Button; +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/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 67% rename from src/org/montclairrobotics/sprocket/states/StateMachine.java rename to src/org/montclairrobotics/sprocket/actions/StateMachine.java index 8c16306..86850a1 100644 --- a/src/org/montclairrobotics/sprocket/states/StateMachine.java +++ b/src/org/montclairrobotics/sprocket/actions/StateMachine.java @@ -1,10 +1,9 @@ -package org.montclairrobotics.sprocket.states; +package org.montclairrobotics.sprocket.actions; import org.montclairrobotics.sprocket.loop.Priority; 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{ @@ -22,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 @@ -38,16 +37,16 @@ public void stop() { } @Override - public void stateUpdate() { + public void enabled() { if(isDone())return; - states[index].stateUpdate(); + states[index].enabled(); while(states[index].isDone()) { states[index].stop(); index++; if(isDone())return; - startState(); - states[index].stateUpdate(); + states[index].start(); + states[index].enabled(); } } @@ -63,19 +62,12 @@ public State[] getStates() public void update() { if(top) { - stateUpdate(); + enabled(); } } - public void startState() - { - if(states[index] instanceof StateMachine) - { - ((StateMachine)states[index]).start(false); - } - else - { - states[index].start(); - } - SmartDashboard.putString("I hope it gets here","It does!"); + @Override + public void disabled() { + // TODO Auto-generated method stub + } } diff --git a/src/org/montclairrobotics/sprocket/utils/Togglable.java b/src/org/montclairrobotics/sprocket/actions/Togglable.java similarity index 60% rename from src/org/montclairrobotics/sprocket/utils/Togglable.java rename to src/org/montclairrobotics/sprocket/actions/Togglable.java index 506f2e9..8dedbc2 100644 --- a/src/org/montclairrobotics/sprocket/utils/Togglable.java +++ b/src/org/montclairrobotics/sprocket/actions/Togglable.java @@ -1,9 +1,6 @@ -package org.montclairrobotics.sprocket.utils; +package org.montclairrobotics.sprocket.actions; public interface Togglable { - public void enable(); - public void disable(); - } diff --git a/src/org/montclairrobotics/sprocket/actions/Toggle.java b/src/org/montclairrobotics/sprocket/actions/Toggle.java new file mode 100644 index 0000000..3fe697b --- /dev/null +++ b/src/org/montclairrobotics/sprocket/actions/Toggle.java @@ -0,0 +1,36 @@ +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(); + } + + @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/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/AutoMode.java b/src/org/montclairrobotics/sprocket/auto/AutoMode.java index 9f4107c..5ae9982 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoMode.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoMode.java @@ -1,18 +1,17 @@ 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.core.Sprocket; import org.montclairrobotics.sprocket.utils.Debug; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * 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{ @@ -21,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) { @@ -36,11 +35,17 @@ public void start() public void stop() { super.stop(); - SprocketRobot.getDriveTrain().useDefaultInput(); + Sprocket.getMainDriveTrain().useDefaultInput(); } 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 cd66d74..53fd874 100644 --- a/src/org/montclairrobotics/sprocket/auto/AutoState.java +++ b/src/org/montclairrobotics/sprocket/auto/AutoState.java @@ -1,10 +1,11 @@ package org.montclairrobotics.sprocket.auto; -import org.montclairrobotics.sprocket.SprocketRobot; +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; -import org.montclairrobotics.sprocket.states.State; /** * This object represents a singular state or step in an autonomous routine. @@ -13,13 +14,16 @@ * 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 double tgtTurn = 0; private double t;//time state was started + public double TIMEOUT=30; public final void start() { t=Updater.getTime(); - SprocketRobot.getDriveTrain().setTempInput(this); + Sprocket.getMainDriveTrain().setTempInput(this); userStart(); } @@ -33,7 +37,7 @@ public final void stop() /*tgtDir=Vector.ZERO; tgtTurn=Angle.ZERO;*/ //output.inputType=DTInput.Type.SPEED; - SprocketRobot.getDriveTrain().useDefaultInput(); + Sprocket.getMainDriveTrain().useDefaultInput(); } /** @@ -55,8 +59,40 @@ 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; } + + @Override + public Vector getDir() { + return tgtDir; + } + + @Override + public double 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 - public void stateUpdate() { + public void enabled() { // TODO Auto-generated method stub } @Override - public boolean isDone() { + public boolean userIsDone() { Debug.num("Time", time); Debug.num("Time in State", timeInState()); - return timeInState()>time; + return false; + } + + @Override + public void disabled() { + // 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 21082e6..0000000 --- a/src/org/montclairrobotics/sprocket/auto/states/Disable.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.montclairrobotics.sprocket.auto.states; - -import org.montclairrobotics.sprocket.states.State; -import org.montclairrobotics.sprocket.utils.Togglable; - -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 633d447..822ec8c 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoderGyro.java @@ -1,118 +1,58 @@ package org.montclairrobotics.sprocket.auto.states; -import org.montclairrobotics.sprocket.control.DashboardInput; +import org.montclairrobotics.sprocket.actions.State; +import org.montclairrobotics.sprocket.actions.StateMachine; 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; -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{ - - - 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 stateUpdate() { - 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 stateUpdate() { - driveGyro.use(); - } + public static State[] makeStates(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() + { - @Override - public boolean isDone() { - // TODO Auto-generated method stub - return false; - }}) - ); - } - - public DriveEncoderGyro(Input dInput, Input a, boolean relative, Input speed, double maxEncAccel, double maxEncTicksPerSec, GyroCorrection driveGyro) { - super( - new TurnGyro(a,driveGyro,relative), - new MultiState(0, - new DriveEncoders(dInput,speed, maxEncAccel, maxEncTicksPerSec), - new State(){ - @Override - public void start() { - driveGyro.setTargetAngle(new Degrees(a.get()),relative); - } - @Override - public void stop() {} - @Override - public void stateUpdate() { - 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(){ - @Override - public void start() { - driveGyro.setTargetAngleRelative(); - } - @Override - public void stop() {} - @Override - public void stateUpdate() { - driveGyro.use(); - } + @Override + public void start() { + // TODO Auto-generated method stub + gcFinal.setTargetAngle(new Degrees(angleFinal.doubleValue()),relativeFinal.compareTo(true)==0); + } + + @Override + public void enabled() { + // TODO Auto-generated method stub + gcFinal.use(); + } - @Override - public boolean isDone() { - // TODO Auto-generated method stub - return false; - }}) - ); + @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 99851b4..8d3dd27 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/DriveEncoders.java @@ -1,8 +1,7 @@ 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.core.Sprocket; import org.montclairrobotics.sprocket.drive.DriveTrain; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.XY; @@ -12,51 +11,37 @@ 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 = SprocketRobot.getDriveTrain(); - 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 - public void stateUpdate() { + public void enabled() { /* tgtDir = new XY(0,Utils.constrain( Math.sqrt(Math.abs(2*maxAccel.get()*(stopDist.get()-dt.getDistance().get()))) @@ -69,25 +54,38 @@ public void stateUpdate() { *(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); - 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); } - + + @Override + public void disabled() { + + } + @Override - public boolean isDone() { - Debug.msg("forwards", forwards?"YES":"NO"); + public boolean userIsDone() { + 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() timeInput, double power) { super(timeInput); this.tgtDir = new XY(0, power); - this.tgtTurn = Angle.ZERO; + this.tgtTurn = 0; } @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 ed38b46..0000000 --- a/src/org/montclairrobotics/sprocket/auto/states/Enable.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.montclairrobotics.sprocket.auto.states; - -import org.montclairrobotics.sprocket.states.State; -import org.montclairrobotics.sprocket.utils.Togglable; - -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 4d0723e..6cf39dd 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnEncoders.java @@ -1,26 +1,26 @@ package org.montclairrobotics.sprocket.auto.states; -import org.montclairrobotics.sprocket.SprocketRobot; import org.montclairrobotics.sprocket.auto.AutoState; +import org.montclairrobotics.sprocket.core.Sprocket; import org.montclairrobotics.sprocket.drive.DriveModule; import org.montclairrobotics.sprocket.geometry.Angle; 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; } @Override public void userStart() { - module = SprocketRobot.getDriveTrain().getModules()[0]; - double modulePos = module.getEnc().getInches().get(); + 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) { finalPos += modulePos; @@ -30,18 +30,18 @@ public void userStart() { } @Override - public void stateUpdate() { + public void enabled() { tgtTurn = turnSpeed; } @Override - public boolean isDone() { - return Math.abs(module.getEnc().getInches().get() - finalPos) < 1.0; + public boolean userIsDone() { + return Math.abs(module.getEnc().getDistance().get() - finalPos) < 1.0; } @Override public void userStop() { - tgtTurn = Angle.ZERO; + tgtTurn = 0.0; } diff --git a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java index 35007a7..10ee0b3 100644 --- a/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java +++ b/src/org/montclairrobotics/sprocket/auto/states/TurnGyro.java @@ -1,57 +1,38 @@ 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 { private Angle tgt; - private Input 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(5); - private static final double timeAtTarget=0.2; + 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); } @@ -60,11 +41,11 @@ public void userStart() gyro.setTargetAngleReset(tgt); } gyro.setMinMaxOut(-0.5, 0.5); - incorrectTime=0; + incorrectTime=Updater.getTime()-timeAtTarget+0.1; } @Override - public void stateUpdate() { + public void enabled() { gyro.use(); if(Math.abs(gyro.getError().toDegrees())>tolerance.toDegrees()) { @@ -73,6 +54,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()); } @@ -83,8 +65,17 @@ public void userStop() } @Override - public boolean isDone() { - return (incorrectTime>timeAtTarget); + public boolean userIsDone() { + return (Updater.getTime()-incorrectTime>timeAtTarget); + } + + + public Angle getTgt() { + return tgt; + } + + public void setTgt(Angle tgt) { + this.tgt = tgt; } } diff --git a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java index 58a2dac..05c6988 100644 --- a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java @@ -1,16 +1,18 @@ package org.montclairrobotics.sprocket.control; +import org.montclairrobotics.sprocket.core.IJoystick; 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.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; -import edu.wpi.first.wpilibj.Joystick; /** * This class takes inputs from a single Joystick and acts as an arcade drive @@ -18,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 Joystick stick; - - private Vector dir; - private Angle 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(Joystick 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 = new Radians(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 Angle 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..b885138 --- /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(final Input dirX,final 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/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/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/control/ExponentDriveInput.java b/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java deleted file mode 100644 index f53d4f2..0000000 --- a/src/org/montclairrobotics/sprocket/control/ExponentDriveInput.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.montclairrobotics.sprocket.control; - -import edu.wpi.first.wpilibj.Joystick; - -public class ExponentDriveInput extends ArcadeDriveInput -{ - - private double p; - - public ExponentDriveInput(Joystick stick) { - this(stick,0.5); - // TODO Auto-generated constructor stub - } - public ExponentDriveInput(Joystick 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 ec63497..145871b 100644 --- a/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/FieldCentricDriveInput.java @@ -1,42 +1,48 @@ package org.montclairrobotics.sprocket.control; -import org.montclairrobotics.sprocket.SprocketRobot; +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.Degrees; -import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; -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.Input; -import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.Togglable; +import org.montclairrobotics.sprocket.utils.SmoothVectorInput; -import edu.wpi.first.wpilibj.Joystick; -public class FieldCentricDriveInput extends ArcadeDriveInput implements Togglable{ +public class FieldCentricDriveInput implements DTInput,Action,Updatable{ private GyroCorrection gyro; private Vector field,robot; + private SmoothVectorInput fieldInput; private boolean forwards; private boolean rotToVector; - public FieldCentricDriveInput(Joystick stick,GyroCorrection gyro) + private boolean enabled=false; + + private static final int SMOOTH_LEN=10; + + public FieldCentricDriveInput(Input stick,GyroCorrection gyro) { - this(stick,gyro,true); + this(stick,gyro,false); } - public FieldCentricDriveInput(Joystick 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,stick); + Updater.add(this, Priority.CONTROL); } @Override public void update() { - super.update(); - field=getRaw(); + field=fieldInput.get(); if(field.getMagnitude()>0.1) { robot=field.rotate(gyro.getCurrentAngleReset().negative()); @@ -53,16 +59,16 @@ public void update() */ @Override public Vector getDir() { - return robot.square(); + return robot; } /** * @return The calculated turning speed for the DriveTrain */ @Override - public Angle getTurn() { + public double getTurn() { - if(rotToVector&&field.getMagnitude()>0.1) + if(enabled&&rotToVector&&field.getMagnitude()>0.1) { if(forwards) { @@ -74,15 +80,27 @@ public Angle getTurn() { } gyro.use(); } - return Angle.ZERO; + return 0; } @Override - public void enable() { - SprocketRobot.getDriveTrain().setTempInput(this); + public void start() { + Sprocket.getMainDriveTrain().setTempInput(this); + enabled=true; + } + @Override + public void stop() { + Sprocket.getMainDriveTrain().useDefaultInput(); + enabled=false; + } + @Override + public void enabled() { + // TODO Auto-generated method stub + } @Override - public void disable() { - SprocketRobot.getDriveTrain().useDefaultInput(); + public void disabled() { + // TODO Auto-generated method stub + } } diff --git a/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java b/src/org/montclairrobotics/sprocket/control/JoystickXAxis.java new file mode 100644 index 0000000..97d6eae --- /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.get().getX(); + } + +} diff --git a/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java b/src/org/montclairrobotics/sprocket/control/JoystickYAxis.java index d1e8517..2e0a28d 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.IJoystick; 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 @@ -11,19 +11,13 @@ */ public class JoystickYAxis implements Input { - private Joystick stick; + private IJoystick 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) { + public JoystickYAxis(IJoystick stick) { this.stick = stick; } @@ -32,7 +26,7 @@ public JoystickYAxis(Joystick 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 9e0a574..0000000 --- a/src/org/montclairrobotics/sprocket/control/SquaredDriveInput.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.montclairrobotics.sprocket.control; - -import edu.wpi.first.wpilibj.Joystick; - -public class SquaredDriveInput extends ArcadeDriveInput{ - - private double xSensitivity = 1.0; - private double ySensitivity = 1.0; - - public SquaredDriveInput(Joystick 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/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..a8a8a76 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/Button.java @@ -0,0 +1,84 @@ +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; +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,Input { + + 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; + } + public Action getAction() + { + return action; + } + + @Override + public void update() { + if(enabled) + { + boolean pressed = 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() { + enabled=true; + } + @Override + public void disable() { + enabled=false; + } + @Override + public Boolean get() { + return button.get(); + } +} diff --git a/src/org/montclairrobotics/sprocket/core/IAutoSelector.java b/src/org/montclairrobotics/sprocket/core/IAutoSelector.java new file mode 100644 index 0000000..ea724d1 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/IAutoSelector.java @@ -0,0 +1,13 @@ +package org.montclairrobotics.sprocket.core; + +import org.montclairrobotics.sprocket.auto.AutoMode; +import org.montclairrobotics.sprocket.utils.Input; + +public interface IAutoSelector extends Input{ + + void addAutoMode(AutoMode mode); + + void setAutoModes(AutoMode[] modes); + + void update(); +} diff --git a/src/org/montclairrobotics/sprocket/core/IDebug.java b/src/org/montclairrobotics/sprocket/core/IDebug.java new file mode 100644 index 0000000..9885040 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/IDebug.java @@ -0,0 +1,8 @@ +package org.montclairrobotics.sprocket.core; + +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/IEncoder.java b/src/org/montclairrobotics/sprocket/core/IEncoder.java new file mode 100644 index 0000000..be262ff --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/IEncoder.java @@ -0,0 +1,8 @@ +package org.montclairrobotics.sprocket.core; + + +public interface IEncoder { + public double getSpeed(); + public double getDistance(); + public void reset(); +} diff --git a/src/org/montclairrobotics/sprocket/core/IJoystick.java b/src/org/montclairrobotics/sprocket/core/IJoystick.java new file mode 100644 index 0000000..d25fb0b --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/IJoystick.java @@ -0,0 +1,9 @@ +package org.montclairrobotics.sprocket.core; + +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/core/IMotor.java b/src/org/montclairrobotics/sprocket/core/IMotor.java new file mode 100644 index 0000000..21e0d67 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/IMotor.java @@ -0,0 +1,7 @@ +package org.montclairrobotics.sprocket.core; + +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 new file mode 100644 index 0000000..d809390 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/IRobot.java @@ -0,0 +1,17 @@ +package org.montclairrobotics.sprocket.core; + +import org.montclairrobotics.sprocket.loop.Updatable; + +public interface IRobot extends Updatable{ + + //Stuff user overrides + 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 new file mode 100644 index 0000000..fcabfed --- /dev/null +++ b/src/org/montclairrobotics/sprocket/core/Sprocket.java @@ -0,0 +1,110 @@ +package org.montclairrobotics.sprocket.core; + +import org.montclairrobotics.sprocket.actions.Action; +import org.montclairrobotics.sprocket.drive.DriveTrain; +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; + + +/** + * @author MHS Robotics + * This class is basically just a wrapper around iterative robot which all Sprocket + * robots must extend. + */ +public class Sprocket{ + + public static DriveTrain driveTrain; + public static IDebug debugger; + + private IRobot robot; + + public enum MODE {AUTO,TELEOP,TEST,DISABLED}; + public MODE curMode; + public Input + autoActionInput, + teleopActionInput, + testActionInput; + public Action currentAction; + + public Sprocket(IRobot robot) + { + this.robot=robot; + Updater.add(robot,Priority.NORMAL); + } + //OUR STUFF HERE + public static DriveTrain getMainDriveTrain() { + return driveTrain; + } + public static void setMainDriveTrain(DriveTrain dt) { + driveTrain = dt; + } + + public void initS() + { + robot.setup(); + } + 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; + } + if(currentAction!=null) + { + currentAction.start(); + } + } + public final void updateS() + { + Updater.loop(); + robot.debugs(); + debugger.update(); + } + public final void stopS() + { + if(currentAction!=null) + { + 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/drive/ControlledMotor.java b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java index 6775678..406fdff 100644 --- a/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java +++ b/src/org/montclairrobotics/sprocket/drive/ControlledMotor.java @@ -1,13 +1,12 @@ package org.montclairrobotics.sprocket.drive; +import org.montclairrobotics.sprocket.actions.Action; +import org.montclairrobotics.sprocket.core.IMotor; 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; -import edu.wpi.first.wpilibj.SpeedController; /** * ControlledMotor is a wrapper class for Motors which allows a motor to be @@ -18,8 +17,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,Action { + private IMotor output; private Input input; private boolean enabled=true; @@ -29,8 +29,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(IMotor motor, Input input) { + this.output=motor; this.input = input; Updater.add(this, Priority.OUTPUT); } @@ -44,8 +44,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(IMotor motor, Input forwardInput, Input reverseInput, double speed) { + this.output=motor; this.input = new ButtonPairInput(forwardInput, reverseInput, speed); Updater.add(this, Priority.OUTPUT); } @@ -58,26 +58,38 @@ 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(IMotor 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 - public void enable() { + public void start() { enabled=true; } @Override - public void disable() { + 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/DTInput.java b/src/org/montclairrobotics/sprocket/drive/DTInput.java index 997cbdf..3836c64 100644 --- a/src/org/montclairrobotics/sprocket/drive/DTInput.java +++ b/src/org/montclairrobotics/sprocket/drive/DTInput.java @@ -1,11 +1,15 @@ 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 { - + /** + * 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/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index 2d5dce2..48c4028 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -1,13 +1,13 @@ package org.montclairrobotics.sprocket.drive; +import org.montclairrobotics.sprocket.core.IMotor; 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.Debug; import org.montclairrobotics.sprocket.utils.PID; /** @@ -24,7 +24,7 @@ public class DriveModule extends Module{ private Vector offset; private Vector force; - private Motor[] motors; + private IMotor[] motors; private double power; @@ -40,7 +40,7 @@ public DriveModule(Vector offset, SEncoder enc, PID pid, Module.MotorInputType inputType, - Motor... motors + IMotor... motors ) { super(enc, pid, inputType, motors); this.offset = offset; @@ -48,7 +48,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); } @@ -57,12 +57,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); } @@ -110,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/DriveTrain.java b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java index 9028642..3fa323f 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java @@ -1,9 +1,8 @@ package org.montclairrobotics.sprocket.drive; -import org.montclairrobotics.sprocket.SprocketRobot; +import org.montclairrobotics.sprocket.core.Sprocket; 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; @@ -25,7 +24,7 @@ public DriveTrain(DriveModule... modules) { this.modules = modules; input=new ZeroDTInput(); pipeline=new ZeroPipeline(); - SprocketRobot.setDriveTrain(this); + Sprocket.setMainDriveTrain(this); Updater.add(this, Priority.DRIVE_CALC); } @@ -34,11 +33,11 @@ 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); - Debug.string("DriveTrain PIPELINE OUTPUT:",target.toString()); + Debug.string("DriveTrain OUTPUT:",target.toString()); mapper.map(target, modules); } diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java index 9f14bc1..0aee6b4 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java @@ -1,22 +1,18 @@ 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.core.IMotor; 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; import org.montclairrobotics.sprocket.utils.PID; -import org.montclairrobotics.sprocket.utils.PID; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.SpeedController; +import java.util.ArrayList; /** * DriveTrainBuilder is a helper class which makes constructing DriveTrains more @@ -45,18 +41,18 @@ 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 Motor(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, Motor... motors) { + public DriveTrainBuilder addWheels(Vector offset, Angle force, IMotor... motors) { modules.add(new DriveModule(offset, new Polar(1, force), motors)); return this; } @@ -101,7 +97,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/drive/GenericMapper.java b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java index b6548bd..9fa2503 100644 --- a/src/org/montclairrobotics/sprocket/drive/GenericMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/GenericMapper.java @@ -1,44 +1,73 @@ 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(); - Angle tgtTurn=driveTarget.getTurn(); - for(DriveModule module:driveModules) - { - tgtTurn=tgtTurn.subtract(getTorque(module.getOffset(),module.getForce(),tgtDir)); + //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; + + //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(); + } } - for(DriveModule module:driveModules) - { - module.set(inverseDot(module.getForce(),tgtDir.add( - new Polar(tgtTurn.toRadians()*module.getOffset().getMagnitude(),module.getOffset().getAngle().add(Angle.QUARTER))))); + + //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 * (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 + double dot = moduleVec.dotProduct(module.getForce().normalize()); //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(); + } } - } - - public static Angle getTorque(Vector offset,Vector force,Vector target) - { - if(offset.getMagnitude()==0) - return Angle.ZERO; - return new Radians(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 + powers[i] = powers[i] * (lowestForce/driveModules[i].getForce().getMagnitude()); //Scales to account for imbalanced motor powers + driveModules[i].set(powers[i]); } - return target.getMagnitude()/diff.cos(); + } -} + +} \ No newline at end of file diff --git a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java index 99a6f34..687c718 100644 --- a/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java +++ b/src/org/montclairrobotics/sprocket/drive/MecanumMapper.java @@ -1,49 +1,54 @@ 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; +/* +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) { - double turn = driveTarget.getTurn().toDegrees(); - - 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; + //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(); //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()); + } - //TODO: SCALING!!!!!!! - m.set(power); + //Finding the highest power calculated + double highestPower = Double.MIN_VALUE; + for(double p : motorPowers) { + if(Math.abs(p) > highestPower) { + 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); - 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())); + for(int i = 0; i < driveModules.length; i++) { + //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]); + } } } diff --git a/src/org/montclairrobotics/sprocket/drive/TankMapper.java b/src/org/montclairrobotics/sprocket/drive/TankMapper.java index e0c7587..4e5fffb 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; @@ -10,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 new file mode 100644 index 0000000..08135b7 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/drive/UniversalMapper.java @@ -0,0 +1,53 @@ +package org.montclairrobotics.sprocket.drive; + +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(); + 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(normDir); + if(f>maxForce) + maxForce=f; + Vector offset=module.getOffset(); + if(offset.getMagnitude()>0) + { + 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); + Vector fVec=module.getForce().setMag(f/maxForce); + Vector offset=module.getOffset(); + if(offset.getMagnitude()>0) + { + double t=offset.setMag(1/offset.getMagnitude()).crossProduct(module.getForce()); + //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().normalize())); + //i++; + } + } + +} 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 ce1c17c..898fbe6 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/AccelLimit.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.drive.steps; -import org.montclairrobotics.sprocket.SprocketRobot; +import org.montclairrobotics.sprocket.actions.Action; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; @@ -10,33 +10,28 @@ 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{ +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 @@ -48,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(); @@ -72,12 +67,22 @@ public DTTarget get(DTTarget in) { } } @Override - public void enable() { + public void start() { enabled=true; } @Override - public void disable() { + 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/Deadzone.java b/src/org/montclairrobotics/sprocket/drive/steps/Deadzone.java index 464f893..e99fadb 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; @@ -10,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()), Togglable { +public class GyroCorrection implements Step, Action { private PID pid; private boolean enabled=true; @@ -23,16 +20,27 @@ public class GyroCorrection implements Step, Togglable { private double minOut=-1; private double maxOut=1; + private double maxError; + private double farP; - public GyroCorrection(Input gyro,PID pid) + public GyroCorrection(Input gyro,PID pid,double maxError,double farP) { this(pid); this.pid.setInput(gyro); + this.maxError=maxError; + this.farP=farP; } - 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.farP=powIfMaxError; + } + public GyroCorrection(PID pid) + { + this(pid,180,1); } public void use() @@ -56,10 +64,17 @@ public DTTarget get(DTTarget in) { DTTarget out=in; if(enabled&&used) { - double tgt=pid.get(); - tgt=Utils.constrain(tgt, minOut, maxOut); - Angle tgtAngle=new Radians(tgt); - out=new DTTarget(in.getDirection(),tgtAngle); + double tgt; + if(Math.abs(pid.getError())>maxError) + { + tgt=farP*pid.getError()*(pid.getP()>0?1:-1); + } + else + { + tgt=pid.get(); + tgt=Utils.constrain(tgt, minOut, maxOut); + } + out=new DTTarget(in.getDirection(),tgt); } used=false; Debug.msg("Gyro Enabled",enabled); @@ -130,12 +145,12 @@ public PID getPID() } @Override - public void enable() { + public void start() { enabled=true; } @Override - public void disable() { + public void stop() { enabled=false; } public void setTargetAngle(Angle a,boolean relative) { @@ -149,5 +164,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/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/SpeedLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java index 5157937..7b108aa 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/SpeedLimiter.java @@ -1,14 +1,14 @@ 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.Togglable; -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) { @@ -16,17 +16,18 @@ public SpeedLimiter(double maxSpeed) } @Override - public void enable() { + public void start() { enabled=true; } @Override - public void disable() { + public void stop() { enabled=false; } @Override public DTTarget get(DTTarget in) { + if(!enabled)return in; Vector out=in.getDirection(); if(out.getMagnitude()>maxSpeed) { @@ -34,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/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/drive/steps/TurnLimiter.java b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java index eb6ddaf..1500fae 100644 --- a/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java +++ b/src/org/montclairrobotics/sprocket/drive/steps/TurnLimiter.java @@ -1,16 +1,15 @@ 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.geometry.Vector; import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Togglable; -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) { @@ -18,22 +17,35 @@ public TurnLimiter(double maxSpeed) } @Override - public void enable() { + public void start() { enabled=true; } @Override - public void disable() { + public void stop() { enabled=false; } @Override public DTTarget get(DTTarget in) { - Angle out=in.getTurn(); - if(Math.abs(out.toRadians())>maxSpeed) + if(!enabled)return in; + 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); } + + @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 4238e56..098d674 100644 --- a/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java +++ b/src/org/montclairrobotics/sprocket/drive/utils/GyroLock.java @@ -1,20 +1,14 @@ package org.montclairrobotics.sprocket.drive.utils; -import org.montclairrobotics.sprocket.SprocketRobot; -import org.montclairrobotics.sprocket.drive.DTTarget; +import org.montclairrobotics.sprocket.actions.Action; 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 { +public class GyroLock implements Updatable, Action { private GyroCorrection gyro; private boolean lastLock; @@ -28,11 +22,11 @@ public GyroLock(GyroCorrection gyro) Updater.add(this, Priority.HIGH); } - public void enable() { + public void start() { enabled = true; } - public void disable() { + public void stop() { enabled = false; } @@ -57,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/drive/utils/MyState.java b/src/org/montclairrobotics/sprocket/drive/utils/MyState.java new file mode 100644 index 0000000..891b64f --- /dev/null +++ b/src/org/montclairrobotics/sprocket/drive/utils/MyState.java @@ -0,0 +1,52 @@ +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 static Input + location, + velocity; + public static Input + rotation, + rotationSpeed; + public static Vector getAbsLocation() { + if(location==null)return Vector.ZERO; + return location.get(); + } + public static Vector getVelocity() { + if(velocity==null)return Vector.ZERO; + return velocity.get(); + } + public static Angle getAbsRotation() { + if(rotation==null)return Angle.ZERO; + return rotation.get(); + } + public static Angle getRotationSpeed() { + if(rotationSpeed==null)return Angle.ZERO; + return rotationSpeed.get(); + } + + 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); + } +} 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..4ea0aee --- /dev/null +++ b/src/org/montclairrobotics/sprocket/frc/DashboardDebug.java @@ -0,0 +1,22 @@ +package org.montclairrobotics.sprocket.frc; + +import org.montclairrobotics.sprocket.core.IDebug; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class DashboardDebug implements IDebug{ + + @Override + public void debugStr(String key, String val) { + SmartDashboard.putString(key, val); + + } + + @Override + public void debugNum(String key, double val) { + SmartDashboard.putNumber(key, val); + + } + + public void update() {} +} 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/frc/DashboardSelector.java b/src/org/montclairrobotics/sprocket/frc/DashboardSelector.java new file mode 100644 index 0000000..079abb8 --- /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.IAutoSelector; + +public class DashboardSelector implements IAutoSelector{ + + @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/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..c4f81b4 100644 --- a/src/org/montclairrobotics/sprocket/control/JoystickButton.java +++ b/src/org/montclairrobotics/sprocket/frc/FRCButton.java @@ -1,11 +1,11 @@ -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.core.Button; +import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.Joystick; -public class JoystickButton extends Button{ +public class FRCButton extends Button{ private Joystick stick; private int id; @@ -16,7 +16,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 +27,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/frc/FRCEncoder.java b/src/org/montclairrobotics/sprocket/frc/FRCEncoder.java new file mode 100644 index 0000000..fb2f966 --- /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.IEncoder{ + + 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..a3585cb --- /dev/null +++ b/src/org/montclairrobotics/sprocket/frc/FRCJoystick.java @@ -0,0 +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/motors/Motor.java b/src/org/montclairrobotics/sprocket/frc/FRCMotor.java similarity index 88% rename from src/org/montclairrobotics/sprocket/motors/Motor.java rename to src/org/montclairrobotics/sprocket/frc/FRCMotor.java index 5e39e29..af4a567 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.IMotor; import org.montclairrobotics.sprocket.utils.Utils; import com.ctre.CANTalon; @@ -7,15 +8,13 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Talon; -public class Motor { +public class FRCMotor implements IMotor{ public enum MotorType { CANTALON, TALON, UNKNOWN } - - private SpeedController motor; private MotorType motorType; @@ -25,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"); } @@ -82,7 +81,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/frc/FRCRobot.java b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java new file mode 100644 index 0000000..0d6c78f --- /dev/null +++ b/src/org/montclairrobotics/sprocket/frc/FRCRobot.java @@ -0,0 +1,74 @@ +package org.montclairrobotics.sprocket.frc; + +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 FRCRobot extends IterativeRobot implements IRobot,Updatable{ + + public Sprocket sprocket; + + public FRCRobot() + { + sprocket=new Sprocket(this); + sprocket.debugger=new DashboardDebug(); + } + + @Override + public void startCompetition() { + super.startCompetition(); + } + @Override + public void robotInit() + { + sprocket.initS(); + } + @Override + public final void autonomousInit() { + sprocket.startS(MODE.AUTO); + } + + @Override + public final void teleopInit() { + sprocket.startS(MODE.TELEOP); + } + + @Override + public final void testInit() { + sprocket.startS(MODE.TEST); + } + + @Override + public final void disabledInit() { + sprocket.stopS(); + } + + @Override + public final void autonomousPeriodic() { + sprocket.updateS(); + } + + @Override + public final void teleopPeriodic() { + sprocket.updateS(); + } + + @Override + public final void testPeriodic() { + sprocket.updateS(); + } + + @Override + public final void disabledPeriodic() { + sprocket.disabledUpdateS(); + } +} 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/ftc/FTCButton.java b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java new file mode 100644 index 0000000..a9b64e0 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCButton.java @@ -0,0 +1,111 @@ +package org.montclairrobotics.sprocket.ftc; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.montclairrobotics.sprocket.core.Button; +import org.montclairrobotics.sprocket.ftc.FTCRobot.GAMEPAD; + +public class FTCButton extends Button { + public 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 static final float THREASHOLD = 50; + + private Gamepad gamepad; + private BUTTON button; + + public FTCButton(GAMEPAD gamepad,BUTTON button) + { + if(gamepad==GAMEPAD.A) + { + this.gamepad=FTCRobot.ftcGamepad1; + } + else + { + this.gamepad=FTCRobot.ftcGamepad2; + } + this.button=button; + } + 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/FTCDebug.java b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java new file mode 100644 index 0000000..20f2afc --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCDebug.java @@ -0,0 +1,32 @@ +package org.montclairrobotics.sprocket.ftc; + +import org.montclairrobotics.sprocket.core.IDebug; + +/** + * Created by Hymowitz on 9/16/2017. + */ + +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); + } + + @Override + 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/FTCJoystick.java b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java new file mode 100644 index 0000000..67e790b --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCJoystick.java @@ -0,0 +1,67 @@ +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{ + 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.ftcGamepad1; + } + else + { + this.gamepad=FTCRobot.ftcGamepad2; + } + this.stick=stick; + } + public FTCJoystick(Gamepad gamepad,STICK stick) + { + this.gamepad=gamepad; + this.stick=stick; + } + + 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); + } + } + 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); + } + } + + 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 new file mode 100644 index 0000000..30365fd --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCMotor.java @@ -0,0 +1,72 @@ +package org.montclairrobotics.sprocket.ftc; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.montclairrobotics.sprocket.core.IMotor; + +public class FTCMotor implements IMotor{ + + private DcMotor motor; + private double zeroPos=0; + + public FTCMotor(String motorID) + { + 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); + 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; + } +<<<<<<< HEAD + public boolean isCloseTo(double pos, double tolerance){ + return Math.abs(motor.getCurrentPosition() - pos) < tolerance; + } + public boolean isCloseTo(double pos){ + return isCloseTo(pos, 30); + } +======= + + public void forward(boolean forward) + { + motor.setDirection(forward? DcMotorSimple.Direction.FORWARD: DcMotorSimple.Direction.REVERSE); + } +>>>>>>> ad3daf0be793a22fd53e0e95637c114fc57628c3 +} diff --git a/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java new file mode 100644 index 0000000..8f6a1e9 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/ftc/FTCRobot.java @@ -0,0 +1,118 @@ +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; + +/** + +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; + 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! + + public static HardwareMap ftcHardwareMap; + public static Gamepad ftcGamepad1; + public static Gamepad ftcGamepad2; + public static Telemetry ftcTelemetry; + + /* + public static void setRobot() + { + if(robot==null) + { + robot=new MyTeleopRobotClass(); + } + }*/ + + @Override + public void init() { + if(robot==null) { + robot=this; + } + robot.ftcSetup(hardwareMap, gamepad1, gamepad2, telemetry); + if (sprocket==null) { + sprocket = new Sprocket(robot); + sprocket.debugger=new FTCDebug(robot); + sprocket.initS(); + } + sprocket.startS(MODE.DISABLED); + this.mode=mode.AUTO; + autoInit(); + } + + 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(); + } + public void sendTelemetry() + { + updateTelemetry(ftcTelemetry); + } + + public void ftcSetup(HardwareMap hardwareMap,Gamepad gamepad1,Gamepad gamepad2,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 new file mode 100644 index 0000000..99ec268 --- /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 com.qualcomm.robotcore.hardware.LightSensor sensor; + + public LightSensor(String sensorID) + { + sensor=FTCRobot.ftcHardwareMap.lightSensor.get("lightGround"); + } + public void enableLed(boolean enable) + { + sensor.enableLed(enable); + } + public Double get() + { + return sensor.getRawLightDetected(); + } +} 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/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/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 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/loop/DisabledUpdater.java b/src/org/montclairrobotics/sprocket/loop/DisabledUpdater.java new file mode 100644 index 0000000..6a6774e --- /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=new ArrayList(); + 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/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/motors/Module.java b/src/org/montclairrobotics/sprocket/motors/Module.java index 4db94a3..5992f18 100644 --- a/src/org/montclairrobotics/sprocket/motors/Module.java +++ b/src/org/montclairrobotics/sprocket/motors/Module.java @@ -1,9 +1,9 @@ package org.montclairrobotics.sprocket.motors; +import org.montclairrobotics.sprocket.core.IMotor; 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 @@ -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); } @@ -74,18 +74,24 @@ public Module(Motor... 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"); pid.setTarget(power); power=(power+pid.get())/maxSpeed.get(); } - for(Motor motor:motors) + for(IMotor motor:motors) { motor.set(power); } - Debug.num("module-" + moduleId, power); + //Debug.num("module-" + moduleId, power); + debug(); + } + + public void debug() + { + Debug.num("module-" + moduleId, power); } /** @@ -97,7 +103,7 @@ public SEncoder getEnc() { public Distance getDistance() { if(enc == null) return Distance.ZERO; - return enc.getInches(); + return enc.getDistance(); } public boolean hasEncoder() { diff --git a/src/org/montclairrobotics/sprocket/motors/SEncoder.java b/src/org/montclairrobotics/sprocket/motors/SEncoder.java index f5f4559..26681b8 100644 --- a/src/org/montclairrobotics/sprocket/motors/SEncoder.java +++ b/src/org/montclairrobotics/sprocket/motors/SEncoder.java @@ -1,20 +1,20 @@ package org.montclairrobotics.sprocket.motors; +import org.montclairrobotics.sprocket.core.IEncoder; 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 { - private Encoder enc; + private IEncoder enc; private int eId; 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,9 +23,9 @@ 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) { + public SEncoder(IEncoder e, double ticksPerInch) { enc = e; this.ticksPerInch = ticksPerInch; } @@ -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 IEncoder getRawEncoder() { return enc; } diff --git a/src/org/montclairrobotics/sprocket/states/State.java b/src/org/montclairrobotics/sprocket/states/State.java deleted file mode 100644 index 9c2ee0c..0000000 --- a/src/org/montclairrobotics/sprocket/states/State.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.montclairrobotics.sprocket.states; - -public interface State { - void start(); - void stop(); - void stateUpdate(); - boolean isDone(); -} diff --git a/src/org/montclairrobotics/sprocket/test/MecanumTest.java b/src/org/montclairrobotics/sprocket/test/MecanumTest.java new file mode 100644 index 0000000..acbafe5 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/MecanumTest.java @@ -0,0 +1,91 @@ +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.CIRCLE),new Input(){ + + @Override + public Double get() { + // TODO Auto-generated method stub + return -0.5; + }})); + dt.setMapper(new UniversalMapper()); + } + + @Override + public void enableMode(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(); + } + + @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 new file mode 100644 index 0000000..99b8256 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/SimpleTest.java @@ -0,0 +1,81 @@ +package org.montclairrobotics.sprocket.test; + +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; +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 ArcadeDriveInput(new TestJoystick(TEST.CIRCLE))); + dt.setMapper(new TankMapper()); + } + + @Override + public void enableMode(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 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/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..36f11df --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/TestDebug.java @@ -0,0 +1,18 @@ +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); + } + + public void update(){} +} diff --git a/src/org/montclairrobotics/sprocket/test/TestJoystick.java b/src/org/montclairrobotics/sprocket/test/TestJoystick.java new file mode 100644 index 0000000..0cd660e --- /dev/null +++ b/src/org/montclairrobotics/sprocket/test/TestJoystick.java @@ -0,0 +1,34 @@ +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, TINY}; + int i=-1; + + private TEST test; + + public TestJoystick(TEST test) + { + this.test=test; + } + + @Override + public Vector get() { + i++; + switch(test) + { + case CIRCLE: + return new XY(Math.sin(i*Math.PI/4),Math.cos(i*Math.PI/4)); + case RANDOM: + return new XY(Math.random()*2-1,Math.random()*2-1); + case TINY: + return new XY(0.01,0.01); + default: + return Vector.ZERO; + } + } + +} 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..de38ed5 --- /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 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 diff --git a/src/org/montclairrobotics/sprocket/utils/Debug.java b/src/org/montclairrobotics/sprocket/utils/Debug.java index 8627e59..efd258f 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.montclairrobotics.sprocket.core.Sprocket; + public class Debug { @@ -9,13 +10,13 @@ public class Debug { public static void num(String key,double value) { if(DEBUG_MODE) - SmartDashboard.putNumber(key, value); + Sprocket.debugger.debugNum(key, value); } public static void string(String key,String value) { if(DEBUG_MODE) - SmartDashboard.putString(key, value); + Sprocket.debugger.debugStr(key, value); } public static void msg(String key,double value) 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/Input.java b/src/org/montclairrobotics/sprocket/utils/Input.java index 47f5678..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 neg(Input a) - { - return new Input(){ - @Override - public Double get() { - return -a.get(); - }}; - } } diff --git a/src/org/montclairrobotics/sprocket/utils/InputDouble.java b/src/org/montclairrobotics/sprocket/utils/InputDouble.java new file mode 100644 index 0000000..a64ad80 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/utils/InputDouble.java @@ -0,0 +1,34 @@ +package org.montclairrobotics.sprocket.utils; + +public abstract class InputDouble extends Number implements Input{ + + /** + * + */ + private static final long serialVersionUID = 7875067748687693370L; + + @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(); + } + +} 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/PID.java b/src/org/montclairrobotics/sprocket/utils/PID.java index 1e47807..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; @@ -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 diff --git a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java index 655ef42..1828bc2 100644 --- a/src/org/montclairrobotics/sprocket/utils/PIDTuner.java +++ b/src/org/montclairrobotics/sprocket/utils/PIDTuner.java @@ -1,20 +1,17 @@ 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.actions.Action; +import org.montclairrobotics.sprocket.core.Button; public class PIDTuner extends PID { private Input TempP; - private Button test; - private Button apply; - private Button run; - private DashboardInput realP,realI,realD; + private Input test; + private Input apply; + private Input run; 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; @@ -23,22 +20,40 @@ public PIDTuner(Input TempP,Input cyclesPer10Sec,Button test,But 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");*/ - apply.setPressAction(new ButtonAction(){ + new Button(apply).setAction(new Action(){ @Override - public void onAction() { + 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 + }}); } - public PIDTuner(Button 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) @@ -46,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() { @@ -64,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 { 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..514f96d --- /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,final 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()); + } + +} 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