TorqueLib/Debug.java at master · TexasTorque/TorqueLib · 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
/**
 * Copyright 2023 Texas Torque.
 *
 * This file is part of TorqueLib, which is licensed under the MIT license.
 * For more details, see ./license.txt or write <jus@justusl.com>.
 */
packageorg.texastorque.torquelib;

importjava.util.HashMap;
importjava.util.Map;

importorg.texastorque.Subsystems;
importorg.texastorque.torquelib.util.TorqueMath;
importorg.texastorque.torquelib.util.TorqueUtil;

importedu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
importedu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
importedu.wpi.first.wpilibj.smartdashboard.Field2d;

publicfinalclassDebugimplementsSubsystems {

publicstaticrecordPoint(intx, inty) {};

publicstaticclassGrid {
privateintwidth, currentPt = 0;

publicstaticGrideslastic() {
returnnewGrid(12);
 }

publicGrid(intwidth) {
this.width = width;

 }

/**
 * This doesnt have to me most time efficient algorithm
 * b/c we only compute this once per entry.
 *
 * TODO: make this an actual algorithm.
 */
publicPointfindSpot(finalintw) {
finalvarpt = newPoint(currentPt % width, currentPt / width);
currentPt += w;
returnpt;
 }
 }

privatestaticfinalMap<Integer, Map<String, Double>> numbers = newHashMap<>();
privatestaticfinalMap<Integer, Map<String, String>> strings = newHashMap<>();
privatestaticfinalMap<Integer, Map<String, Boolean>> bools = newHashMap<>();
privatestaticfinalMap<Integer, Grid> grids = newHashMap<>();

publicstaticvoidinitDashboard() {
Shuffleboard.update();
 }

publicstaticvoidlog(finalStringkey, finaldoublenumber) {
log(getTab(), key, number, 2, 1);
 }
publicstaticvoidlog(finalStringkey, finaldoublenumber, finalintwidth) {
log(getTab(), key, number, width, 1);
 }

privatestaticvoidlog(finalShuffleboardTabtab, finalStringkey, finaldoublenumber, finalintwidth, finalintheight) {
finalinttabCode = tab.hashCode();
if (!grids.containsKey(tabCode)) {
grids.put(tabCode, Grid.eslastic());
 }

if (!numbers.containsKey(tabCode)) {
numbers.put(tabCode, newHashMap<String, Double>());
 }

if (!numbers.get(tabCode).containsKey(key)) {
finalvarpt = grids.get(tabCode).findSpot(width);
tab.addNumber(key, () -> numbers.get(tabCode).get(key)).withSize(width, height).withPosition(pt.x, pt.y);
 }
numbers.get(tabCode).put(key, TorqueMath.round(number, 4));
 }

publicstaticvoidlog(finalStringkey, finalStringstring) {
log(getTab(), key, string, 2, 1);
 }
publicstaticvoidlog(finalStringkey, finalStringstring, finalintwidth) {
log(getTab(), key, string, width, 1);
 }

privatestaticvoidlog(finalShuffleboardTabtab, finalStringkey, finalStringstring, finalintwidth, finalintheight) {
finalinttabCode = tab.hashCode();
if (!grids.containsKey(tabCode)) {
grids.put(tabCode, Grid.eslastic());
 }

if (!strings.containsKey(tabCode)) {
strings.put(tabCode, newHashMap<String, String>());
 }

if (!strings.get(tabCode).containsKey(key)) {
finalvarpt = grids.get(tabCode).findSpot(width);
tab.addString(key, () -> strings.get(tabCode).get(key)).withSize(width, height).withPosition(pt.x, pt.y);
 }
strings.get(tabCode).put(key, string);
 }

publicstaticvoidlog(finalStringkey, finalbooleanbool) {
log(getTab(), key, bool, 2, 1);
 }
publicstaticvoidlog(finalStringkey, finalbooleanbool, finalintwidth) {
log(getTab(), key, bool, width, 1);
 }
privatestaticvoidlog(finalShuffleboardTabtab, finalStringkey, finalbooleanbool, finalintwidth, finalintheight) {
finalinttabCode = tab.hashCode();
if (!grids.containsKey(tabCode)) {
grids.put(tabCode, Grid.eslastic());
 }

if (!bools.containsKey(tabCode)) {
bools.put(tabCode, newHashMap<String, Boolean>());
 }

if (!bools.get(tabCode).containsKey(key)) {
finalvarpt = grids.get(tabCode).findSpot(width);
tab.addBoolean(key, () -> bools.get(tabCode).get(key)).withSize(width, height).withPosition(pt.x, pt.y);
 }
bools.get(tabCode).put(key, bool);
 }

publicstaticvoidfield(finalStringkey, finalField2dfield) {
getTab().add(key, field);
 }

privatestaticShuffleboardTabgetTab() {
finalStringclassName = TorqueUtil.getStackTraceElement(4).getClassName();
StringtabName = className.substring(className.lastIndexOf('.') + 1);
if (tabName.contains("$")) {
tabName = tabName.substring(0, tabName.lastIndexOf('$'));
 }
returnShuffleboard.getTab(tabName);
 }
}