Skip to content

Commit 83ed27c

Browse files
prankX02prankX02
committed
Added XeroFramework
1 parent 136406c commit 83ed27c

20 files changed

Lines changed: 2222 additions & 0 deletions

‎src/main/java/org/xerosw/SampleAverager.java‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
package org.xerosw;
2+
3+
public class SampleAverager {
4+
private double [] last_pos_ ;
5+
private int count_ ;
6+
private boolean complete_ ;
7+
8+
public SampleAverager(int count) {
9+
count_ = 0 ;
10+
complete_ = false ;
11+
last_pos_ = new double[count] ;
12+
}
13+
14+
public void addSample(double s) {
15+
last_pos_[count_++] = s ;
16+
if (count_ == last_pos_.length) {
17+
count_ = 0 ;
18+
complete_ = true ;
19+
}
20+
}
21+
22+
public void reset() {
23+
count_ = 0 ;
24+
complete_ = false ;
25+
}
26+
27+
public boolean isComplete() {
28+
return complete_ ;
29+
}
30+
31+
public double maxDeviationFromAverage() {
32+
double sum = 0.0 ;
33+
for (int i = 0 ; i < last_pos_.length ; i++) {
34+
sum += last_pos_[i] ;
35+
}
36+
double avg = sum / last_pos_.length ;
37+
38+
double max = 0.0 ;
39+
for (int i = 0 ; i < last_pos_.length ; i++) {
40+
double diff = Math.abs(last_pos_[i] - avg) ;
41+
if (diff > max)
42+
max = diff ;
43+
}
44+
45+
return max ;
46+
}
47+
}

‎src/main/java/org/xerosw/hid/XeroGamepad.java‎

