FTCIntoTheDeep/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PestoFTCConfig.java at master · SHP-Robotics/FTCIntoTheDeep · 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
package org.firstinspires.ftc.teamcode;

import static com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.FORWARD;
import static com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.REVERSE;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.shprobotics.pestocore.algorithms.PID;
import com.shprobotics.pestocore.drivebases.DeterministicTracker;
import com.shprobotics.pestocore.drivebases.MecanumController;
import com.shprobotics.pestocore.drivebases.TeleOpController;
import com.shprobotics.pestocore.drivebases.ThreeWheelOdometryTracker;

@Config
public class PestoFTCConfig {
    public static double ODOMETRY_TICKS_PER_INCH = 505.3169;
    public static double FORWARD_OFFSET = 0;
    public static double ODOMETRY_WIDTH = 14.35782;
    public static double DECELERATION = 2.0;
    public static double MAX_VELOCITY = 46;
//    public static double headingP = 0.65;
    public static double headingP = 3.0;
//    public static double headingI = 0.5;
    public static double headingI = 0;
    public static double headingD = 0;
//    public static double maxHeadingI = 0.3;
    public static double maxHeadingI = 0;
    public static PID headingPID = new PID(headingP, headingI, headingD);

    public static double endpointP = 0.03;
    public static double endpointI = 0;
    public static double endpointD = 0;
    public static double maxEndpointI = 0;

    public static PID endpointPID = new PID(endpointP, endpointI, endpointD);
    public static final DcMotorSimple.Direction leftEncoderDirection = FORWARD;
    public static final DcMotorSimple.Direction centerEncoderDirection = REVERSE;
    public static final DcMotorSimple.Direction rightEncoderDirection = FORWARD;

    public static String leftName = "leftFront";
    public static String centerName = "rightFront";
    public static String rightName = "rightRear";

    public static MecanumController getMecanumController(HardwareMap hardwareMap) {
        MecanumController mecanumController = new MecanumController(hardwareMap, new String[] {
                "leftFront",
                "rightFront",
                "leftRear",
                "rightRear"
        });

        mecanumController.configureMotorDirections(new DcMotorSimple.Direction[]{
                REVERSE,
                FORWARD,
                REVERSE,
                FORWARD
        });

//        double magnitude = new Vector2D(70, 60).getMagnitude();
//        mecanumController.setPowerVectors(new Vector2D[]{
//                Vector2D.scale(new Vector2D(70, 60), 1/magnitude),
//                Vector2D.scale(new Vector2D(-70, 60), 1/magnitude),
//                Vector2D.scale(new Vector2D(-70, 60), 1/magnitude),
//                Vector2D.scale(new Vector2D(70, 60), 1/magnitude)
//        });
//
//            mecanumController.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
//            mecanumController.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        return mecanumController;
    }

    public static TeleOpController getTeleOpController(MecanumController mecanumController, DeterministicTracker tracker, HardwareMap hardwareMap) {
        TeleOpController teleOpController = new TeleOpController(mecanumController, hardwareMap);
        teleOpController.useTrackerIMU(tracker);

        teleOpController.configureIMU(
                RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
                RevHubOrientationOnRobot.UsbFacingDirection.UP
        );

        teleOpController.setSpeedController((gamepad) -> {
            if (gamepad.right_bumper) {
                return 0.6;
            }
            return 1.0;
        });

        teleOpController.counteractCentripetalForce(tracker, MAX_VELOCITY); //TODO PUT IN NOTEBOOK

        return teleOpController;
    }

    public static DeterministicTracker getTracker(HardwareMap hardwareMap) {
        return new ThreeWheelOdometryTracker.TrackerBuilder(
                hardwareMap,
                ODOMETRY_TICKS_PER_INCH,
                FORWARD_OFFSET,
                ODOMETRY_WIDTH,
                leftName,
                centerName,
                rightName,
                leftEncoderDirection,
                centerEncoderDirection,
                rightEncoderDirection
        ).build();
    }

    public static void configure(){
        headingPID.setMaxIntegralProportionRatio(maxHeadingI);
        endpointPID.setMaxIntegralProportionRatio(maxEndpointI);


    }
}