Skip to content

Commit

Permalink
nearly finalized internals code (plus jetson and minus double shot lo…
Browse files Browse the repository at this point in the history 
…gic)
  • Loading branch information
@eggaskin @ky28059
eggaskin authored and ky28059 committed Feb 25, 2022
1 parent d1936db commit 53216dc
Show file tree
Hide file tree
Showing 5 changed files with 202 additions and 167 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ public static final class ClimbConstants {
public static final class JetsonConstants {
public static final int turretCameraPort = 1181;
public static final int intakeCameraPort = 1182;
public static final int jetsonPort = 1337;
public static final int jetsonPort = 5800;
public static final String jetsonIP = "10.1.92.94";
}

Expand Down
11 changes: 5 additions & 6 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ public class RobotContainer {
private final InternalSubsystem internalSubsystem;
// private final ClimbSubsystem climbSubsystem;

private final JetsonConnection jetson = null;
private JetsonConnection jetson = null;
private final PowerController powerController = null;

// Controllers and buttons
Expand All @@ -66,11 +66,7 @@ public class RobotContainer {
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {

// Instantiate the Jetson connection
// jetson = new JetsonConnection();
// jetson.run();


// Instantiate subsystems
tankSubsystem = new TankSubsystem();
turretSubsystem = new TurretSubsystem(tankSubsystem, jetson);
Expand Down Expand Up @@ -197,6 +193,9 @@ private void controllerBindings() {
turretSubsystem.setHoodPower(hoodPower);
}, turretSubsystem));
}
// Instantiate the Jetson connection
jetson = new JetsonConnection();

}

/**
Expand Down
280 changes: 157 additions & 123 deletions src/main/java/frc/robot/jetson/JetsonConnection.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,166 +7,200 @@
import java.net.UnknownHostException;
import java.util.Arrays;

import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.HttpCamera;

import static frc.robot.Constants.JetsonConstants.*;

public class JetsonConnection implements Runnable {
public class JetsonConnection {

private final JetsonConnectionRunnable runnable;

// Instance variables
private Thread thread;
private Socket socket;
private BufferedReader stdIn;

// Bool for connection status
public boolean isConnected = false;

// Camera data
private boolean turretVisionStatus = false; // Can the camera provide turret vision data?
private double turretTheta = 0; // Delta theta of turret to target
private double hubDistance = 0; // Distance to hub
private boolean ballDetected = false; // If intake camera has detected a ball

public JetsonConnection() {
this.thread = new Thread(this);
this.thread.start();

// Start camera streams
HttpCamera turretCamera = createCamera("Turret", turretCameraPort);
CameraServer.startAutomaticCapture(turretCamera);

HttpCamera intakeCamera = createCamera("Intake", intakeCameraPort);
CameraServer.startAutomaticCapture(intakeCamera);
}

@Override
public void run() {
while (true) {
try {
// If thread interrupted
if (Thread.interrupted()) return;

// Check if we need to connect
if (stdIn == null || socket == null || socket.isClosed() || !socket.isConnected() || !socket.isBound()) {
isConnected = connect();

if (!isConnected) {
System.out.println("Unable to connect to Jetson");
// Wait before trying to connect again
Thread.sleep(500);
}
} else {
readCameraData();
}
} catch (InterruptedException e) {
// If interrupted, exit
return;
} catch (Exception e) {
System.out.println("Outer exception caught in CAMERA code. Unknown error. Will keep trying to connect to socket");
}
}
}

/**
* Attempt to connect to the jetson socket.
* @return If the connection was successful.
*/
public boolean connect() {
try {
socket = new Socket(jetsonIP, jetsonPort);
stdIn = new BufferedReader(new InputStreamReader(socket.getInputStream()));
System.out.println("Connected to Jetson address=" + jetsonIP + " at port=" + jetsonPort);

return true;
} catch (UnknownHostException e) {
socket = null;
stdIn = null;
} catch (IOException e) {
socket = null;
stdIn = null;
} catch (Exception e) {
socket = null;
stdIn = null;
System.out.println("UNKNOWN ERROR: something went wrong in camera connection!");
}
return false;
}

/**
* Read the camera data and update stored values.
* data: (bool -- turret vision status, double -- turret theta, double -- hub distance, boolean -- ball detected)
*/
public void readCameraData() throws InterruptedException {
try {
String in = stdIn.readLine();
if (in != null) {
String[] data = in.replace("(", "").replace(")", "").split(",");

System.out.println(Arrays.toString(data));

turretVisionStatus = strToBool(data[0]);
turretTheta = Double.parseDouble(data[1]);
hubDistance = Double.parseDouble(data[2]);
ballDetected = strToBool(data[3]);
}
} catch (IOException e) {
e.printStackTrace();
} catch (NullPointerException e) {
System.out.println("Unable to parse camera data: NullPointerException");
} catch (NumberFormatException e) {
System.out.println("Unable to parse camera data: NumberFormatException");
}
}

/**
* Converts string to a boolean. Either "False" or "True".
* @param str The string to convert.
* @return The boolean value.
*/
private boolean strToBool(String str) {
return str.equalsIgnoreCase("true");
runnable = new JetsonConnectionRunnable();
thread = new Thread(runnable);
thread.setDaemon(true);
thread.start();
}


/**
* Returns whether the turret camera can see vision targets, in case there are momentary lapses in vision or the
* hub is currently in the turntable blind spot.
* @return Whether turret vision is working.
*/
public boolean turretVisionWorking() {
return turretVisionStatus;
return runnable.turretVisionStatus;
}

