Reverted back

FRC6854 · Jan 16, 2020 · 688c31a · 688c31a
1 parent 1d49c17
commit 688c31a
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 95 deletions.
diff --git a/src/main/java/viking/MotionProfileBuffer.java b/src/main/java/viking/MotionProfileBuffer.java
diff --git a/src/main/java/viking/controllers/ctre/VikingSRX.java b/src/main/java/viking/controllers/ctre/VikingSRX.java
@@ -1,15 +1,18 @@
 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;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 
 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;
     }
-}
+}