Lines changed: 207 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
package org.xerosw.hid;
2+
3+
import org.littletonrobotics.junction.Logger;
4+
5+
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
6+
import edu.wpi.first.wpilibj2.command.Command;
7+
import edu.wpi.first.wpilibj2.command.Commands;
8+
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
9+
import edu.wpi.first.wpilibj2.command.button.Trigger;
10+
11+
public class XeroGamepad {
12+
13+
private final CommandXboxController gamepad_;
14+
private final int port_;
15+
16+
private boolean locked_;
17+
18+
public XeroGamepad(int port) {
19+
port_ = port;
20+
gamepad_ = new CommandXboxController(port);
21+
locked_ = false;
22+
23+
Logger.recordOutput("Gamepad/" + port_ + "/Locked", locked_);
24+
}
25+
26+
/**
27+
* Sets if the inputs from this gamepad is locked from drivers.
28+
* @param enabled
29+
*/
30+
public void setLocked(boolean locked) {
31+
locked_ = locked;
32+
Logger.recordOutput("Gamepad/" + port_ + "/Locked", locked_);
33+
}
34+
35+
/**
36+
* A command for {@link #setLocked(boolean)}
37+
* @param locked
38+
* @return A command to set the lock state of this gamepad.
39+
*/
40+
public Command setLockCommand(boolean locked) {
41+
return Commands.runOnce(() -> setLocked(locked));
42+
}
43+
44+
/**
45+
* Whether or not this gamepad is locked from driver input.
46+
* @return If its locked
47+
*/
48+
public boolean isLocked() {
49+
return locked_;
50+
}
51+
52+
/**
53+
* Whether or not this gamepad is unlocked from driver input.
54+
* @return If its unlocked
55+
*/
56+
public boolean isUnlocked() {
57+
return !locked_;
58+
}
59+
60+
/*
61+
* Buttons
62+
*/
63+
64+
public Trigger a() {
65+
return ifUnlocked(gamepad_.a());
66+
}
67+
68+
public Trigger b() {
69+
return ifUnlocked(gamepad_.b());
70+
}
71+
72+
public Trigger x() {
73+
return ifUnlocked(gamepad_.x());
74+
}
75+
76+
public Trigger y() {
77+
return ifUnlocked(gamepad_.y());
78+
}
79+
80+
public Trigger leftBumper() {
81+
return ifUnlocked(gamepad_.leftBumper());
82+
}
83+
84+
public Trigger rightBumper() {
85+
return ifUnlocked(gamepad_.rightBumper());
86+
}
87+
88+
public Trigger back() {
89+
return ifUnlocked(gamepad_.back());
90+
}
91+
92+
public Trigger start() {
93+
return ifUnlocked(gamepad_.start());
94+
}
95+
96+
public Trigger leftStick() {
97+
return ifUnlocked(gamepad_.leftStick());
98+
}
99+
100+
public Trigger rightStick() {
101+
return ifUnlocked(gamepad_.rightStick());
102+
}
103+
104+
public Trigger leftTrigger(double threshold) {
105+
return ifUnlocked(gamepad_.leftTrigger(threshold));
106+
}
107+
108+
public Trigger leftTrigger() {
109+
return ifUnlocked(gamepad_.leftTrigger());
110+
}
111+
112+
public Trigger rightTrigger(double threshold) {
113+
return ifUnlocked(gamepad_.rightTrigger(threshold));
114+
}
115+
116+
public Trigger rightTrigger() {
117+
return ifUnlocked(gamepad_.rightTrigger());
118+
}
119+
120+
public Trigger pov(int angle) {
121+
return ifUnlocked(gamepad_.pov(angle));
122+
}
123+
124+
public Trigger povUp() {
125+
return ifUnlocked(gamepad_.povUp());
126+
}
127+
128+
public Trigger povUpRight() {
129+
return ifUnlocked(gamepad_.povUpRight());
130+
}
131+
132+
public Trigger povRight() {
133+
return ifUnlocked(gamepad_.povRight());
134+
}
135+
136+
public Trigger povDownRight() {
137+
return ifUnlocked(gamepad_.povDownRight());
138+
}
139+
140+
public Trigger povDown() {
141+
return ifUnlocked(gamepad_.povDown());
142+
}
143+
144+
public Trigger povDownLeft() {
145+
return ifUnlocked(gamepad_.povDownLeft());
146+
}
147+
148+
public Trigger povLeft() {
149+
return ifUnlocked(gamepad_.povLeft());
150+
}
151+
152+
public Trigger povUpLeft() {
153+
return ifUnlocked(gamepad_.povUpLeft());
154+
}
155+
156+
public Trigger povCenter() {
157+
return ifUnlocked(gamepad_.povCenter());
158+
}
159+
160+
/*
161+
* Axes
162+
*/
163+
164+
public double getLeftX() {
165+
return zeroIfLocked(gamepad_.getLeftX());
166+
}
167+
168+
public double getRightX() {
169+
return zeroIfLocked(gamepad_.getRightX());
170+
}
171+
172+
public double getLeftY() {
173+
return zeroIfLocked(gamepad_.getLeftY());
174+
}
175+
176+
public double getRightY() {
177+
return zeroIfLocked(gamepad_.getRightY());
178+
}
179+
180+
public double getLeftTriggerAxis() {
181+
return zeroIfLocked(gamepad_.getLeftTriggerAxis());
182+
}
183+
184+
public double getRightTriggerAxis() {
185+
return zeroIfLocked(gamepad_.getRightTriggerAxis());
186+
}
187+
188+
public void setRumble(RumbleType type, double value) {
189+
gamepad_.setRumble(type, value);
190+
}
191+
192+
193+
public boolean isConnected() {
194+
return gamepad_.isConnected();
195+
}
196+
197+
// Utils
198+
199+
private Trigger ifUnlocked(Trigger trigger) {
200+
return trigger.and(this::isUnlocked);
201+
}
202+
203+
private double zeroIfLocked(double axis) {
204+
return isUnlocked() ? axis : 0.0;
205+
}
206+
207+
}

