Code review of main

build.gradle

@@ -2,7 +2,7 @@ plugins {
     id "java"
     id 'java-library'
     id 'maven-publish'
-    id "edu.wpi.first.GradleRIO" version "2023.4.1"
+    id "edu.wpi.first.GradleRIO" version "2023.4.2"
     id 'com.google.protobuf' version '0.8.19'
 }
 

docs/FollowPoints.md

@@ -0,0 +1,53 @@
+# The FollowPoints() Procedure
+
+## What is FollowPoints?
+
+FollowPoints is a procdure in which the robot follows a path of points. It does not find the robot's position, but utilizes another position-finding system (such as odometry).
+
+## How does FollowPoints work?
+
+FollowPoints utilizes the "Pure Pursuit" method. This method involves a circle drawn around the robot.
+
+![Image](images/followPoints1.png)
+
+At the start of the method, the robot moves towards the first point. In th above image, the robot would move towards Point 1.
+
+![Image](images/followPoints2.png)
+
+Once the circle around the robot intersects the line between Points 1 and 2, the robot starts to move toward the point where this line intersects the circle. In the image above, this is the purple point (the orange point is also a place where the circle and line intersect, but the robot will move towards the intersection point closest to Point 2). The robot will continue moving towards this point, moving closer to the line, until it reaches the line between Points 2 and 3, when it will start to follow that line. This procedure ensures the robot travels along a smooth curve.
+
+## FollowPoints Customization Options
+
+### Critical Points
+
+Though it allows for a smooth curve, Pure Pursuit means that the robot won't actually reach any of the points, and will instead curve around them. It also means that if there are three consecutive and near-colinear points, the method will bypass the second point, as the robot will immediately intersect the line between the second  and third points.
+
+To remedy this, the `FollowPoints()` procedure allows for points to be specified as "critical points." The robot must reach a critical point before moving to the next point. The last point in a `FollowPoints()` procedure is always treated as a critical point, no matter if it is actually specified as one.
+
+### Headers
+
+All points are inputted as `PointDir` objects, which contain a header as an argument. The robot will rotate so it is facing the header of the point it is moving towards.
+
+### Procedures
+
+Optionally, an array of procedures may be provided as an input. If given, once the robot reaches a given point (or starts moving towards the next point in the case the point is not a critical point), it will run that procedure. The boolean `stopRobot` determines whether the robot will stop and wait for the procedure to finish before continuing.
+
+## Desmos Integration
+
+For the 2023 season, FollowPoints has been integrated with [the Desmos model of the field](https://www.desmos.com/calculator/v6mpw9cwbn).
+
+![Image](images/desmosFieldModel.png)
+
+To use this model, drag the points across the field. To make a point a critical point, click the red/gray dots at the bottom of the field (the first dot corresponds to the first point, the second to the second point, etc.). To change the heading at each point, click on the purple dot attached to each point to rotate it by 15 degrees. The model currently does not support procedures.
+
+To add a new point, add its x- and y-coordinate to the table. Add a 0 or 1 to the c_p list corresponding to if the point is critical, and add an angle measure in degrees to the a list for the heading.
+
+To export the path, open the console (Ctrl+Shift+J), scroll to the bottom of the Desmos equations list and copy-and-paste the code into the console. Copy-and-paste the resulting json format into the deploy folder (C:\Users\admin\Documents\GitHub\2023\src\main\deploy).
+
+## Running FollowPoints()
+
+There are several constructors accepted by FollowPoints():
+
+* An array of PointDirs will provide a path, but will not allow for the specification of procedures or critical points.
+* A string with the name of a json file will run the path generated by a Desmos model.
+* A default constructor will run a path with provided "Steps," objects containing a PointDir, a "critical point" boolean, a procedure, and a "stop robot at procedure" boolean.

docs/Odometry.md

@@ -0,0 +1,45 @@
+# Odometry
+
+## What is Odometry?
+
+Odometry is a method to calculate the position of the robot based on how far each of the wheels has traveled.
+
+## How does Odometry work?
+
+On a swerve drive-bot, each wheel can both spin and rotate. We use this to estimate where each robot has traveled. Each wheel keeps track of how far it has traveled, as well as its heading, and we use this to calculate the position of each wheel.
+
+![Image](images/odometry_wheel.png)
+
+Given a wheel's starting position, distance traveled, and angle change, an arc is used to estimate the new x- and y-positions of that wheel. Once positions for all four wheels are calculated, they are averaged to find the new position of the robot.
+
+## Benefits of Odometry
+
+* **It does not require any external hardware.** Odometry is entirely based on the wheels, and does not require any cameras or lights, and so will rarely break. This leaves it as a robust backup in case another position-finding system fails.
+* **It works fast.** Odometry can easily run at 100 or even 1000 times per second, meaning it can be used in the times between updates from another position-finding system.
+
+## Limitations of Odometry
+
+* **It is highly dependent on how accurately the circumference of the wheel can be measured.** Even a small error can lead to odometry thinking the robot is meters away from where it actually is.
+* **Error compounds over time.** Unlike other forms of position-finding, odometry error only gets worse the longer the robot is driven. This puts odometry as best used in-combination with another form of position-finding.
+* **Wheel slippage leads to huge amounts of error.** Odometry assumes there is no wheel slippage, and that if a wheel spins once, it has traveled the length of its circumference. If the robot runs into an object but the wheels keep spinning, the position will be thrown off.
+
+## How to use Odometry
+
+Odometry should be run in the Drive.java mechanism, which allows it to work even when Tele-Op is turned off. 
+
+### Initialization
+
+To initialize Odometry, use the constructor
+`swerveOdometry(MotorController[] motors, CANCoder[] CANCoders, Point[] wheelLocations, double wheelCircumference, double gearRatio, int encoderToRevolutionConstant, double rateLimiterTime)`.
+
+* motorList, CANCoderList, and wheelLocations should contain arrays with an element corresponding to each wheel in the same order.
+* gearRatio and encoderToRevolutionConstant are dependent on the swerve module.
+* rateLimiterTime is how often odometry should run. The faster it runs, the more accurate it will be, but the more memory it takes up.
+
+### Running Odometry
+
+To use odometry, set a PointDir object equal to the `odometry.run()` method within `Robot.drive.run()`.
+
+### Integrating Odometry with other Position-Finding Methods
+
+Use the `setCurrentPosition(Point P)` method to set the odometry current position to a position found from another source.

docs/Point and PointDir.md

@@ -0,0 +1,16 @@
+# Point and PointDir
+
+## What are Point and PointDir?
+
+Point and PointDir are two classes which make it easy to represent a position with or without a heading. PointDir inherits from Point.
+
+## How to use Point and PointDir
+
+Points have an x-coordinate and a y-coordinate, which can be accessed via `getX()` and `getY()`. PointDir additionally has a heading, which can be accessed via `getHeading()`.
+
+Point has several useful methods:
+
+* `distance(Point a)` returns the distance between the current point and `a`.
+* `slope(Point a)` returns the slope of the line between the current point and `a`. It has a maximum and minimum of ±1000.
+* `add(Point a)` returns a point with an whose coordinates are the sums of the corresponding coordinates of the current point and `a`.
+* `scaleVector(Point inputPoint, double scale)` returns a point whose coordinates are the coordinates of a vector between the current point and `inputPoint`, scaled to have a length of `scale`.

docs/SwerveDrive.md

@@ -0,0 +1,57 @@
+# Swerve Drive
+
+## What is swerve drive?
+
+Swerve drive is a type of drive train that allows for independent control of each wheel. This allows for the robot to move in any direction, and rotate in place. It also allows for the robot to move sideways or rotate while moving linearly, which is useful for some games.
+
+## How does swerve drive work?
+
+Swerve drive works by having each wheel connected to a motor that can rotate 360 degrees. Each wheel has a steering and a driving motor. The steering motor rotates the wheel, and the driving motor rotates the wheel and the robot.
+
+Here is an example of a swerve drive module (mk4i):
+
+![Image](images/swervedrive.png)
+
+As you can see, there are 2 motors which control the orientation and speed of the wheel. There is also an absolute encoder that allows the robot to know the orientation of the wheel even after it is fully turned off.
+
+When 4 of these modules are connected to a robot, it can move in any direction and rotate in place by using vector addition.
+
+Strafing is the process of moving in 2 dimentions by orienting all of the wheels at the same angle and moving them at the same speed.
+
+Rotating is the process of rotating in place by orienting all of the wheels at 90 degrees angles from each other and then turning them at the same speed.
+
+When strafing and rotating are done at the same time, we get a vector sum of the two vectors for each wheel (speed is a magnitude while orientation is well, orientation). Since rotatin and strafing are going to be different for each wheels, the robot is able to move while rotating.
+
+One caveat is that the added vectors could be greater than 1, there, we need to scale all of the vectors down.
+
+## Benefits of swerve drive
+
+* **Agility.** Because swerve can move in any directions and rotate, it is very easy to avoid defense from other robots. It is also easier to move around the field and score points for most objectives.
+* **Modular.** Because swerve drive modules are modular, the robot is easily repairable It is also possible to drive the robot with only 3 modules working (if one module is broken during a match for example).
+
+## Limitations of sweve drive
+
+* **Very complex.** Because of the complex nature of swerve drive, the code is to read or understand. Moreover, the chance of something going wrong is high.
+* **Draws a lot of power.** Swerve drive uses 8 motors at once, which draw a lot of power, it is **nessessary** to prepare the amount of amps drawn by each motor on the robot.
+
+## How to use swerve drive
+
+Call the `swerveDrive` procedure in the `Drive` mechanism. This procedure takes 3 parameters:
+
+* `x` - The x component of the vector to move in.
+* `y` - The y component of the vector to move in.
+* `rotation` - The amount to rotate the robot.
+
+You can also inpute a pointDir, but this should only be used for `FollowPoints`.
+### Initialization
+
+Calling `Drive()` will initialize the mechanism. This will initialize the motors and the encoders.
+
+### Running swerveDrive
+
+To use the mecanism, call the `swerveDrive` method. You can simply use joysticks as inputs. 
+No procedures are needed for swerve, all is containted in the `Drive.java` mechanism.
+
+### Integrating Mechanism with other code
+
+Thanks to the option to add a pointDir in the method `swerveDrive`, it is possible to use swerve drive with `FollowPoints` (or another procedure).

docs/candle.md

@@ -0,0 +1,9 @@
+# A Simple Guide on Using CANdle to Display Single Colors
+
+Use `Robot.candle.setColor(int r, int g, int b);` to display single colors on the CANdle.
+
+**Yellow:** R: 255 G: 255 B: 0
+
+**Purple:** R: 255 G: 0 B: 255
+
+**Off** R: 0 G: 0 B: 0

src/main/deploy/FollowPoints.json

@@ -0,0 +1,12 @@
+{
+	"points": [
+		{
+			"coordinates": [1, 0, 0],
+			"critical": false,
+			"procedure": {
+				"name" : "DoNothing()",
+				"stop" : false
+			}
+		}
+	]
+}

src/main/java/com/team766/hal/GenericRobotMain.java

@@ -122,13 +122,13 @@ public void teleopInit() {
 			m_autonMode = null;
 		}
 
-		if (m_oiContext == null) {
+		if (m_oiContext == null && m_oi != null) {
 			m_oiContext = Scheduler.getInstance().startAsync(m_oi);
 		}
 	}
 
 	public void teleopPeriodic() {
-		if (m_oiContext.isDone()) {
+		if (m_oiContext != null && m_oiContext.isDone()) {
 			m_oiContext = Scheduler.getInstance().startAsync(m_oi);
 			Logger.get(Category.OPERATOR_INTERFACE).logRaw(Severity.WARNING, "Restarting OI context");
 		}

src/main/java/com/team766/hal/RobotProvider.java

@@ -24,7 +24,7 @@ public abstract class RobotProvider {
 	public static RobotProvider instance;
 
 	protected EncoderReader[] encoders = new EncoderReader[20];
-	protected SolenoidController[] solenoids = new SolenoidController[10];
+	protected SolenoidController[] solenoids = new SolenoidController[20];
 	protected GyroReader[] gyros = new GyroReader[13];
 	protected HashMap<String, CameraReader> cams = new HashMap<String, CameraReader>();
 	protected JoystickReader[] joysticks = new JoystickReader[8];
@@ -212,7 +212,7 @@ public DoubleSolenoid getSolenoid(String configName) {
 					.toArray(SolenoidController[]::new));
 			return new DoubleSolenoid(forwardSolenoids, reverseSolenoids);
 		} catch (IllegalArgumentException ex) {
-			Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, "Solenoid %s not found in config file, using mock solenoid instead", configName);
+			Logger.get(Category.CONFIGURATION).logData(Severity.ERROR, "Solenoid %s not found in config file, using mock solenoid instead %s", configName, ex.toString());
 			return new DoubleSolenoid(null, null);
 		}
 	}

src/main/java/com/team766/hal/wpilib/Solenoid.java

@@ -6,6 +6,6 @@
 
 public class Solenoid extends edu.wpi.first.wpilibj.Solenoid implements SolenoidController {
 	public Solenoid(int channel) {
-		super(PneumaticsModuleType.CTREPCM, channel);
+		super(PneumaticsModuleType.REVPH, channel);
 	}
 }

src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java

@@ -33,6 +33,7 @@
 import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.PneumaticHub;
 import edu.wpi.first.wpilibj.PneumaticsControlModule;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.SPI;
@@ -94,6 +95,12 @@ public void run() {
 	public WPIRobotProvider() {
 		m_dataRefreshThread = new Thread(m_DataRefreshRunnable, "DataRefreshThread");
 		m_dataRefreshThread.start();
+
+		try {
+			ph.enableCompressorAnalog(80, 100);
+		} catch (Exception ex) {
+			LoggerExceptionUtils.logException(ex);
+		}
 	}
 
 	private MotorController[][] motors =
@@ -102,6 +109,7 @@ public WPIRobotProvider() {
 	// The presence of this object allows the compressor to run before we've declared any solenoids.
 	@SuppressWarnings("unused")
 	private PneumaticsControlModule pcm = new PneumaticsControlModule();
+	private PneumaticHub ph = new PneumaticHub();
 
 	@Override
 	public MotorController getMotor(int index, String configPrefix, MotorController.Type type,
@@ -128,7 +136,7 @@ public MotorController getMotor(int index, String configPrefix, MotorController.
 				motor = new CANVictorMotorController(index);
 				break;
 			case TalonFX:
-				final ValueProvider<String> CANBus = ConfigFileReader.getInstance().getString(type + ".CANBus");
+				final ValueProvider<String> CANBus = ConfigFileReader.getInstance().getString(configPrefix + ".CANBus");
 				motor = new CANTalonFxMotorController(index, CANBus.get());
 				break;
 			case VictorSP: