ChainLynx2020/MotionProfiling.java at master · ChainLynxRobotics/ChainLynx2020 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
// package frc.robot;

// import java.io.File;
// import java.io.IOException;

// import jaci.pathfinder.*;
// import jaci.pathfinder.followers.EncoderFollower;


// import com.analog.adis16448.frc.ADIS16448_IMU;

// import com.ctre.phoenix.motorcontrol.can.BaseMotorController;

// import com.ctre.phoenix.motorcontrol.can.VictorSPX;

// public class MotionProfiling {
//     private DriveTrain driveTrain;
//     private final double wheelBaseWidth = 2.25; // Width in feet
//     private final double wheelDiameter = 0.1524; // meters
//     private final VictorSPX leftMotor;
//     private final VictorSPX rightMotor;
//     private final int encoderTicksPerRevolution = 4096;
//     private final double maxVelocity = 13; // ft/s
//     private EncoderFollower left;
//     private EncoderFollower right;

//     public MotionProfiling(DriveTrain driveTrain, String setupLeft, String setupRight) throws IOException {
//         this.driveTrain = driveTrain;
//         leftMotor = driveTrain.getLeftMotor();
//         rightMotor = driveTrain.getRightMotor();
//         // pathweaver has an error with mixing up left and right
//         Trajectory trajectoryLeft = PathfinderFRC.getTrajectory(setupRight);
//         Trajectory trajectoryRight = PathfinderFRC.getTrajectory(setupLeft);

//         left = new EncoderFollower(trajectoryLeft);
//         right = new EncoderFollower(trajectoryRight);

//         left.configureEncoder(((BaseMotorController) leftMotor).getSelectedSensorPosition(), encoderTicksPerRevolution,
//                 wheelDiameter);
//         right.configureEncoder(rightMotor.getSelectedSensorPosition(), encoderTicksPerRevolution, wheelDiameter);

//         left.configurePIDVA(0.9, 0.0, 0.0, 1 / maxVelocity, 0); //Filler PID vals
//         right.configurePIDVA(0.9, 0.0, 0.0, 1 / maxVelocity, 0);
//     }
//     public void update() {
//         double l = left.calculate(leftMotor.getSelectedSensorPosition());
//         double r = right.calculate(rightMotor.getSelectedSensorPosition());
//         double gyroHeading = driveTrain.getGyro().getGyroAngleY(); // Assuming the gyro is giving a
//                                                                      // value in degrees
//         double desiredHeading = -Pathfinder.r2d(left.getHeading());  // Should also be in degrees

//         double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading);
//         double turn = 0.8 * (-1.0/80.0) * angleDifference;

//         driveTrain.autoUpdateSpeed(l + turn, r - turn);
//     }

//     public boolean isFinished() {
//         if (left.isFinished() && right.isFinished()) {
//             return true;
//         } else {
//             return false;
//         }
//     }

//     public void reset(){
//         left.reset();
//         right.reset();
//         left.configureEncoder(leftMotor.getSelectedSensorPosition(), encoderTicksPerRevolution, wheelDiameter);
//         right.configureEncoder(rightMotor.getSelectedSensorPosition(), encoderTicksPerRevolution, wheelDiameter);
//     }
// }