subsystem-library/TagInfo.java at master · perSEVERE-5962/subsystem-library · 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
141
142
143
144
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import frc.robot.subsystems.DetectAprilTags;

/** April tag information for the speaker */
public class TagInfo {
    public static NetworkTable aprilTagTable = NetworkTableInstance.getDefault().getTable("apriltags");

    private NetworkTable tagTable;

    public class TagPosition {
        private NetworkTable tagPosTable;
        private NetworkTableEntry entryX;
        private NetworkTableEntry entryY;
        private NetworkTableEntry entryZ;

        private TagPosition(NetworkTable table) {
            this.tagPosTable = table.getSubTable("pos");
            this.entryX = tagPosTable.getEntry("x");
            this.entryY = tagPosTable.getEntry("y");
            this.entryZ = tagPosTable.getEntry("z");
        }

        public NetworkTable getPosTable() {
            return this.tagPosTable;
        };

        /** Left/right */
        public double getX() {
            return this.entryX.getDouble(0);
        }
        /** Height */
        public double getY() {
            return this.entryY.getDouble(0);
        }
        /** Distance */
        public double getZ() {
            return this.entryZ.getDouble(0);
        }

        public Vec3 getAll() {
            return new Vec3(this.entryX.getDouble(0), this.entryY.getDouble(0), this.entryZ.getDouble(0));
        }
    }
    private TagPosition tagPosEntries;

    public class TagRotation {
        private NetworkTable tagRotTable;
        private NetworkTableEntry entryX;
        private NetworkTableEntry entryY;
        private NetworkTableEntry entryZ;

        private NetworkTableEntry dyaw;

        private TagRotation(NetworkTable table) {
            this.tagRotTable = table.getSubTable("rot");
            this.entryX = tagRotTable.getEntry("x");
            this.entryY = tagRotTable.getEntry("y");
            this.entryZ = tagRotTable.getEntry("z");
            this.dyaw = tagRotTable.getEntry("dyaw");
        }

        public NetworkTable getRotTable() {
            return this.tagRotTable;
        };

        /** Pitch */
        public double getX() {
            return this.entryX.getDouble(0);
        }
        /** Yaw */
        public double getY() {
            return this.entryY.getDouble(0);
        }
        /** Roll */
        public double getZ() {
            return this.entryZ.getDouble(0);
        }

        /** Positive = left; Negative = right */
        public double getDyaw() {
            return this.dyaw.getDouble(0);
        }

        public Vec3 getAll() {
            return new Vec3(this.entryX.getDouble(0), this.entryY.getDouble(0), this.entryZ.getDouble(0));
        }
    }
    private TagRotation tagRotEntries;

    private int tagId = -1;
    private GenericEntry tagEntry;

    public TagInfo(int tagId) {
        this.tagTable = NetworkTableInstance.getDefault().getTable("apriltags").getSubTable("tag" + tagId);
        this.tagPosEntries = new TagPosition(this.tagTable);
        this.tagRotEntries = new TagRotation(this.tagTable);
        this.tagId = tagId;
        this.tagEntry = Shuffleboard.getTab("April Tag Info").add("Tag " + tagId, 0).getEntry();
    }

    public int getId() {
        return this.tagId;
    }

    public TagPosition getPos() {
        return tagPosEntries;
    }

    public TagRotation getRot() {
        return tagRotEntries;
    }

    public GenericEntry getEntry() {
        return tagEntry;
    }

    public void update() {
        int index = this.tagId;
        if (index != -1) {
            Vec3 tagPos = DetectAprilTags.getAprilTagPos(index);
            Vec3 tagRot = DetectAprilTags.getAprilTagRot(index);
            if (tagPos != null && tagRot != null) {
                double angleToTag = Math.toDegrees(Math.atan2(tagPos.getX(), tagPos.getZ()));
                tagPosEntries.entryX.setDouble(tagPos.getX());
                tagPosEntries.entryY.setDouble(tagPos.getY());
                tagPosEntries.entryZ.setDouble(tagPos.getZ());
                tagRotEntries.entryX.setDouble(tagRot.getX());
                tagRotEntries.entryY.setDouble(tagRot.getY());
                tagRotEntries.entryZ.setDouble(tagRot.getZ());
                tagRotEntries.dyaw.setDouble(angleToTag);
            }
        }
    }
}