ChainLynx2020/DriveTrain.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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
package frc.robot;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.LambdaJoystick.ThrottlePosition;
// import com.analog.adis16448.frc.ADIS16448_IMU;

public class DriveTrain {
    private final VictorSPX leftMotor1;
    private final VictorSPX rightMotor1;
    private final VictorSPX leftMotor2;
    private final VictorSPX rightMotor2;
    // public ADIS16448_IMU gyro;


    public double throttleInput;
    public double VelocityCheck;
    public double speedbrake;

    private boolean throttleMode = true;
    private boolean stopDriveMotors;

    public boolean Brakes;
    public boolean braketoggler=true;
    public boolean drivingOffSpeed;
    public boolean masteralarm = false;
    public boolean revrSpeedWarn = false;
    public boolean rushing = false;
    public boolean RvsThrottleWarn;
    public boolean throttleForward = true;
    public boolean velocityNeverToExcede = false;
    public boolean velocityToTurn;
    public boolean auto=false;


    public int throttleDirectionConstant = 1;


    public DriveTrain(final int leftPort1, final int leftPort2, final int rightPort1, final int rightPort2,
            final int gyroPortNumber) {
        leftMotor1 = new VictorSPX(leftPort1);
        leftMotor2 = new VictorSPX(leftPort2);
        rightMotor1 = new VictorSPX(rightPort1);
        rightMotor2 = new VictorSPX(rightPort2);


    }

    public void updateSpeed(final ThrottlePosition throttlePosition) {
        double scaledX = throttlePosition.x;
        double scaledY = throttlePosition.y;
        double scaledZ = throttlePosition.z;
        final double scaleFactorA = 0.3;
        final double scaleFactorB = 0.7;
        // Top is X scale bottem is Y
        final double scaleFactorC = 0.3;
        final double scaleFactorD = 0.7;
        scaledY = (scaleFactorC * Math.abs(throttlePosition.y))
                + (scaleFactorD * throttlePosition.y * throttlePosition.y);
        scaledX = (scaleFactorA * Math.abs(throttlePosition.x))
                + (scaleFactorB * throttlePosition.x * throttlePosition.x);
        if (throttlePosition.x < 0)
            scaledX = -scaledX;
        if (throttlePosition.y < 0)
            scaledY = -scaledY;

        double throttle1 = scaledZ * -1.00;
        // double throttle1 = 1.00;
        double throttle2 = throttleMode ? ((throttle1 + 1.00) / 2.00) : 0.30; // Throttle as a value between 1
                                                                                        // and 2
        double throttle3 = throttle2 * 100.00;
        double thrust1 = (java.lang.Math.abs((throttlePosition.y * 1.00) * throttle3)); // Thrust as a value between 1
                                                                                        // and 100

        /*
         * in theory creates a value double trust which gives a value between 0 and 1
         * for the y input and should Give proportion thrust out put when throtle is
         * enabled)
         */

        velocityNeverToExcede = (thrust1 >= 85.00) ? true : false;
        velocityToTurn = (thrust1 > 20.00) ? true : false;

        SmartDashboard.putNumber("status/throttlePrime", throttle3);
        SmartDashboard.putNumber("status/thrust", thrust1);
        SmartDashboard.putNumber("raw data/Xraw", throttlePosition.x);
        SmartDashboard.putNumber("raw data/Yraw", throttlePosition.y);
        SmartDashboard.putNumber("raw data/Zraw", throttlePosition.z);


        scaledX = (scaledX * 0.5 * (stopDriveMotors==false ? (throttle2) : 0.00));
        scaledY = scaledY * throttleDirectionConstant * (auto ==false ? (throttle2) : 0.10);


        final double right = ((-scaledX - scaledY) * -1);// +throttlePosition.z; //why plus throttle z?//dunno, just leave it for now
        final double left = (scaledY - scaledX) * -1;


        leftMotor1.set(ControlMode.PercentOutput, left);
        rightMotor1.set(ControlMode.PercentOutput, right);
        leftMotor2.follow(leftMotor2);
        rightMotor2.follow(rightMotor1);
    }

    public void autoUpdateSpeed(double left, double right) {
        leftMotor1.set(ControlMode.PercentOutput, left);
        rightMotor1.set(ControlMode.PercentOutput, right);
        leftMotor2.follow(leftMotor1);
        rightMotor2.follow(rightMotor1);
    }

    public void stopAuto() {
        auto=false;
    }

    public void startAuto() {
        auto = true;
    }

    public void togglethrottleMode() {
        throttleMode = !throttleMode;
        SmartDashboard.putBoolean("status/LowSpeed", throttleMode);
    }

    public void setThrottleDirectionConstant() {
        throttleDirectionConstant *= -1;
        throttleForward = !throttleForward;
        SmartDashboard.putBoolean("status/foward", throttleForward);

    }

	// public ADIS16448_IMU getGyro() {
	// 	return gyro;
	// }


}