‎src/main/java/org/xerosw/math/PieceWiseLinear.java‎

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
package org.xerosw.math;
2+
3+
/// \file
4+
5+
import java.util.ArrayList;
6+
import java.util.Collections;
7+
import java.util.Comparator;
8+
import java.util.List;
9+
10+
import edu.wpi.first.math.geometry.Translation2d;
11+
12+
/// \brief this class represents an XY function that consists of a series of line segments
13+
public class PieceWiseLinear {
14+
private List<Translation2d> points_;
15+
16+
/// \brief This constructor creates the PieceWiseLinear object from a set of points given.
17+
/// The points are sorted on the X value so they do not have to be in order.
18+
/// \param points the set of points provided
19+
public PieceWiseLinear(double[] values) throws Exception {
20+
List<Translation2d> points = new ArrayList<Translation2d>() ;
21+
22+
int i = 0 ;
23+
while (i < values.length) {
24+
Translation2d pt = new Translation2d(values[i], values[i + 1]) ;
25+
points.add(pt) ;
26+
i += 2 ;
27+
}
28+
init(points) ;
29+
}
30+
31+
/// \brief This method returns the number of points that make up the piece wise linear function
32+
/// \returns the number of points that make up the piece wise linear function
33+
public int size() {
34+
return points_.size();
35+
}
36+
37+
/// \brief This method returns a point given its index.
38+
/// Note, this function returns the point after sorting, so the values returned will not
39+
/// match the values provided if the original values were not sorted on the X value.
40+
/// \returns a point given its index
41+
public Translation2d get(int i) {
42+
return points_.get(i);
43+
}
44+
45+
/// \brief This method returns a Y value given an X value.
46+
/// If the X value supplied is before the first X value of the piece wise linear function, the
47+
/// Y value of the first point is returned. If the X value supplied is after the X value of the
48+
/// last X value of the piece wise linear function, the Y value of the last point is returned. If
49+
/// the X value supplied is within the points of the piece wise linear function, the segment containing
50+
/// the X value is found an a Y value is retured based on linear interpolation between the start and end
51+
/// of the containing segment.
52+
/// \returns the Y value given the X value.
53+
public double getValue(double x) {
54+
double ret;
55+
56+
if (x < points_.get(0).getX()) {
57+
ret = points_.get(0).getY();
58+
} else if (x > points_.get(points_.size() - 1).getX()) {
59+
ret = points_.get(points_.size() - 1).getY();
60+
} else {
61+
int which_x = findWhich(x);
62+
63+
Translation2d low = points_.get(which_x);
64+
Translation2d high = points_.get(which_x + 1);
65+
66+
double m = ((high.getY() - low.getY()) / (high.getX() - low.getX()));
67+
double b = high.getY() - (high.getX() * m);
68+
69+
ret = (m * x) + b;
70+
71+
}
72+
73+
return ret;
74+
}
75+
76+
private void init(List<Translation2d> points) throws Exception {
77+
points_ = new ArrayList<Translation2d>();
78+
79+
if (points.size() == 0)
80+
throw new Exception("cannot have empty points list as input");
81+
82+
for (int i = 0; i < points.size(); i++)
83+
points_.add(points.get(i));
84+
85+
Collections.sort(points_, new Comparator<Translation2d>() {
86+
public int compare(Translation2d p1, Translation2d p2) {
87+
if (p1.getX() < p2.getX())
88+
return -1;
89+
90+
if (p1.getX() > p2.getX())
91+
return 1;
92+
93+
if (p1.getY() < p2.getY())
94+
return -1;
95+
96+
if (p1.getY() > p2.getY())
97+
return 1;
98+
99+
return 0;
100+
}
101+
});
102+
}
103+
104+
private int findWhich(double x) {
105+
for (int i = 0; i < points_.size(); i++) {
106+
if (x >= points_.get(i).getX() && x < points_.get(i + 1).getX()) {
107+
return i;
108+
}
109+
}
110+
111+
return -1;
112+
}
113+
}

0 commit comments

Comments
 (0)