IntoTheDeep/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java at master · IGNITE-Pathways/IntoTheDeep · 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
//package org.firstinspires.ftc.teamcode;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.DualNum;
//import com.acmerobotics.roadrunner.Time;
//import com.acmerobotics.roadrunner.Twist2dDual;
//import com.acmerobotics.roadrunner.Vector2d;
//import com.acmerobotics.roadrunner.Vector2dDual;
//import com.acmerobotics.roadrunner.ftc.Encoder;
//import com.acmerobotics.roadrunner.ftc.FlightRecorder;
//import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
//import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
//import com.acmerobotics.roadrunner.ftc.RawEncoder;
//import com.qualcomm.robotcore.hardware.DcMotorEx;
//import com.qualcomm.robotcore.hardware.DcMotorSimple;
//import com.qualcomm.robotcore.hardware.HardwareMap;
//
//import org.firstinspires.ftc.teamcode.messages.ThreeDeadWheelInputsMessage;
//
//@Config
//public final class ThreeDeadWheelLocalizer implements Localizer {
//    public static class Params {
//        public double par0YTicks = 0.0; // y position of the first parallel encoder (in tick units)
//        public double par1YTicks = 1.0; // y position of the second parallel encoder (in tick units)
//        public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
//    }
//
//    public static Params PARAMS = new Params();
//
//    public final Encoder par0, par1, perp;
//
//    public final double inPerTick;
//
//    private double lastPar0Pos, lastPar1Pos, lastPerpPos;
//    private boolean initialized;
//
//    public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
//        // TODO: make sure your config has **motors** with these names (or change them)
//        //   the encoders should be plugged into the slot matching the named motor
//        //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
//        par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0")));
//        par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1")));
//        perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
//
//        // TODO: reverse encoder directions if needed
//        //   par0.setDirection(DcMotorSimple.Direction.REVERSE);
//
//        this.inPerTick = inPerTick;
//
//        FlightRecorder.write("THREE_DEAD_WHEEL_PARAMS", PARAMS);
//    }
//
//    public Twist2dDual<Time> update() {
//        PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
//        PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
//        PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
//
//        FlightRecorder.write("THREE_DEAD_WHEEL_INPUTS", new ThreeDeadWheelInputsMessage(par0PosVel, par1PosVel, perpPosVel));
//
//        if (!initialized) {
//            initialized = true;
//
//            lastPar0Pos = par0PosVel.position;
//            lastPar1Pos = par1PosVel.position;
//            lastPerpPos = perpPosVel.position;
//
//            return new Twist2dDual<>(
//                    Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
//                    DualNum.constant(0.0, 2)
//            );
//        }
//
//        double par0PosDelta = par0PosVel.position - lastPar0Pos;
//        double par1PosDelta = par1PosVel.position - lastPar1Pos;
//        double perpPosDelta = perpPosVel.position - lastPerpPos;
//
//        Twist2dDual<Time> twist = new Twist2dDual<>(
//                new Vector2dDual<>(
//                        new DualNum<Time>(new double[] {
//                                (PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
//                                (PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
//                        }).times(inPerTick),
//                        new DualNum<Time>(new double[] {
//                                (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
//                                (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
//                        }).times(inPerTick)
//                ),
//                new DualNum<>(new double[] {
//                        (par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
//                        (par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
//                })
//        );
//
//        lastPar0Pos = par0PosVel.position;
//        lastPar1Pos = par1PosVel.position;
//        lastPerpPos = perpPosVel.position;
//
//        return twist;
//    }
//}