diff --git a/src/main/java/viking/MotionProfileBuffer.java b/src/main/java/viking/MotionProfileBuffer.java deleted file mode 100644 index fd4e7ca..0000000 --- a/src/main/java/viking/MotionProfileBuffer.java +++ /dev/null @@ -1,83 +0,0 @@ -package viking; - -import com.ctre.phoenix.motion.BufferedTrajectoryPointStream; -import com.ctre.phoenix.motion.TrajectoryPoint; - -public class MotionProfileBuffer { - - private double metersPerRevolution = 0; - - private Double[][] leftPoints = null; - private Double[][] rightPoints = null; - - private BufferedTrajectoryPointStream leftBuffer = new BufferedTrajectoryPointStream(); - private BufferedTrajectoryPointStream rightBuffer = new BufferedTrajectoryPointStream(); - - public MotionProfileBuffer(String folder, double metersPerRevolution) { - leftPoints = CSVFileManager.pathLeft(folder); - rightPoints = CSVFileManager.pathRight(folder); - - System.out.println("Done reading from CSV"); - - writeLeftBuffer(leftPoints, leftPoints.length); - writeRightBuffer(rightPoints, rightPoints.length); - - System.out.println("Done writing buffers"); - } - - public BufferedTrajectoryPointStream getLeftBuffer() { - return leftBuffer; - } - - public BufferedTrajectoryPointStream getRightBuffer() { - return rightBuffer; - } - - private void writeLeftBuffer(Double[][] profile, int totalCnt) { - TrajectoryPoint point = new TrajectoryPoint(); - - for (int i = 0; i < totalCnt; ++i) { - - double positionRot = profile[i][0] * (1 / metersPerRevolution); - double velocityRPM = profile[i][1] * (1 / metersPerRevolution); - int durationMilliseconds = profile[i][2].intValue(); - - point.timeDur = durationMilliseconds; - point.position = positionRot * 4096; - point.velocity = velocityRPM * 4096 / 600.0; - point.profileSlotSelect0 = 0; - point.profileSlotSelect1 = 0; - point.zeroPos = (i == 0); - point.isLastPoint = ((i + 1) == totalCnt); - point.arbFeedFwd = 0; - - leftBuffer.Write(point); - } - - System.out.println("Done Writing Left Buffer"); - } - - private void writeRightBuffer(Double[][] profile, int totalCnt) { - TrajectoryPoint point = new TrajectoryPoint(); - - for (int i = 0; i < totalCnt; ++i) { - - double positionRot = profile[i][0] * (1 / metersPerRevolution); - double velocityRPM = profile[i][1] * (1 / metersPerRevolution); - int durationMilliseconds = profile[i][2].intValue(); - - point.timeDur = durationMilliseconds; - point.position = positionRot * 4096; - point.velocity = velocityRPM * 4096 / 600.0; - point.profileSlotSelect0 = 0; - point.profileSlotSelect1 = 0; - point.zeroPos = (i == 0); - point.isLastPoint = ((i + 1) == totalCnt); - point.arbFeedFwd = 0; - - rightBuffer.Write(point); - } - - System.out.println("Done Writing Right Buffer"); - } -} diff --git a/src/main/java/viking/controllers/ctre/VikingSRX.java b/src/main/java/viking/controllers/ctre/VikingSRX.java index d3b412b..3c4acab 100644 --- a/src/main/java/viking/controllers/ctre/VikingSRX.java +++ b/src/main/java/viking/controllers/ctre/VikingSRX.java @@ -1,6 +1,7 @@ package viking.controllers.ctre; import com.ctre.phoenix.motion.BufferedTrajectoryPointStream; +import com.ctre.phoenix.motion.TrajectoryPoint; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -8,8 +9,10 @@ public class VikingSRX { - private BufferedTrajectoryPointStream pointStream = new BufferedTrajectoryPointStream(); private TalonSRX motor; + private BufferedTrajectoryPointStream bufferedStream = new BufferedTrajectoryPointStream(); + + private double metersPerRevolution = 0; /** * Constructor for VikingSRX without encoder @@ -40,10 +43,13 @@ public VikingSRX(int id, boolean inverted) { */ public VikingSRX(int id, boolean inverted, boolean sensorPhase, FeedbackDevice device, double kF, double kP, double kI, - double kD, double velocity, double acceleration) { + double kD, double velocity, double acceleration, + double metersPerRevolution) { this.motor = new TalonSRX(id); + this.metersPerRevolution = metersPerRevolution; + motor.configFactoryDefault(); // Invert Power @@ -79,7 +85,35 @@ public VikingSRX(int id, boolean inverted, boolean sensorPhase, motor.setSelectedSensorPosition(0); } + public void initMotionBuffer(Double[][] profile, int totalCnt) { + TrajectoryPoint point = new TrajectoryPoint(); // temp for for loop, since unused params are initialized + // automatically, you can alloc just one + + /* Insert every point into buffer, no limit on size */ + for (int i = 0; i < totalCnt; ++i) { + + double positionRot = profile[i][0] * (1 / metersPerRevolution); + double velocityRPM = profile[i][1] * (1 / metersPerRevolution); + int durationMilliseconds = profile[i][2].intValue(); + + /* for each point, fill our structure and pass it to API */ + point.timeDur = durationMilliseconds; + point.position = positionRot * 4096; // Convert Revolutions to + // Units + point.velocity = velocityRPM * 4096 / 600.0; // Convert RPM to + // Units/100ms + point.profileSlotSelect0 = 0; /* which set of gains would you like to use [0,3]? */ + point.profileSlotSelect1 = 0; /* auxiliary PID [0,1], leave zero */ + point.zeroPos = (i == 0); /* set this to true on the first point */ + point.isLastPoint = ((i + 1) == totalCnt); /* set this to true on the last point */ + point.arbFeedFwd = 0; /* you can add a constant offset to add to PID[0] output here */ + + bufferedStream.Write(point); + } + } + public void resetMotionProfile() { + bufferedStream.Clear(); motor.clearMotionProfileTrajectories(); } @@ -99,22 +133,14 @@ public void motionMagic(double ticks) { motor.set(ControlMode.MotionMagic, ticks); } - public void setBufferProfileStream(BufferedTrajectoryPointStream stream) { - pointStream = stream; - } - public void motionProfileStart() { - motor.startMotionProfile(pointStream, 10, ControlMode.MotionProfile); + motor.startMotionProfile(bufferedStream, 10, ControlMode.MotionProfile); } public void setNeutralMode(NeutralMode mode) { motor.setNeutralMode(mode); } - public void zeroSensor() { - motor.setSelectedSensorPosition(0); - } - public int getTicks() { return motor.getSelectedSensorPosition(); } @@ -131,7 +157,11 @@ public boolean isMotionProfileFinished() { return motor.isMotionProfileFinished(); } + public void zeroSensor() { + motor.setSelectedSensorPosition(0); + } + public TalonSRX getTalonSRX() { return motor; } -} +} \ No newline at end of file