From 6be98c76e924eb888296133eabebe0d0c4bd1963 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Dec 2016 13:15:09 -0500 Subject: [PATCH 1/6] Pipelines! --- .../sprocket/drive/DriveModule.java | 91 ++++++++++--------- .../sprocket/drive/DrivePipeline.java | 42 --------- .../sprocket/drive/DrivePipelineStep.java | 7 -- .../sprocket/drive/DriveTrain.java | 17 ---- .../sprocket/drive/DriveTrainBuilder.java | 7 -- .../sprocket/drive/DriveTrainInput.java | 25 ----- .../sprocket/drive/DriveTrainMapper.java | 7 -- .../sprocket/drive/DriveTrainTarget.java | 3 +- .../sprocket/drive/Drivetrain.java | 40 ++++++++ .../sprocket/geometry/Angle.java | 4 +- .../sprocket/geometry/Degrees.java | 6 +- .../sprocket/geometry/Polar.java | 11 ++- .../sprocket/geometry/Radians.java | 6 +- .../sprocket/geometry/Vector.java | 6 +- .../sprocket/geometry/XY.java | 11 ++- .../sprocket/pipeline/Pipeline.java | 36 ++++++++ .../sprocket/pipeline/Step.java | 5 + 17 files changed, 168 insertions(+), 156 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/drive/DrivePipeline.java delete mode 100644 src/org/montclairrobotics/sprocket/drive/DrivePipelineStep.java delete mode 100644 src/org/montclairrobotics/sprocket/drive/DriveTrain.java delete mode 100644 src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java delete mode 100644 src/org/montclairrobotics/sprocket/drive/DriveTrainInput.java delete mode 100644 src/org/montclairrobotics/sprocket/drive/DriveTrainMapper.java create mode 100644 src/org/montclairrobotics/sprocket/drive/Drivetrain.java create mode 100644 src/org/montclairrobotics/sprocket/pipeline/Pipeline.java create mode 100644 src/org/montclairrobotics/sprocket/pipeline/Step.java diff --git a/src/org/montclairrobotics/sprocket/drive/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index 72c2995..9cb618a 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -1,47 +1,54 @@ package org.montclairrobotics.sprocket.drive; -import edu.wpi.first.wpilibj.SpeedController; import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Inch; +import org.montclairrobotics.sprocket.geometry.Polar; +import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.utils.PID; - -public class DriveModule extends Motor { - - private Angle forceAngle; - private Vector offset; - private Inch maxSpeed; - - public DriveModule(SpeedController motor, - Angle forceAngle, Vector offset, - SEncoder enc, PID pid, Inch maxSpeed, - boolean invert) { - super(motor, enc, pid, invert); - this.forceAngle = forceAngle; - this.offset = offset; - this.maxSpeed = maxSpeed; - } - - public DriveModule(SpeedController motor, - Angle forceAngle, Vector offset, - SEncoder enc, PID pid, Inch maxSpeed) { - this(motor, forceAngle, offset, enc, pid, maxSpeed, false); - } - - public DriveModule(SpeedController motor, - Angle forceAngle, Vector offset) { - this(motor, forceAngle, offset, null, null, null, false); - } - - public Angle getForceAngle() { - return forceAngle; - } - - public Inch getMaxSpeed() { - return maxSpeed; - } - - public Vector getOffset() { - return offset; - } +import org.montclairrobotics.sprocket.loop.Priority; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; +import org.montclairrobotics.sprocket.pipeline.Pipeline; + +public class DriveModule implements Updatable{ + private Motor[] motors; + private Pipeline outputPipeline=new Pipeline(0.0); + private Vector force; + private Vector offset; + private double tgtForce; + + public DriveModule(Vector force,Vector offset,Motor... motors){ + this.motors=motors; + Updater.add(this, Priority.OUTPUT); + } + + public Angle setDirection(Vector tgtDirection) + { + tgtForce=0; + return addForce(tgtDirection); + } + public void setTurn(Angle turn) + { + Vector turnForce=offset.rotate(Angle.QUARTER).scale(turn.toRadians()*offset.getMagnitude(),true); + addForce(turnForce); + } + public Angle addForce(Vector f) + { + tgtForce+=force.dotProduct(f)/force.getMagnitude(); + return new Radians(force.scale(tgtForce, true).rotate(offset.getAngle().negative()).getX()/offset.getMagnitude()); + } + + public void setOutputPipeline(Pipeline p) + { + this.outputPipeline=p; + } + + @Override + public void update() { + tgtForce=outputPipeline.get(tgtForce); + for(Motor motor:motors) + { + motor.set(tgtForce); + } + } + } diff --git a/src/org/montclairrobotics/sprocket/drive/DrivePipeline.java b/src/org/montclairrobotics/sprocket/drive/DrivePipeline.java deleted file mode 100644 index 7fd55ff..0000000 --- a/src/org/montclairrobotics/sprocket/drive/DrivePipeline.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.montclairrobotics.sprocket.drive; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; - -public class DrivePipeline { - - private List steps; - - public DrivePipeline(DrivePipelineStep... stepList) { - steps = Arrays.asList(stepList); - } - - public DrivePipeline() { - steps = new ArrayList<>(); - } - - - public void setPipeline(List steps) { - this.steps = steps; - } - - public void addStepFirst(DrivePipelineStep step) { - steps.add(0, step); - } - - public void addStepLast(DrivePipelineStep step) { - steps.add(step); - } - - public void addStep(DrivePipelineStep step) { - steps.add(step); - } - - - public void run(DriveTrainTarget target) { - for(DrivePipelineStep step : steps) { - step.run(target); - } - } -} diff --git a/src/org/montclairrobotics/sprocket/drive/DrivePipelineStep.java b/src/org/montclairrobotics/sprocket/drive/DrivePipelineStep.java deleted file mode 100644 index 861eef6..0000000 --- a/src/org/montclairrobotics/sprocket/drive/DrivePipelineStep.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.montclairrobotics.sprocket.drive; - -public interface DrivePipelineStep { - - void run(DriveTrainTarget driveTarget); - -} diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java b/src/org/montclairrobotics/sprocket/drive/DriveTrain.java deleted file mode 100644 index 2638de2..0000000 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrain.java +++ /dev/null @@ -1,17 +0,0 @@ -package org.montclairrobotics.sprocket.drive; - -import java.util.ArrayList; - -public class DriveTrain { - - private ArrayList driveModules; - private DrivePipeline pipeline; - private DriveTrainMapper powerMapper; - private DriveTrainInput input; - - - public DriveTrain(DriveModule[] modules, DriveTrainInput input, DrivePipeline pipeline, DriveTrainMapper mapper) { - - } - -} diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java deleted file mode 100644 index efe35a4..0000000 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.montclairrobotics.sprocket.drive; - -public class DriveTrainBuilder { - - - -} diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainInput.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainInput.java deleted file mode 100644 index fbce8ec..0000000 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainInput.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.montclairrobotics.sprocket.drive; - -import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.loop.Priority; -import org.montclairrobotics.sprocket.loop.Updatable; -import org.montclairrobotics.sprocket.loop.Updater; - -public abstract class DriveTrainInput implements Updatable { - - protected MotorInputType inputType; - - public DriveTrainInput(MotorInputType type) { - inputType = type; - Updater.add(this, Priority.CONTROL); - } - - public abstract Vector getDirection(); - - public abstract double getTurn(); - - public MotorInputType getInputType() { - return inputType; - } -} - diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainMapper.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainMapper.java deleted file mode 100644 index 710baca..0000000 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainMapper.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.montclairrobotics.sprocket.drive; - -public interface DriveTrainMapper { - - void map(DriveTrainTarget driveTarget, DriveModule[] driveModules); - -} diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainTarget.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainTarget.java index d85fca1..95aee57 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainTarget.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveTrainTarget.java @@ -5,7 +5,8 @@ public class DriveTrainTarget { - private Vector direction; + public static final DriveTrainTarget ZERO = new DriveTrainTarget(Vector.ZERO,Angle.ZERO,MotorInputType.SPEED); + private Vector direction; private Angle turn; private MotorInputType inputType; diff --git a/src/org/montclairrobotics/sprocket/drive/Drivetrain.java b/src/org/montclairrobotics/sprocket/drive/Drivetrain.java new file mode 100644 index 0000000..14717e8 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/drive/Drivetrain.java @@ -0,0 +1,40 @@ +package org.montclairrobotics.sprocket.drive; + +import java.util.ArrayList; + +import org.montclairrobotics.sprocket.geometry.Angle; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.pipeline.Pipeline; +import org.montclairrobotics.sprocket.pipeline.Step; + +public class Drivetrain implements Updatable{ + private Pipeline input; + private DriveModule[] modules; + + public Drivetrain(DriveModule[] modules) + { + this.input=new Pipeline(DriveTrainTarget.ZERO); + this.modules=modules; + } + + public void setInput(Pipeline input) + { + this.input=input; + } + + @Override + public void update() { + DriveTrainTarget target=input.get(); + Vector tgtDirection=target.getDirection(); + Angle tgtTurn=target.getTurn(); + for(DriveModule module:modules) + { + tgtTurn=tgtTurn.add(module.setDirection(tgtDirection)); + } + for(DriveModule module:modules) + { + module.setTurn(tgtTurn); + } + } +} diff --git a/src/org/montclairrobotics/sprocket/geometry/Angle.java b/src/org/montclairrobotics/sprocket/geometry/Angle.java index 78332b9..0186af8 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Angle.java +++ b/src/org/montclairrobotics/sprocket/geometry/Angle.java @@ -2,7 +2,8 @@ public interface Angle { - /** + Angle ZERO=new Degrees(0),QUARTER = new Degrees(90),HALF = new Degrees(180); + /** * Returns the represented angle in degrees * @return the angle in degrees */ @@ -32,6 +33,7 @@ public interface Angle { * Returns the negative of this angle * @return the negative of this angle */ + Angle negative(); Angle opposite(); diff --git a/src/org/montclairrobotics/sprocket/geometry/Degrees.java b/src/org/montclairrobotics/sprocket/geometry/Degrees.java index fec5e3b..1b8ed63 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Degrees.java +++ b/src/org/montclairrobotics/sprocket/geometry/Degrees.java @@ -29,7 +29,11 @@ public Angle subtract(Angle a) { } @Override - public Angle opposite() { + public Angle negative() { return new Degrees(-toDegrees()); } + public Angle opposite() + { + return new Degrees(180+toDegrees()); + } } diff --git a/src/org/montclairrobotics/sprocket/geometry/Polar.java b/src/org/montclairrobotics/sprocket/geometry/Polar.java index 748dea7..d82d365 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Polar.java +++ b/src/org/montclairrobotics/sprocket/geometry/Polar.java @@ -41,12 +41,19 @@ public Vector subtract(Vector v) { } @Override - public Vector scale(double s) { - return new Polar(magnitude * s, angle); + public Vector scale(double s,boolean norm) { + if(norm) + return new Polar(s, angle); + else + return new Polar(magnitude * s, angle); } @Override public double dotProduct(Vector v) { return (getX() * v.getX()) + (getY() * v.getY()); } + + public Vector rotate(Angle a) { + return new Polar(magnitude,angle.add(a)); + } } diff --git a/src/org/montclairrobotics/sprocket/geometry/Radians.java b/src/org/montclairrobotics/sprocket/geometry/Radians.java index 7215bed..1a5b37b 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Radians.java +++ b/src/org/montclairrobotics/sprocket/geometry/Radians.java @@ -28,8 +28,12 @@ public Angle subtract(Angle a) { return new Radians(toRadians() - a.toRadians()); } + public Angle opposite() + { + return new Radians(2*Math.PI+toRadians()); + } @Override - public Angle opposite() { + public Angle negative() { return new Radians(-toRadians()); } } diff --git a/src/org/montclairrobotics/sprocket/geometry/Vector.java b/src/org/montclairrobotics/sprocket/geometry/Vector.java index 26ee854..694d835 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Vector.java +++ b/src/org/montclairrobotics/sprocket/geometry/Vector.java @@ -2,13 +2,15 @@ public interface Vector { - double getMagnitude(); + Vector ZERO = new XY(0,0); + double getMagnitude(); Angle getAngle(); double getX(); double getY(); Vector add(Vector v); Vector subtract(Vector v); - Vector scale(double s); + Vector scale(double s,boolean norm); + Vector rotate(Angle a); double dotProduct(Vector v); } diff --git a/src/org/montclairrobotics/sprocket/geometry/XY.java b/src/org/montclairrobotics/sprocket/geometry/XY.java index cb9d1b6..7c91427 100644 --- a/src/org/montclairrobotics/sprocket/geometry/XY.java +++ b/src/org/montclairrobotics/sprocket/geometry/XY.java @@ -41,7 +41,11 @@ public Vector subtract(Vector v) { } @Override - public Vector scale(double s) { + public Vector scale(double s,boolean norm) { + if(norm&&getMagnitude()>0) + { + s/=getMagnitude(); + } return new XY(x * s, y * s); } @@ -49,4 +53,9 @@ public Vector scale(double s) { public double dotProduct(Vector v) { return (x * v.getX()) + (y * v.getY()); } + + @Override + public Vector rotate(Angle a) { + return new Polar(getMagnitude(),getAngle().add(a)); + } } diff --git a/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java b/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java new file mode 100644 index 0000000..21adfff --- /dev/null +++ b/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java @@ -0,0 +1,36 @@ +package org.montclairrobotics.sprocket.pipeline; + +import java.util.ArrayList; + +import org.montclairrobotics.sprocket.drive.DriveTrainInput; + +public class Pipeline { + + private ArrayList> steps; + private T init; + + public Pipeline(T init,ArrayList> steps) + { + this.steps=steps; + this.init=init; + } + + public Pipeline(T init) { + this.steps=new ArrayList>(); + this.init=init; + } + + public T get() + { + return get(init); + } + + public T get(T res) + { + for(Step step:steps) + { + res=step.get(init); + } + return res; + } +} diff --git a/src/org/montclairrobotics/sprocket/pipeline/Step.java b/src/org/montclairrobotics/sprocket/pipeline/Step.java new file mode 100644 index 0000000..85b3a30 --- /dev/null +++ b/src/org/montclairrobotics/sprocket/pipeline/Step.java @@ -0,0 +1,5 @@ +package org.montclairrobotics.sprocket.pipeline; + +public interface Step { + public T get(T in); +} From 8e9e6ec840c0d1f7123714d36377036b9d09f121 Mon Sep 17 00:00:00 2001 From: jackhymowitz Date: Tue, 27 Dec 2016 17:07:57 -0500 Subject: [PATCH 2/6] It compiles! It must work. --- .../sprocket/SprocketRobot.java | 3 +- .../sprocket/control/ArcadeDriveInput.java | 51 +++++++++++-------- .../sprocket/drive/DriveModule.java | 1 - .../sprocket/drive/Drivetrain.java | 3 -- .../sprocket/drive/Motor.java | 4 +- .../sprocket/drive/SEncoder.java | 3 +- .../sprocket/geometry/Angle.java | 2 + .../sprocket/geometry/Degrees.java | 5 ++ .../sprocket/geometry/Radians.java | 5 ++ .../sprocket/httpserver/Http.java | 2 +- .../sprocket/pipeline/Pipeline.java | 2 - .../sprocket/utils/Grip.java | 6 +-- 12 files changed, 51 insertions(+), 36 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/SprocketRobot.java b/src/org/montclairrobotics/sprocket/SprocketRobot.java index 72c1ccf..6c9da92 100644 --- a/src/org/montclairrobotics/sprocket/SprocketRobot.java +++ b/src/org/montclairrobotics/sprocket/SprocketRobot.java @@ -1,8 +1,9 @@ package org.montclairrobotics.sprocket; -import edu.wpi.first.wpilibj.IterativeRobot; import org.montclairrobotics.sprocket.loop.Updater; +import edu.wpi.first.wpilibj.IterativeRobot; + public class SprocketRobot extends IterativeRobot { @Override diff --git a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java index 136ba20..a251282 100644 --- a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java @@ -1,53 +1,60 @@ package org.montclairrobotics.sprocket.control; -import edu.wpi.first.wpilibj.Joystick; +import org.montclairrobotics.sprocket.drive.DriveTrainTarget; import org.montclairrobotics.sprocket.drive.MotorInputType; -import org.montclairrobotics.sprocket.drive.DriveTrainInput; +import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Inch; import org.montclairrobotics.sprocket.geometry.Polar; import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.geometry.XY; +import org.montclairrobotics.sprocket.pipeline.Step; + +import edu.wpi.first.wpilibj.Joystick; -public class ArcadeDriveInput extends DriveTrainInput { +public class ArcadeDriveInput implements Step { private Joystick stick; - private boolean speedControl; - private double maxSpeed; + MotorInputType inputType; + + //private boolean speedControl; + private Inch maxSpeed; + private Angle maxTurn; private Vector dir; - private double turn; + private Angle turn; public ArcadeDriveInput(Joystick stick) { - super(MotorInputType.PERCENT); - speedControl = false; + inputType=MotorInputType.PERCENT; this.stick = stick; + this.maxSpeed=new Inch(1); + this.maxTurn=Angle.QUARTER; } - public ArcadeDriveInput(Joystick stick, Inch maxSpeed) { - super(MotorInputType.SPEED); - speedControl = true; + public ArcadeDriveInput(Joystick stick, Inch maxSpeed, Angle maxTurn) { + inputType=MotorInputType.SPEED; this.stick = stick; - this.maxSpeed = maxSpeed.get(); + this.maxSpeed = maxSpeed; + this.maxTurn=maxTurn; } - @Override public void update() { - turn = stick.getX(); - dir = new Polar(stick.getMagnitude(), new Degrees(stick.getDirectionDegrees())); - - if(speedControl) { - dir.scale(maxSpeed); - } + maxTurn = maxTurn.times(stick.getX()); + dir = new XY(0,stick.getY()*maxSpeed.get()); } - @Override public Vector getDirection() { return dir; } - @Override - public double getTurn() { + public Angle getTurn() { return turn; } + + @Override + public DriveTrainTarget get(DriveTrainTarget in) { + // TODO Auto-generated method stub + return new DriveTrainTarget(getDirection(),getTurn(),inputType); + } } diff --git a/src/org/montclairrobotics/sprocket/drive/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index 9cb618a..2003b69 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -1,7 +1,6 @@ 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; import org.montclairrobotics.sprocket.loop.Priority; diff --git a/src/org/montclairrobotics/sprocket/drive/Drivetrain.java b/src/org/montclairrobotics/sprocket/drive/Drivetrain.java index 14717e8..41b3ec1 100644 --- a/src/org/montclairrobotics/sprocket/drive/Drivetrain.java +++ b/src/org/montclairrobotics/sprocket/drive/Drivetrain.java @@ -1,12 +1,9 @@ package org.montclairrobotics.sprocket.drive; -import java.util.ArrayList; - import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.pipeline.Pipeline; -import org.montclairrobotics.sprocket.pipeline.Step; public class Drivetrain implements Updatable{ private Pipeline input; diff --git a/src/org/montclairrobotics/sprocket/drive/Motor.java b/src/org/montclairrobotics/sprocket/drive/Motor.java index 562e85b..f96ff12 100644 --- a/src/org/montclairrobotics/sprocket/drive/Motor.java +++ b/src/org/montclairrobotics/sprocket/drive/Motor.java @@ -1,11 +1,11 @@ package org.montclairrobotics.sprocket.drive; +import org.montclairrobotics.sprocket.utils.PID; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Talon; -import org.montclairrobotics.sprocket.utils.PID; -public class Motor { +public class Motor{ public enum MotorType { CANTALON, diff --git a/src/org/montclairrobotics/sprocket/drive/SEncoder.java b/src/org/montclairrobotics/sprocket/drive/SEncoder.java index 8067d38..582b07b 100644 --- a/src/org/montclairrobotics/sprocket/drive/SEncoder.java +++ b/src/org/montclairrobotics/sprocket/drive/SEncoder.java @@ -1,8 +1,9 @@ package org.montclairrobotics.sprocket.drive; -import edu.wpi.first.wpilibj.Encoder; import org.montclairrobotics.sprocket.geometry.Inch; +import edu.wpi.first.wpilibj.Encoder; + public class SEncoder { private Encoder enc; diff --git a/src/org/montclairrobotics/sprocket/geometry/Angle.java b/src/org/montclairrobotics/sprocket/geometry/Angle.java index 0186af8..311be7a 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Angle.java +++ b/src/org/montclairrobotics/sprocket/geometry/Angle.java @@ -36,5 +36,7 @@ public interface Angle { Angle negative(); Angle opposite(); + Angle times(double x); + } diff --git a/src/org/montclairrobotics/sprocket/geometry/Degrees.java b/src/org/montclairrobotics/sprocket/geometry/Degrees.java index 1b8ed63..58dfe4f 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Degrees.java +++ b/src/org/montclairrobotics/sprocket/geometry/Degrees.java @@ -36,4 +36,9 @@ public Angle opposite() { return new Degrees(180+toDegrees()); } + + @Override + public Angle times(double x) { + return new Degrees(toDegrees()*x); + } } diff --git a/src/org/montclairrobotics/sprocket/geometry/Radians.java b/src/org/montclairrobotics/sprocket/geometry/Radians.java index 1a5b37b..cfc097b 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Radians.java +++ b/src/org/montclairrobotics/sprocket/geometry/Radians.java @@ -36,4 +36,9 @@ public Angle opposite() public Angle negative() { return new Radians(-toRadians()); } + + @Override + public Angle times(double x) { + return new Radians(toRadians()*x); + } } diff --git a/src/org/montclairrobotics/sprocket/httpserver/Http.java b/src/org/montclairrobotics/sprocket/httpserver/Http.java index c34142c..8107b86 100644 --- a/src/org/montclairrobotics/sprocket/httpserver/Http.java +++ b/src/org/montclairrobotics/sprocket/httpserver/Http.java @@ -17,10 +17,10 @@ import java.io.OutputStream; import java.net.InetSocketAddress; +import com.sun.net.httpserver.Headers; import com.sun.net.httpserver.HttpExchange; import com.sun.net.httpserver.HttpHandler; import com.sun.net.httpserver.HttpServer; -import com.sun.net.httpserver.Headers; public class Http implements Runnable{ diff --git a/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java b/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java index 21adfff..13a59b9 100644 --- a/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java +++ b/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java @@ -2,8 +2,6 @@ import java.util.ArrayList; -import org.montclairrobotics.sprocket.drive.DriveTrainInput; - public class Pipeline { private ArrayList> steps; diff --git a/src/org/montclairrobotics/sprocket/utils/Grip.java b/src/org/montclairrobotics/sprocket/utils/Grip.java index ca8cc89..7d45fb1 100644 --- a/src/org/montclairrobotics/sprocket/utils/Grip.java +++ b/src/org/montclairrobotics/sprocket/utils/Grip.java @@ -1,8 +1,8 @@ package org.montclairrobotics.sprocket.utils; -import org.montclairrobotics.sprocket.updater.Priority; -import org.montclairrobotics.sprocket.updater.Updatable; -import org.montclairrobotics.sprocket.updater.Updater; +import org.montclairrobotics.sprocket.loop.Priority; +import org.montclairrobotics.sprocket.loop.Updatable; +import org.montclairrobotics.sprocket.loop.Updater; import edu.wpi.first.wpilibj.networktables.NetworkTable; From 61effd0b42f7e2db70ef722368b1976fe72d96f7 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Wed, 11 Jan 2017 15:10:44 -0500 Subject: [PATCH 3/6] New Jack Branch --- .../sprocket/geometry/Distance.java | 29 ++++ .../sprocket/geometry/IN.java | 10 ++ .../sprocket/geometry/Inch.java | 54 -------- .../sprocket/geometry/Polar.java | 28 ++-- .../sprocket/geometry/Side.java | 6 - .../sprocket/geometry/Vector.java | 8 +- .../sprocket/geometry/XY.java | 34 ++--- .../sprocket/httpserver/Http.java | 97 -------------- .../sprocket/httpserver/MJpg.java | 124 ------------------ 9 files changed, 78 insertions(+), 312 deletions(-) create mode 100644 src/org/montclairrobotics/sprocket/geometry/Distance.java create mode 100644 src/org/montclairrobotics/sprocket/geometry/IN.java delete mode 100644 src/org/montclairrobotics/sprocket/geometry/Inch.java delete mode 100644 src/org/montclairrobotics/sprocket/geometry/Side.java delete mode 100644 src/org/montclairrobotics/sprocket/httpserver/Http.java delete mode 100644 src/org/montclairrobotics/sprocket/httpserver/MJpg.java diff --git a/src/org/montclairrobotics/sprocket/geometry/Distance.java b/src/org/montclairrobotics/sprocket/geometry/Distance.java new file mode 100644 index 0000000..5e5ddee --- /dev/null +++ b/src/org/montclairrobotics/sprocket/geometry/Distance.java @@ -0,0 +1,29 @@ +package org.montclairrobotics.sprocket.geometry; + +public class Distance { + public static final Distance ZERO = new Distance(0); + public static final Distance IN = new Distance(1); + public static final Distance CM = new Distance(1/2.54); + public static final Distance M = new Distance(100,CM); + public static final Distance FT = new Distance(12); + + private double inches; + + public Distance(double i) + { + this.inches=i; + } + public Distance(double d,Distance unit) + { + this(d*unit.get()); + } + + public double get() + { + return inches; + } + public double get(Distance unit) + { + return inches/unit.get(); + } +} diff --git a/src/org/montclairrobotics/sprocket/geometry/IN.java b/src/org/montclairrobotics/sprocket/geometry/IN.java new file mode 100644 index 0000000..b3e998a --- /dev/null +++ b/src/org/montclairrobotics/sprocket/geometry/IN.java @@ -0,0 +1,10 @@ +package org.montclairrobotics.sprocket.geometry; + +public class IN extends Distance{ + + public IN(double i) { + super(i); + // TODO Auto-generated constructor stub + } + +} diff --git a/src/org/montclairrobotics/sprocket/geometry/Inch.java b/src/org/montclairrobotics/sprocket/geometry/Inch.java deleted file mode 100644 index 05be13f..0000000 --- a/src/org/montclairrobotics/sprocket/geometry/Inch.java +++ /dev/null @@ -1,54 +0,0 @@ -package org.montclairrobotics.sprocket.geometry; - -public class Inch { - - public static final double TO_CM = 2.54; - public static final double TO_M = TO_CM/100; - public static final double TO_FT = 1/12; - - private double inches; - - - public Inch(double inches) { - this.inches = inches; - } - - - - - public static Inch fromCM(double cm) { - return new Inch(cm/TO_CM); - } - - public static Inch fromM(double m) { - return new Inch(m / TO_M); - } - - public static Inch fromFeet(double ft) { - return new Inch(ft / TO_FT); - } - - - - - public double getInches() { - return inches; - } - - public double get() { - return inches; - } - - public double getCM() { - return inches * TO_CM; - } - - public double getM() { - return inches * TO_M; - } - - public double getFeet() { - return inches * TO_FT; - } - -} diff --git a/src/org/montclairrobotics/sprocket/geometry/Polar.java b/src/org/montclairrobotics/sprocket/geometry/Polar.java index 78f2266..1ed07a1 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Polar.java +++ b/src/org/montclairrobotics/sprocket/geometry/Polar.java @@ -2,16 +2,20 @@ public class Polar implements Vector { - private double magnitude; + private Distance magnitude; private Angle angle; - public Polar(double mag, Angle a) { + public Polar(double mag,Angle a) + { + this(new IN(mag),a); + } + public Polar(Distance mag, Angle a) { magnitude = mag; angle = a; } @Override - public double getMagnitude() { + public Distance getMagnitude() { return magnitude; } @@ -21,23 +25,23 @@ public Angle getAngle() { } @Override - public double getX() { - return magnitude * Math.sin(angle.toRadians()); + public Distance getX() { + return new IN(magnitude.get() * Math.sin(angle.toRadians())); } @Override - public double getY() { - return magnitude * Math.cos(angle.toRadians()); + public Distance getY() { + return new IN(magnitude.get() * Math.cos(angle.toRadians())); } @Override public Vector add(Vector v) { - return new XY(getX() + v.getX(), getY() + v.getY()); + return new XY(getX().get() + v.getX().get(), getY().get() + v.getY().get()); } @Override public Vector subtract(Vector v) { - return new XY(getX() - v.getX(), getY() - v.getY()); + return new XY(getX().get() - v.getX().get(), getY().get() - v.getY().get()); } @Override @@ -45,12 +49,12 @@ public Vector scale(double s,boolean norm) { if(norm) return new Polar(s,angle); else - return new Polar(magnitude * s, angle); + return new Polar(magnitude.get() * s, angle); } @Override - public double dotProduct(Vector v) { - return (getX() * v.getX()) + (getY() * v.getY()); + public Distance dotProduct(Vector v) { + return new IN((getX().get() * v.getX().get()) + (getY().get() * v.getY().get())); } @Override diff --git a/src/org/montclairrobotics/sprocket/geometry/Side.java b/src/org/montclairrobotics/sprocket/geometry/Side.java deleted file mode 100644 index 6f2336e..0000000 --- a/src/org/montclairrobotics/sprocket/geometry/Side.java +++ /dev/null @@ -1,6 +0,0 @@ -package org.montclairrobotics.sprocket.geometry; - -public enum Side { - LEFT, - RIGHT -} diff --git a/src/org/montclairrobotics/sprocket/geometry/Vector.java b/src/org/montclairrobotics/sprocket/geometry/Vector.java index b7f5c60..cc4fda0 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Vector.java +++ b/src/org/montclairrobotics/sprocket/geometry/Vector.java @@ -2,14 +2,14 @@ public interface Vector { - double getMagnitude(); + Distance getMagnitude(); Angle getAngle(); - double getX(); - double getY(); + Distance getX(); + Distance getY(); Vector add(Vector v); Vector subtract(Vector v); Vector scale(double s,boolean norm); - double dotProduct(Vector v); + Distance dotProduct(Vector v); Vector rotate(Angle angle); } diff --git a/src/org/montclairrobotics/sprocket/geometry/XY.java b/src/org/montclairrobotics/sprocket/geometry/XY.java index faa67f9..2006da5 100644 --- a/src/org/montclairrobotics/sprocket/geometry/XY.java +++ b/src/org/montclairrobotics/sprocket/geometry/XY.java @@ -2,54 +2,58 @@ public class XY implements Vector { - private double x; - private double y; + private Distance x; + private Distance y; - public XY(double x, double y) { + public XY(double x,double y) + { + this(new IN(x),new IN(y)); + } + public XY(Distance x, Distance y) { this.x = x; this.y = y; } @Override - public double getMagnitude() { - return Math.sqrt(x*x + y*y); + public Distance getMagnitude() { + return new IN(Math.sqrt(x.get()*x.get() + y.get()*y.get())); } @Override public Angle getAngle() { - return new Radians(Math.atan(x/y)); + return new Radians(Math.atan(x.get()/y.get())); } @Override - public double getX() { + public Distance getX() { return x; } @Override - public double getY() { + public Distance getY() { return y; } @Override public Vector add(Vector v) { - return new XY(v.getX() + x, v.getY() + y); + return new XY(v.getX().get() + x.get(), v.getY().get() + y.get()); } @Override public Vector subtract(Vector v) { - return new XY(x - v.getX(), y - v.getY()); + return new XY(x.get() - v.getX().get(), y.get() - v.getY().get()); } @Override public Vector scale(double s,boolean norm) { - if(norm&&getMagnitude()!=0) - s/=getMagnitude(); - return new XY(x * s, y * s); + if(norm&&getMagnitude().get()!=0) + s/=getMagnitude().get(); + return new XY(x.get() * s, y.get() * s); } @Override - public double dotProduct(Vector v) { - return (x * v.getX()) + (y * v.getY()); + public Distance dotProduct(Vector v) { + return new IN((x.get() * v.getX().get()) + (y.get() * v.getY().get())); } @Override diff --git a/src/org/montclairrobotics/sprocket/httpserver/Http.java b/src/org/montclairrobotics/sprocket/httpserver/Http.java deleted file mode 100644 index c34142c..0000000 --- a/src/org/montclairrobotics/sprocket/httpserver/Http.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.montclairrobotics.sprocket.httpserver; -/* - * TO USE (in eclipse): - * Right click on project name - * Click Build Path > Configure Build Path - * Select Libraries - * Click the Down Arrow next to JRE System Library - * Double Click "Access Rules" - * Click Add - * Change Resolution to Accessible - * Use this pattern: com/sun/** - * Click OK - * Click OK - * Click OK - */ -import java.io.IOException; -import java.io.OutputStream; -import java.net.InetSocketAddress; - -import com.sun.net.httpserver.HttpExchange; -import com.sun.net.httpserver.HttpHandler; -import com.sun.net.httpserver.HttpServer; -import com.sun.net.httpserver.Headers; - -public class Http implements Runnable{ - - private Thread myThread; - - private HttpServer server; - private boolean on=false; - private int port; - private String subDir; - - public Http(int port, String subDir) - { - this.port=port; - this.subDir=subDir; - myThread=new Thread(this); - myThread.start(); - } - - public void run() - { - startServer(); - } - public void startServer() - { - try { - server = HttpServer.create(new InetSocketAddress(port), 0); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - return; - } - server.createContext("/"+subDir, new MyHandler()); - server.setExecutor(null); // creates a default executor - server.start(); - on=true; - } - - public void stopServer() - { - if(server!=null && on) - { - server.stop(0); - on=false; - } - } - - class MyHandler implements HttpHandler { - @Override - public void handle(HttpExchange t) throws IOException { - request(t); - } - } - public void request(HttpExchange t) { - try{ - Headers h=t.getResponseHeaders(); - byte[]response=getResponse(h,t); - t.sendResponseHeaders(200, response.length); - OutputStream os = t.getResponseBody(); - os.write(response); - os.close(); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } - /** - * Processes an http request, - * filling out header and returning the response - * @param h the Headers, add() each header - * @param exchange the HttpExchange (don't have to use) - * @return the byte[] that you want to send - */ - public byte[] getResponse(Headers h,HttpExchange exchange){return null;} -} diff --git a/src/org/montclairrobotics/sprocket/httpserver/MJpg.java b/src/org/montclairrobotics/sprocket/httpserver/MJpg.java deleted file mode 100644 index 92191f3..0000000 --- a/src/org/montclairrobotics/sprocket/httpserver/MJpg.java +++ /dev/null @@ -1,124 +0,0 @@ -package org.montclairrobotics.sprocket.httpserver; - - - -/** - * Copyright (c) 2013 Joshua Dickson - * - * Permission is hereby granted, free of charge, to any person obtaining a copy of - * this software and associated documentation files (the "Software"), to deal in - * the Software without restriction, including without limitation the rights to - * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies - * of the Software, and to permit persons to whom the Software is furnished to do - * so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -import java.awt.image.BufferedImage; -import java.io.ByteArrayOutputStream; -import java.io.File; -import java.io.OutputStream; -import java.util.ArrayList; -import java.util.List; - -import javax.imageio.ImageIO; - -import com.sun.net.httpserver.HttpExchange; - -/** - * A basic implementation of a MJPG streamer as a Java servlet. MJPG is commonly used to deliver - * image information from networked cameras via an HTTP stream. The set up of this class allws - * for dynamic image information that is created in real time to be sent to any tool capable of - * reading MJPG including several major internet browsers (Safari, Chrome, and Firefox). - * - * We exclude four necessary images, which can be places in the user's home directory. We name - * our revolving images 'winter', 'spring', 'summer', and 'fall'. Images can be of any type, but - * this class is set up to read JPG images. Altering the image source type involves changing - * the ImageIO.write() function call that returns the image as a byte array. - * - * The byte array could also be loaded live and not generated from a static file. - * - * Feedback may be sent to josh dot dickson at wpi dot edu. - * - * @author Joshua Dickson - * @version December 10, 2013 - */ -public class MJpg extends Http { - - private final List imageByteList; - private int currentIndex; - - - public MJpg(int port,String fileName){ - super(port, fileName); - - // set the index - currentIndex = 0; - - // load images from the user's home directory into the list of image bytes - String[] names = {"summer", "fall", "winter", "spring"}; - imageByteList = new ArrayList(0); - - for(String name : names) { - try { - File image = new File(name + ".jpg"); - BufferedImage originalImage = ImageIO.read(image); - ByteArrayOutputStream baos = new ByteArrayOutputStream(); - ImageIO.write( originalImage, "jpg", baos ); - baos.flush(); - imageByteList.add(baos.toByteArray()); - baos.close(); - } catch (Exception ex) { - System.err.println("There was a problem loading the images."); - } - } - - } - public void request(HttpExchange t) { - try{ - byte[] data = getResponse(); - t.sendResponseHeaders(200,0); - OutputStream outputStream = t.getResponseBody(); - outputStream.write(( - "HTTP/1.0 200 OK\r\n" + - "Connection: close\r\n" + - "Max-Age: 0\r\n" + - "Expires: 0\r\n" + - "Access-Control-Allow-Origin: *" + - "Cache-Control: no-cache, private\r\n" + - "Pragma: no-cache\r\n" + - "Content-Type: multipart/x-mixed-replace; " + - "boundary=--BoundaryString\r\n\r\n").getBytes()); - while (true) { - data = getResponse(); - outputStream.write(( - "--BoundaryString\r\n" + - "Content-type: image/jpg\r\n" + - "Content-Length: " + - data.length + - "\r\n\r\n").getBytes()); - outputStream.write(data); - outputStream.write("\r\n\r\n".getBytes()); - outputStream.flush(); - } - } - catch(Exception e) - { - } - } - public byte[] getResponse() { - currentIndex=(currentIndex+1)%4; - return imageByteList.get(currentIndex); - } - -} \ No newline at end of file From 8a139b6c33a7402fbcef638fb8183ae54bc55f37 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Wed, 11 Jan 2017 15:54:47 -0500 Subject: [PATCH 4/6] more stuff --- .../sprocket/drive/DriveTrainBuilder.java | 74 -------------- .../sprocket/drive/Drivetrain.java | 12 ++- .../sprocket/httpserver/Http.java | 97 ------------------- .../sprocket/pipeline/Pipeline.java | 4 +- .../sprocket/utils/Input.java | 21 +--- .../montclairrobotics/sprocket/utils/PID.java | 2 +- 6 files changed, 14 insertions(+), 196 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java delete mode 100644 src/org/montclairrobotics/sprocket/httpserver/Http.java diff --git a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java b/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java deleted file mode 100644 index 37f2ebe..0000000 --- a/src/org/montclairrobotics/sprocket/drive/DriveTrainBuilder.java +++ /dev/null @@ -1,74 +0,0 @@ -package org.montclairrobotics.sprocket.drive; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.SpeedController; -import org.montclairrobotics.sprocket.control.ArcadeDriveInput; -import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.Inch; -import org.montclairrobotics.sprocket.geometry.Side; -import org.montclairrobotics.sprocket.geometry.XY; -import org.montclairrobotics.sprocket.utils.PID; - -import java.util.ArrayList; - -public class DriveTrainBuilder { - - private ArrayList modules; - private DriveTrainInput input; - - public DriveTrainBuilder() { - modules = new ArrayList<>(); - } - - - public DriveTrainBuilder addDriveModule(DriveModule module) { - modules.add(module); - return this; - } - - public DriveTrainBuilder addWheel(SpeedController motor, Side side, SEncoder enc, PID pid, Inch maxSpeed, boolean invert) { - DriveModule module = new DriveModule(motor, new Degrees(0), - side == Side.LEFT ? new XY(-1, 0) : new XY(1, 0), - enc, pid, maxSpeed); - if(invert) { - module.setInverted(true); - } - return this; - } - - public DriveTrainBuilder addWheel(SpeedController motor, Side side, SEncoder enc, PID pid, Inch maxSpeed) { - return addWheel(motor, side, enc, pid, maxSpeed, false); - } - - public DriveTrainBuilder addWheel(SpeedController motor, Side side) { - return addWheel(motor, side, null, null, null); - } - - public DriveTrainBuilder setInput(DriveTrainInput input) { - this.input = input; - return this; - } - - public DriveTrainBuilder setArcadeDriveInput(Joystick stick) { - return setInput(new ArcadeDriveInput(stick)); - } - - public DriveTrainBuilder setArcadeDriveInputSpeedControl(Joystick stick, Inch maxSpeed) { - return setInput(new ArcadeDriveInput(stick, maxSpeed)); - } - - public DriveTrain build() throws InvalidDriveTrainException { - if(modules.size() == 0) { - throw new InvalidDriveTrainException("DriveTrain requires at least one drive module to be defined."); - } - - - } - -} - -class InvalidDriveTrainException extends Exception { - public InvalidDriveTrainException(String message) { - super(message); - } -} \ No newline at end of file diff --git a/src/org/montclairrobotics/sprocket/drive/Drivetrain.java b/src/org/montclairrobotics/sprocket/drive/Drivetrain.java index 41b3ec1..d25ca3c 100644 --- a/src/org/montclairrobotics/sprocket/drive/Drivetrain.java +++ b/src/org/montclairrobotics/sprocket/drive/Drivetrain.java @@ -4,9 +4,10 @@ import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Updatable; import org.montclairrobotics.sprocket.pipeline.Pipeline; +import org.montclairrobotics.sprocket.utils.Input; public class Drivetrain implements Updatable{ - private Pipeline input; + private Input input; private DriveModule[] modules; public Drivetrain(DriveModule[] modules) @@ -15,7 +16,7 @@ public Drivetrain(DriveModule[] modules) this.modules=modules; } - public void setInput(Pipeline input) + public void setInput(Input input) { this.input=input; } @@ -25,13 +26,16 @@ public void update() { DriveTrainTarget target=input.get(); Vector tgtDirection=target.getDirection(); Angle tgtTurn=target.getTurn(); + Angle torque; for(DriveModule module:modules) + { - tgtTurn=tgtTurn.add(module.setDirection(tgtDirection)); + torque=torque.add(module.getTorque(tgtDirection)); } + tgtTurn.subtract(torque); for(DriveModule module:modules) { - module.setTurn(tgtTurn); + module.setDirectionTurn(tgtDirection,tgtTurn); } } } diff --git a/src/org/montclairrobotics/sprocket/httpserver/Http.java b/src/org/montclairrobotics/sprocket/httpserver/Http.java deleted file mode 100644 index 8107b86..0000000 --- a/src/org/montclairrobotics/sprocket/httpserver/Http.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.montclairrobotics.sprocket.httpserver; -/* - * TO USE (in eclipse): - * Right click on project name - * Click Build Path > Configure Build Path - * Select Libraries - * Click the Down Arrow next to JRE System Library - * Double Click "Access Rules" - * Click Add - * Change Resolution to Accessible - * Use this pattern: com/sun/** - * Click OK - * Click OK - * Click OK - */ -import java.io.IOException; -import java.io.OutputStream; -import java.net.InetSocketAddress; - -import com.sun.net.httpserver.Headers; -import com.sun.net.httpserver.HttpExchange; -import com.sun.net.httpserver.HttpHandler; -import com.sun.net.httpserver.HttpServer; - -public class Http implements Runnable{ - - private Thread myThread; - - private HttpServer server; - private boolean on=false; - private int port; - private String subDir; - - public Http(int port, String subDir) - { - this.port=port; - this.subDir=subDir; - myThread=new Thread(this); - myThread.start(); - } - - public void run() - { - startServer(); - } - public void startServer() - { - try { - server = HttpServer.create(new InetSocketAddress(port), 0); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - return; - } - server.createContext("/"+subDir, new MyHandler()); - server.setExecutor(null); // creates a default executor - server.start(); - on=true; - } - - public void stopServer() - { - if(server!=null && on) - { - server.stop(0); - on=false; - } - } - - class MyHandler implements HttpHandler { - @Override - public void handle(HttpExchange t) throws IOException { - request(t); - } - } - public void request(HttpExchange t) { - try{ - Headers h=t.getResponseHeaders(); - byte[]response=getResponse(h,t); - t.sendResponseHeaders(200, response.length); - OutputStream os = t.getResponseBody(); - os.write(response); - os.close(); - } catch (IOException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - } - /** - * Processes an http request, - * filling out header and returning the response - * @param h the Headers, add() each header - * @param exchange the HttpExchange (don't have to use) - * @return the byte[] that you want to send - */ - public byte[] getResponse(Headers h,HttpExchange exchange){return null;} -} diff --git a/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java b/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java index 13a59b9..09f642a 100644 --- a/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java +++ b/src/org/montclairrobotics/sprocket/pipeline/Pipeline.java @@ -2,7 +2,9 @@ import java.util.ArrayList; -public class Pipeline { +import org.montclairrobotics.sprocket.utils.Input; + +public class Pipeline implements Input{ private ArrayList> steps; private T init; diff --git a/src/org/montclairrobotics/sprocket/utils/Input.java b/src/org/montclairrobotics/sprocket/utils/Input.java index b58271b..90ee41b 100644 --- a/src/org/montclairrobotics/sprocket/utils/Input.java +++ b/src/org/montclairrobotics/sprocket/utils/Input.java @@ -1,23 +1,6 @@ package org.montclairrobotics.sprocket.utils; -public class Input { +public interface Input { - private double value; - - public Input(double value) { - this.value = value; - } - - public Input() { - this.value = 0.0; - } - - - public double get() { - return get(); - } - - public void set(double value) { - this.value = value; - } + public T get(); } diff --git a/src/org/montclairrobotics/sprocket/utils/PID.java b/src/org/montclairrobotics/sprocket/utils/PID.java index a6d880f..4c3e8b6 100644 --- a/src/org/montclairrobotics/sprocket/utils/PID.java +++ b/src/org/montclairrobotics/sprocket/utils/PID.java @@ -17,7 +17,7 @@ public class PID implements Updatable { - private Input input; + private Input input; private double P,I,D,minIn,maxIn,minOut,maxOut; private boolean calculated=false; From 41f0a515d932a5ff04ebb700697085539e07d573 Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Wed, 11 Jan 2017 16:35:48 -0500 Subject: [PATCH 5/6] Moar changes-done for now --- .../sprocket/control/ArcadeDriveInput.java | 19 +++-- .../sprocket/drive/DriveModule.java | 71 ++----------------- .../sprocket/drive/Drivetrain.java | 2 +- .../sprocket/drive/EncoderInput.java | 7 +- .../sprocket/drive/SEncoder.java | 10 +-- .../sprocket/drive/TheBestMapper.java | 48 ------------- .../sprocket/geometry/Vector.java | 3 +- 7 files changed, 28 insertions(+), 132 deletions(-) delete mode 100644 src/org/montclairrobotics/sprocket/drive/TheBestMapper.java diff --git a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java index a251282..046c60d 100644 --- a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java @@ -4,21 +4,26 @@ import org.montclairrobotics.sprocket.drive.MotorInputType; import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.Inch; +import org.montclairrobotics.sprocket.geometry.Distance; +import org.montclairrobotics.sprocket.geometry.IN; import org.montclairrobotics.sprocket.geometry.Polar; 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.pipeline.Step; +import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.Joystick; -public class ArcadeDriveInput implements Step { +public class ArcadeDriveInput implements Input,Updatable { private Joystick stick; MotorInputType inputType; //private boolean speedControl; - private Inch maxSpeed; + private Distance maxSpeed; private Angle maxTurn; private Vector dir; @@ -27,15 +32,17 @@ public class ArcadeDriveInput implements Step { public ArcadeDriveInput(Joystick stick) { inputType=MotorInputType.PERCENT; this.stick = stick; - this.maxSpeed=new Inch(1); + this.maxSpeed=new IN(1); this.maxTurn=Angle.QUARTER; + Updater.add(this, Priority.INPUT); } - public ArcadeDriveInput(Joystick stick, Inch maxSpeed, Angle maxTurn) { + public ArcadeDriveInput(Joystick stick, Distance maxSpeed, Angle maxTurn) { inputType=MotorInputType.SPEED; this.stick = stick; this.maxSpeed = maxSpeed; this.maxTurn=maxTurn; + Updater.add(this, Priority.INPUT); } @@ -53,7 +60,7 @@ public Angle getTurn() { } @Override - public DriveTrainTarget get(DriveTrainTarget in) { + public DriveTrainTarget get() { // TODO Auto-generated method stub return new DriveTrainTarget(getDirection(),getTurn(),inputType); } diff --git a/src/org/montclairrobotics/sprocket/drive/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index ba04027..4446aea 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -2,60 +2,7 @@ import edu.wpi.first.wpilibj.SpeedController; -import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Inch; -import org.montclairrobotics.sprocket.geometry.Polar; -import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.utils.PID; - -public class DriveModule extends Motor { - - private Angle forceAngle; - private Vector offset; - private Inch maxSpeed; - - private Vector forceVector; - - public DriveModule(SpeedController motor, - Angle forceAngle, Vector offset, - SEncoder enc, PID pid, Inch maxSpeed, - boolean invert) { - super(motor, enc, pid, invert); - this.forceAngle = forceAngle; - this.offset = offset; - this.maxSpeed = maxSpeed; - - this.forceVector=new Polar(maxSpeed.get(),forceAngle); - } - - public DriveModule(SpeedController motor, - Angle forceAngle, Vector offset, - SEncoder enc, PID pid, Inch maxSpeed) { - this(motor, forceAngle, offset, enc, pid, maxSpeed, false); - } - - public DriveModule(SpeedController motor, - Angle forceAngle, Vector offset) { - this(motor, forceAngle, offset, null, null, null, false); - } - public Angle getForceAngle() { - return forceAngle; - } - - public Inch getMaxSpeed() { - return maxSpeed; - } - - public Vector getOffset() { - return offset; - } - - public Vector getForceVector() - { - return forceVector; - } -======= import org.montclairrobotics.sprocket.geometry.Angle; import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; @@ -75,28 +22,20 @@ public DriveModule(Vector force,Vector offset,Motor... motors){ this.motors=motors; Updater.add(this, Priority.OUTPUT); } - - public Angle setDirection(Vector tgtDirection) - { - tgtForce=0; - return addForce(tgtDirection); - } - public void setTurn(Angle turn) + public Angle getTorque(Vector tgtDirection) { - Vector turnForce=offset.rotate(Angle.QUARTER).scale(turn.toRadians()*offset.getMagnitude(),true); - addForce(turnForce); + return null; + } - public Angle addForce(Vector f) + public void setDirectionTurn(Vector tgtDirection,Angle tgtTurn) { - tgtForce+=force.dotProduct(f)/force.getMagnitude(); - return new Radians(force.scale(tgtForce, true).rotate(offset.getAngle().negative()).getX()/offset.getMagnitude()); + } public void setOutputPipeline(Pipeline p) { this.outputPipeline=p; } - @Override public void update() { tgtForce=outputPipeline.get(tgtForce); diff --git a/src/org/montclairrobotics/sprocket/drive/Drivetrain.java b/src/org/montclairrobotics/sprocket/drive/Drivetrain.java index d25ca3c..63029d8 100644 --- a/src/org/montclairrobotics/sprocket/drive/Drivetrain.java +++ b/src/org/montclairrobotics/sprocket/drive/Drivetrain.java @@ -26,7 +26,7 @@ public void update() { DriveTrainTarget target=input.get(); Vector tgtDirection=target.getDirection(); Angle tgtTurn=target.getTurn(); - Angle torque; + Angle torque=Angle.ZERO; for(DriveModule module:modules) { diff --git a/src/org/montclairrobotics/sprocket/drive/EncoderInput.java b/src/org/montclairrobotics/sprocket/drive/EncoderInput.java index 6852fd0..391d371 100644 --- a/src/org/montclairrobotics/sprocket/drive/EncoderInput.java +++ b/src/org/montclairrobotics/sprocket/drive/EncoderInput.java @@ -2,7 +2,7 @@ import org.montclairrobotics.sprocket.utils.Input; -public class EncoderInput extends Input { +public class EncoderInput implements Input { private SEncoder enc; @@ -11,10 +11,7 @@ public EncoderInput(SEncoder e) { } @Override - public double get() { + public Double get() { return enc.getSpeed().get(); } - - @Override - public void set(double value) {} } diff --git a/src/org/montclairrobotics/sprocket/drive/SEncoder.java b/src/org/montclairrobotics/sprocket/drive/SEncoder.java index 582b07b..b74d659 100644 --- a/src/org/montclairrobotics/sprocket/drive/SEncoder.java +++ b/src/org/montclairrobotics/sprocket/drive/SEncoder.java @@ -1,6 +1,6 @@ package org.montclairrobotics.sprocket.drive; -import org.montclairrobotics.sprocket.geometry.Inch; +import org.montclairrobotics.sprocket.geometry.Distance; import edu.wpi.first.wpilibj.Encoder; @@ -35,12 +35,12 @@ public double getTickRate() { return enc.getRate(); } - public Inch getInches() { - return new Inch(getTicks()/ticksPerInch); + public Distance getInches() { + return new Distance(getTicks()/ticksPerInch); } - public Inch getSpeed() { - return new Inch(getTickRate()/ticksPerInch); + public Distance getSpeed() { + return new Distance(getTickRate()/ticksPerInch); } public void reset() { diff --git a/src/org/montclairrobotics/sprocket/drive/TheBestMapper.java b/src/org/montclairrobotics/sprocket/drive/TheBestMapper.java deleted file mode 100644 index 9a908bc..0000000 --- a/src/org/montclairrobotics/sprocket/drive/TheBestMapper.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.montclairrobotics.sprocket.drive; - -import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Degrees; -import org.montclairrobotics.sprocket.geometry.Radians; -import org.montclairrobotics.sprocket.geometry.Vector; - -public class TheBestMapper implements DriveTrainMapper{ - - @Override - public void map(DriveTrainTarget driveTarget, DriveModule[] driveModules) { - - - Vector tgtDirection=driveTarget.getDirection(); - Angle tgtTurn=driveTarget.getTurn(); - - Vector force,offset; - //This loop is to compensate for the torque generated - for(DriveModule module:driveModules) - { - force=module.getForceVector(); - offset=module.getOffset(); - tgtTurn=tgtTurn.subtract( - new Radians( - force - .scale( - force.dotProduct(tgtDirection)/force.getMagnitude(), true) - .rotate(offset.getAngle().opposite()) - .getX()/offset.getMagnitude())); - } - //This actually sets the power - for(DriveModule module:driveModules) - { - force=module.getForceVector(); - offset=module.getOffset(); - module.set( - force.dotProduct( - tgtDirection.add( - offset - .rotate(new Degrees(90)) - .scale(tgtTurn.toRadians()*offset.getMagnitude(),true) - ) - )/force.getMagnitude()); - - } - } - -} diff --git a/src/org/montclairrobotics/sprocket/geometry/Vector.java b/src/org/montclairrobotics/sprocket/geometry/Vector.java index cc4fda0..25dba12 100644 --- a/src/org/montclairrobotics/sprocket/geometry/Vector.java +++ b/src/org/montclairrobotics/sprocket/geometry/Vector.java @@ -2,7 +2,8 @@ public interface Vector { - Distance getMagnitude(); + Vector ZERO = new XY(0,0); + Distance getMagnitude(); Angle getAngle(); Distance getX(); Distance getY(); From 4d92bf3d0edae55bfd0d8c2bd2045f16c195d8fe Mon Sep 17 00:00:00 2001 From: Rafi Baum Date: Wed, 11 Jan 2017 16:36:47 -0500 Subject: [PATCH 6/6] Organized imports --- .../montclairrobotics/sprocket/control/ArcadeDriveInput.java | 3 --- src/org/montclairrobotics/sprocket/drive/DriveModule.java | 4 ---- src/org/montclairrobotics/sprocket/drive/Motor.java | 1 + 3 files changed, 1 insertion(+), 7 deletions(-) diff --git a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java index 046c60d..d4d8d30 100644 --- a/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java +++ b/src/org/montclairrobotics/sprocket/control/ArcadeDriveInput.java @@ -3,16 +3,13 @@ import org.montclairrobotics.sprocket.drive.DriveTrainTarget; import org.montclairrobotics.sprocket.drive.MotorInputType; import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Distance; import org.montclairrobotics.sprocket.geometry.IN; -import org.montclairrobotics.sprocket.geometry.Polar; 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.pipeline.Step; import org.montclairrobotics.sprocket.utils.Input; import edu.wpi.first.wpilibj.Joystick; diff --git a/src/org/montclairrobotics/sprocket/drive/DriveModule.java b/src/org/montclairrobotics/sprocket/drive/DriveModule.java index 4446aea..5f0e74d 100644 --- a/src/org/montclairrobotics/sprocket/drive/DriveModule.java +++ b/src/org/montclairrobotics/sprocket/drive/DriveModule.java @@ -1,10 +1,6 @@ package org.montclairrobotics.sprocket.drive; -import edu.wpi.first.wpilibj.SpeedController; - - import org.montclairrobotics.sprocket.geometry.Angle; -import org.montclairrobotics.sprocket.geometry.Radians; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.loop.Priority; import org.montclairrobotics.sprocket.loop.Updatable; diff --git a/src/org/montclairrobotics/sprocket/drive/Motor.java b/src/org/montclairrobotics/sprocket/drive/Motor.java index f96ff12..1bc8fc3 100644 --- a/src/org/montclairrobotics/sprocket/drive/Motor.java +++ b/src/org/montclairrobotics/sprocket/drive/Motor.java @@ -1,6 +1,7 @@ package org.montclairrobotics.sprocket.drive; import org.montclairrobotics.sprocket.utils.PID; + import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Talon;