TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

RedCrab: First pass of odometry/localization

Browse Source
This commit is contained in:
Cyberarm
2024-01-26 20:53:59 -06:00
parent 06228b955c
commit dca17c786d
5 changed files with 187 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java
View File

@@ -4,6 +4,9 @@ import android.util.Log;
import java.io.ByteArrayOutputStream;
import java.io.IOException;
import java.net.DatagramPacket;
import java.net.InetAddress;
import java.net.MulticastSocket;
import java.nio.charset.StandardCharsets;
import java.util.ArrayList;
import java.util.Locale;
@@ -16,8 +19,16 @@ public class CyberarmTelemetry {
private static final int MAX_PACKET_RAW_SIZE = 508; // bytes
private static final int PACKET_HEADER_SIZE = 16; // bytes
private static final int MAX_PACKET_BODY_SIZE = MAX_PACKET_RAW_SIZE - PACKET_HEADER_SIZE; // bytes
private static final String MULTICAST_ADDRESS = "236.77.83.76";
private static final int MULTICAST_PORT = 6388;
private static final int MULTICAST_TTL = 5;
private static final String TAG = "CYBERARM_TELEMETRY";
private final ArrayList<ByteArrayOutputStream> queueBuffer = new ArrayList<>();
private InetAddress group;
private MulticastSocket multicastSocket;
private boolean usable = false;
enum Encode {
// Generic/Type encodings
INTEGER,
@@ -33,10 +44,35 @@ public class CyberarmTelemetry {
GAMEPAD, // all 15 buttons + joysticks + triggers input values
MOTOR, // current power, velocity, position, target position, and current (amps)
SERVO, // current target position
CONTINUOUS_SERVO, // current power
SENSOR_2M_DISTANCE, // Rev 2 meter distance sensor
SENSOR_DIGITAL, // touch or other digital/binary sensor
}
CyberarmTelemetry() {
try {
this.group = InetAddress.getByName(MULTICAST_ADDRESS);
this.multicastSocket = new MulticastSocket(MULTICAST_PORT);
this.multicastSocket.setTimeToLive(MULTICAST_TTL);
this.multicastSocket.joinGroup(group);
this.usable = true;
} catch (IOException e) {
Log.e(TAG, "FAILED to create multicast socket!");
e.printStackTrace();
this.usable = false;
}
}
/**
* Whether the multicast socket was successfully created
* @return usable
*/
public boolean isUsable() {
return usable;
}
public void publish() throws IOException
{
ByteArrayOutputStream buffer = new ByteArrayOutputStream();
@@ -99,8 +135,15 @@ public class CyberarmTelemetry {
output.write(buffer.toByteArray());
}
private void commitPacket(ByteArrayOutputStream buffer) {
private void commitPacket(ByteArrayOutputStream buffer) throws IOException {
// TODO: send multicast packet(s) on LAN
// Drop packet if multicast socket is not yet created or failed to be created.
if (!isUsable())
return;
DatagramPacket packet = new DatagramPacket(buffer.toByteArray(), buffer.size(), group, MULTICAST_PORT);
multicastSocket.send(packet);
}
private void packInt(ByteArrayOutputStream stream, int i) throws IOException {
@@ -113,28 +156,24 @@ public class CyberarmTelemetry {
stream.write(Long.toBinaryString(i).getBytes());
}
private void packFloat(ByteArrayOutputStream stream, float i, int precision) throws IOException {
private void packFloat(ByteArrayOutputStream stream, float i) throws IOException {
stream.write(Encode.FLOAT.ordinal());
String string = String.format("%." + precision + "f", i, Locale.US);
stream.write(string.length());
stream.write(string.getBytes(StandardCharsets.UTF_8));
String[] string = String.format(Locale.US, "%.8f", i).split("\\.");
int wholeNum = Integer.parseInt(string[0]);
int decimalPart = Integer.parseInt(string[1]);
stream.write(wholeNum);
stream.write(decimalPart);
}
private void packFloat(ByteArrayOutputStream stream, float i) throws IOException
{
packFloat(stream, i, 3);
}
private void packDouble(ByteArrayOutputStream stream, double i, int precision) throws IOException {
private void packDouble(ByteArrayOutputStream stream, double i) throws IOException {
stream.write(Encode.DOUBLE.ordinal());
String string = String.format("%." + precision + "f", i, Locale.US);
stream.write(string.length());
stream.write(string.getBytes(StandardCharsets.UTF_8));
}
String[] string = String.format(Locale.US, "%.8f", i).split("\\.");
int wholeNum = Integer.parseInt(string[0]);
int decimalPart = Integer.parseInt(string[1]);
private void packDouble(ByteArrayOutputStream stream, double i) throws IOException
{
packDouble(stream, i, 3);
stream.write(wholeNum);
stream.write(decimalPart);
}
private void packString(ByteArrayOutputStream stream, String i) throws IOException {

87
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java Normal file
View File

@@ -0,0 +1,87 @@
package dev.cyberarm.minibots.red_crab;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.slf4j.helpers.Util;
import dev.cyberarm.engine.V2.Utilities;
public class Localizer {
private final RedCrabMinibot robot;
private double rawX = 0, rawY = 0, rawR = 0;
private double trackWidthMM = 365.0, forwardOffsetMM = -140.0, wheelDiameterMM = 90.0;
private int lastEncoderXLeft = 0, lastEncoderXRight = 0, lastEncoderYCenter = 0;
public Localizer(RedCrabMinibot robot) {
this.robot = robot;
// Preset last encoder to current location to not require resetting encoders, ever. (🤞)
this.lastEncoderXLeft = robot.deadWheelXLeft.getCurrentPosition();
this.lastEncoderXRight = robot.deadWheelXRight.getCurrentPosition();
this.lastEncoderYCenter = robot.deadWheelYCenter.getCurrentPosition();
}
public void reset() {
rawX = 0;
rawY = 0;
rawR = 0;
}
// FIXME
public void teleport(double xMM, double yMM) {
this.rawX = xMM;
this.rawY = yMM;
}
// FIXME
public void teleport(double xMM, double yMM, double headingDegrees) {
this.rawX = xMM;
this.rawY = yMM;
this.rawR = (AngleUnit.DEGREES).toRadians(AngleUnit.normalizeDegrees(headingDegrees)); // cursed :(
}
public void integrate() {
int leftEncoder = robot.deadWheelXLeft.getCurrentPosition();
int rightEncoder = robot.deadWheelXRight.getCurrentPosition();
int centerEncoder = robot.deadWheelYCenter.getCurrentPosition();
double heading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
int deltaLeft = leftEncoder - lastEncoderXLeft;
int deltaRight = rightEncoder - lastEncoderXRight;
int deltaCenter = centerEncoder - lastEncoderYCenter;
double phi = (deltaLeft - deltaRight) / trackWidthMM;
double deltaMiddle = (deltaLeft + deltaRight) / 2.0;
double deltaPerp = deltaCenter - forwardOffsetMM * phi;
double deltaX = deltaMiddle * Math.cos(heading) - deltaPerp * Math.sin(heading);
double deltaY = deltaMiddle * Math.sin(heading) + deltaPerp * Math.cos(heading);
rawX += deltaX;
rawY += deltaY;
rawR += phi;
lastEncoderXLeft = leftEncoder;
lastEncoderXRight = rightEncoder;
lastEncoderYCenter = centerEncoder;
}
public double xMM() {
return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawX);
}
public double yMM() {
return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawY);
}
// FIXME
public double headingDegrees() {
return rawR;
}
// FIXME? (report radians as halves or proper whole?)
public double headingRadians() {
return rawR;
}
}

37
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java
View File

@@ -20,6 +20,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
import dev.cyberarm.drivers.EncoderCustomKB2040;
import dev.cyberarm.engine.V2.CyberarmEngine;
import dev.cyberarm.engine.V2.Utilities;
@@ -82,6 +83,8 @@ public class RedCrabMinibot {
public final DcMotorEx frontLeft, frontRight, backLeft, backRight, winch;
public final DcMotorEx clawArm;
public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm;
public final DcMotorEx deadWheelXLeft, deadWheelXRight;
public final EncoderCustomKB2040 deadWheelYCenter;
final CyberarmEngine engine;
@@ -110,6 +113,8 @@ public class RedCrabMinibot {
/// Doohickey
public VisionPortal visionPortal = null;
public static Localizer localizer;
public RedCrabMinibot(boolean autonomous) {
engine = CyberarmEngine.instance;
@@ -228,8 +233,25 @@ public class RedCrabMinibot {
hookArm.setDirection(Servo.Direction.FORWARD);
// hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF:
/// DEAD WHEELS ///
deadWheelXLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("deadwheel_x_left");
deadWheelXRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("deadwheel_x_right");
deadWheelYCenter = engine.hardwareMap.get(EncoderCustomKB2040.class, "deadwheel_y_center");
deadWheelXLeft.setDirection(DcMotorSimple.Direction.REVERSE);
deadWheelXRight.setDirection(DcMotorSimple.Direction.FORWARD);
// deadWheelYCenter.setDirection(DcMotorSimple.Direction.FORWARD);
// Bulk read from hubs
Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL);
// Initialize localizer
if (autonomous || RedCrabMinibot.localizer == null) {
RedCrabMinibot.localizer = new Localizer(this);
}
if (autonomous)
RedCrabMinibot.localizer.reset();
}
public void reloadConfig() {
@@ -290,6 +312,21 @@ public class RedCrabMinibot {
public void standardTelemetry() {
engine.telemetry.addLine();
if (RedCrabMinibot.localizer != null) {
engine.telemetry.addLine("Localizer");
engine.telemetry.addData("X (MM)", "%.2fmm", RedCrabMinibot.localizer.xMM());
engine.telemetry.addData("Y (MM)", "%.2fmm", RedCrabMinibot.localizer.yMM());
engine.telemetry.addData("R (De)", "%.2fdeg", RedCrabMinibot.localizer.headingDegrees());
engine.telemetry.addLine();
}
engine.telemetry.addLine("Deadwheels");
engine.telemetry.addData("X Left", deadWheelXLeft.getCurrentPosition());
engine.telemetry.addData("X Right", deadWheelXRight.getCurrentPosition());
// Use .getLastPosition instead of .getCurrentPosition here to not make an additional round trip just for telemetry
engine.telemetry.addData("Y Center", deadWheelYCenter.getLastPosition());
engine.telemetry.addLine();
engine.telemetry.addLine("Motors");
engine.telemetry.addData(
"Front Left",

3
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java
View File

@@ -10,6 +10,9 @@ public abstract class RedCrabAutonomousEngine extends CyberarmEngine {
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
if (RedCrabMinibot.localizer != null) {
RedCrabMinibot.localizer.integrate();
}
super.loop();

3
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java
View File

@@ -22,6 +22,9 @@ public class RedCrabTeleOpEngine extends CyberarmEngine {
@Override
public void loop() {
Utilities.hubsClearBulkReadCache(hardwareMap);
if (RedCrabMinibot.localizer != null) {
RedCrabMinibot.localizer.integrate();
}
super.loop();
}