/**
* Gets the calculated turret angle.
* @return The desired turntable angle.
*/
public double getTurretTheta() {
return turretTheta;
return runnable.turretTheta;
}

/**
* Gets the calculated hub distance.
* @return The distance from the camera to the hub.
*/
public double getHubDistance() {
return hubDistance;
return runnable.hubDistance;
}

/**
* Gets whether a ball is in range of the intake camera.
* @return Whether a ball is in intake range.
*/
public boolean ballDetected() {
return ballDetected;
}
return runnable.ballDetected;
}

class JetsonConnectionRunnable implements Runnable {

private Socket socket;
private BufferedReader stdIn;

// Bool for connection status
public boolean isConnected = false;

// Camera data
private boolean turretVisionStatus = false; // Can the camera provide turret vision data?
private double turretTheta = 0; // Delta theta of turret to target
private double hubDistance = 0; // Distance to hub
private boolean ballDetected = false; // If intake camera has detected a ball

@Override
public void run() {
while (true) {
try {
// If thread interrupted
if (Thread.interrupted()) return;

// Check if we need to connect
if (stdIn == null || socket == null || socket.isClosed() || !socket.isConnected() || !socket.isBound()) {
isConnected = connect();

if (!isConnected) {
System.out.println("Unable to connect to Jetson");
// Wait before trying to connect again
Thread.sleep(500);
}
} else {
readCameraData();
}
} catch (InterruptedException e) {
// If interrupted, exit
return;
} catch (Exception e) {
System.out.println("Outer exception caught in CAMERA code. Unknown error. Will keep trying to connect to socket");
}
}
}

/**
* Creates an MJPEG camera over the Jetson connection.
* @param port The port to connect to.
* @return The camera object.
*/
public HttpCamera createCamera(String name, int port) {
return new HttpCamera(name + " - Port " + port, "http://" + jetsonIP + ":" + port + "/?action=stream");
/**
* Attempt to connect to the jetson socket.
* @return If the connection was successful.
*/
public boolean connect() {
try {
socket = new Socket(jetsonIP, jetsonPort);
stdIn = new BufferedReader(new InputStreamReader(socket.getInputStream()));
System.out.println("Connected to Jetson address=" + jetsonIP + " at port=" + jetsonPort);

return true;
} catch (UnknownHostException e) {
socket = null;
stdIn = null;
} catch (IOException e) {
socket = null;
stdIn = null;
} catch (Exception e) {
socket = null;
stdIn = null;
System.out.println("UNKNOWN ERROR: something went wrong in camera connection!");
}
return false;
}

/**
* Read the camera data and update stored values.
* data: (bool -- turret vision status, double -- turret theta, double -- hub distance, boolean -- ball detected)
*/
public void readCameraData() throws InterruptedException {
try {
String in = stdIn.readLine();
if (in != null) {
String[] data = in.replace("(", "").replace(")", "").split(",");

System.out.println(Arrays.toString(data));

turretVisionStatus = strToBool(data[0]);
turretTheta = Double.parseDouble(data[1]);
hubDistance = Double.parseDouble(data[2]);
ballDetected = strToBool(data[3]);
}
} catch (IOException e) {
e.printStackTrace();
} catch (NullPointerException e) {
System.out.println("Unable to parse camera data: NullPointerException");
} catch (NumberFormatException e) {
System.out.println("Unable to parse camera data: NumberFormatException");
}
}

/**
* Converts string to a boolean. Either "False" or "True".
* @param str The string to convert.
* @return The boolean value.
*/
private boolean strToBool(String str) {
return str.equalsIgnoreCase("true");
}

/**
* Returns whether the turret camera can see vision targets, in case there are momentary lapses in vision or the
* hub is currently in the turntable blind spot.
* @return Whether turret vision is working.
*/
public boolean turretVisionWorking() {
return turretVisionStatus;
}

/**
* Gets the calculated turret angle.
* @return The desired turntable angle.
*/
public double getTurretTheta() {
return turretTheta;
}

/**
* Gets the calculated hub distance.
* @return The distance from the camera to the hub.
*/
public double getHubDistance() {
return hubDistance;
}

/**
* Gets whether a ball is in range of the intake camera.
* @return Whether a ball is in intake range.
*/
public boolean ballDetected() {
return ballDetected;
}

/**
* Creates an MJPEG camera over the Jetson connection.
* @param port The port to connect to.
* @return The camera object.
*/
public HttpCamera createCamera(String name, int port) {
return new HttpCamera(name + " - Port " + port, "http://" + jetsonIP + ":" + port + "/?action=stream");
}
}
}
}
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ private IntakePosition(double value) {
private IntakePosition currentPosition = IntakePosition.DEPLOYED;

private double intakePower = 0;
private boolean internalsFull = false;

// Deploy position PID constants
private static final double kP = 0.125;
Expand Down Expand Up @@ -114,12 +115,10 @@ public void periodic() {
// and there are less than 2 balls in internals, run the intake motor
// TODO: how should we work in the jetson ball detection code with variable
// intake speeds from the driver?
// Also, if running the intake in reverse is an option, how should internals
// ball count be worked into this?
boolean readyToIntake = /* internalSubsystem.getBallCount() < 2 && */
boolean readyToIntake = internalSubsystem.getBallCount() < 2 &&
currentPosition == IntakePosition.DEPLOYED;

intake.set(readyToIntake ? intakePower : 0);
intake.set((readyToIntake) ? intakePower : 0);

shuffleboardDeployPosition.setValue(deploy.getSelectedSensorPosition());
}
Expand Down
Loading

0 comments on commit 53216dc

Please sign in to comment.