tri11paragon
/
4p78-final-project
mirror of https://github.com/tri11paragon/4p78-final-project.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

updated protocol, fixed turning being reversed

main
ParkerTenBroeck 2025-04-23 20:39:01 -04:00
parent 9c9583b5a3
commit e83243ade5
10 changed files with 264 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
jdbg/src/Future.java Normal file
View File

@ -0,0 +1,59 @@
import java.util.concurrent.TimeoutException;
import java.util.function.Consumer;
public abstract class Future<T, E extends Throwable> {
private T val;
private E err;
private Consumer<T> then = ignore -> {
this.accepted = false;
this.notifyAll();
};
private Consumer<E> error = ignore -> {
this.accepted = false;
this.notifyAll();
};
private boolean accepted;
protected synchronized void resolve(T val) {
this.val = val;
accepted = true;
then.accept(this.val);
}
protected synchronized void resolve(E err) {
this.err = err;
accepted = true;
error.accept(this.err);
}
public synchronized Future<T, E> then(Consumer<T> then) {
this.then = then;
if (this.val != null && !accepted) {
accepted = true;
this.then.accept(this.val);
}
return this;
}
public synchronized Future<T, E> error(Consumer<E> error) {
this.error = error;
if (this.err != null && !accepted) {
accepted = true;
this.error.accept(this.err);
}
return this;
}
public synchronized T await(long ms) throws InterruptedException, TimeoutException, E {
if (this.val == null && this.err == null) this.wait(ms);
if (this.val == null && this.err == null) throw new TimeoutException();
if (accepted) throw new IllegalStateException("Value already accepted");
accepted = true;
if (this.err != null) throw this.err;
return val;
}
public T await() throws InterruptedException, E, TimeoutException {
return await(0);
}
}

108
jdbg/src/Gui.java
View File

@ -1,20 +1,24 @@
import javax.swing.*;
import java.awt.*;
import java.awt.event.*;
import java.io.IOException;
import java.net.SocketException;
import java.net.UnknownHostException;
import java.util.ArrayList;
public class Gui extends JFrame implements KeyListener, MouseWheelListener {
private VisualPanel visualPanel;
private JButton zeroButton;
private JButton averageButton;
private Robot robot;
record Pos(double x, double y){}
private final ArrayList<Pos> maps = new ArrayList<>();
public Gui() throws SocketException, UnknownHostException {
robot = new Robot();
setTitle("Wrobot");
setSize(600, 600);
setSize(1200, 600);
setDefaultCloseOperation(EXIT_ON_CLOSE);
setLayout(new BorderLayout());
@ -23,27 +27,17 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener {
zeroButton = new JButton("Zero");
zeroButton.addActionListener(e -> {
try {
robot.sendZero();
new Thread(() -> {
var sum = 0.0;
int number = 500;
for(int i = 0; i < number; i ++){
try {
sum += robot.getEverything()[19-1];
} catch (IOException ex) {
throw new RuntimeException(ex);
}
}
System.out.println(sum/number);
}).start();
} catch (IOException ex) {
throw new RuntimeException(ex);
}
robot.sendZero().error(ex -> ex.printStackTrace());
maps.clear();
});
zeroButton.setFocusable(false);
add(zeroButton, BorderLayout.SOUTH);
averageButton = new JButton("Find Average Angle");
averageButton.addActionListener( e -> averageAngle());
averageButton.setFocusable(false);
add(averageButton, BorderLayout.NORTH);
setFocusable(true);
addKeyListener(this);
@ -55,8 +49,7 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener {
new Thread(() -> {
while(true){
try{
var data = robot.getDataPlus();
visualPanel.updateData(data);
visualPanel.updateData(robot.getDataPlus().await(1000));
Thread.sleep(20);
}catch (Exception e){
e.printStackTrace();
@ -66,6 +59,21 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener {
}
public void averageAngle(){
new Thread(() -> {
var sum = 0.0;
int number = 500;
for(int i = 0; i < number; i ++){
try {
sum += robot.getEverything().await()[19-1];
} catch (Exception ex) {
throw new RuntimeException(ex);
}
}
System.out.println(sum/number);
}).start();
}
@Override
public void keyPressed(KeyEvent e) {
@ -77,8 +85,8 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener {
case KeyEvent.VK_S -> visualPanel.offsetY -= panStep;
case KeyEvent.VK_A -> visualPanel.offsetX += panStep;
case KeyEvent.VK_D -> visualPanel.offsetX -= panStep;
case KeyEvent.VK_PLUS, KeyEvent.VK_EQUALS -> visualPanel.zoom *= 1.1;
case KeyEvent.VK_MINUS -> visualPanel.zoom /= 1.1;
case KeyEvent.VK_PLUS, KeyEvent.VK_EQUALS -> visualPanel.zoom *= 1.1f;
case KeyEvent.VK_MINUS -> visualPanel.zoom /= 1.1f;
}
repaint();
@ -108,18 +116,21 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener {
public void mouseClicked(MouseEvent e) {
Point screenPoint = e.getPoint();
float worldX = (screenPoint.x - getWidth() / 2f - offsetX) / zoom;
float worldY = -((screenPoint.y - getHeight() / 2f - offsetY) / zoom);
try {
float worldY = -(screenPoint.y - getHeight() / 2f - offsetY) / zoom;
robot.sendTargetPos(worldX, worldY);
} catch (IOException ex) {
throw new RuntimeException(ex);
}
}
});
}
public void updateData(Robot.DataPacketPlus dpp) {
this.dpp = dpp;
if(dpp.distance()<2500){
var x = dpp.x() + 0.0393701*dpp.distance()*Math.cos((dpp.yaw()+90)/180*Math.PI);
var y = dpp.y() + 0.0393701*dpp.distance()*Math.sin((dpp.yaw()+90)/180*Math.PI);
synchronized (maps){
maps.add(new Pos(x, y));
}
}
repaint();
}
@ -135,26 +146,47 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener {
int width = getWidth();
int height = getHeight();
// Flip back to draw text
g.scale(1, 1);
g.setColor(Color.WHITE);
g.fillRect(0, 0, width, height);
g.translate((double) width / 2 + offsetX, (double) height / 2 + offsetY);
g.scale(1, -1); // Y-up
int px = Math.round(dpp.x() * zoom);
int py = Math.round(dpp.y() * zoom);
g.setColor(Color.BLACK);
g.drawString(String.format("Position: (%.2f, %.2f)", dpp.x(), dpp.y()), -getWidth() / 2.0f + 10 - offsetX, -getHeight() / 2.0f + 20 - offsetY);
g.drawString(String.format("Yaw: %.2f°", dpp.yaw()), -getWidth() / 2.0f + 10 - offsetX, -getHeight() / 2.0f + 40 - offsetY);
g.drawString(String.format("Target Yaw: %.2f°", dpp.t_yaw()), -getWidth() / 2.0f - offsetX + 10, -getHeight() / 2.0f + 60 - offsetY);
g.drawString(String.format("Zoom: %.1f", zoom), -getWidth() / 2.0f - offsetX + 10, -getHeight() / 2.0f + 80 - offsetY);
int pt_x = Math.round(dpp.t_x() * zoom);
int pt_y = Math.round(dpp.t_y() * zoom);
g.scale(zoom, -zoom); // Y-up
int px = Math.round(dpp.x());
int py = Math.round(dpp.y());
int pt_x = Math.round(dpp.t_x());
int pt_y = Math.round(dpp.t_y());
// Target
g.setColor(Color.GRAY);
g.fillOval(pt_x-3, pt_y-3, 6, 6);
g.fillOval(pt_x - 3, pt_y - 3, 6, 6);
// Position dot
g.setColor(Color.BLUE);
g.fillOval(px - 5, py - 5, 10, 10);
g.setColor(Color.ORANGE);
ArrayList<Pos> cln;
synchronized (maps) {
cln = new ArrayList<>(maps);
}
for (var pos : cln) {
g.fillOval((int) (pos.x - 1), (int) (pos.y - 2), 2, 2);
}
// Yaw direction
{
float yawRad = (float) Math.toRadians(dpp.yaw());
@ -176,14 +208,6 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener {
// Line to origin
g.setColor(Color.GREEN.darker());
g.drawLine(pt_x, pt_y, px, py);
// Flip back to draw text
g.scale(1, -1);
g.setColor(Color.BLACK);
g.drawString(String.format("Position: (%.2f, %.2f)", dpp.x(), dpp.y()), -getWidth() / 2 + 10, -getHeight() / 2 + 20);
g.drawString(String.format("Yaw: %.2f°", dpp.yaw()), -getWidth() / 2 + 10, -getHeight() / 2 + 40);
g.drawString(String.format("Target Yaw: %.2f°", dpp.t_yaw()), -getWidth() / 2 + 10, -getHeight() / 2 + 60);
g.drawString(String.format("Zoom: %.1f", zoom), -getWidth() / 2 + 10, -getHeight() / 2 + 80);
}
}
}

13
jdbg/src/PidPanel.java
View File

@ -81,9 +81,8 @@ public class PidPanel extends JPanel {
this.add(buttonRow);
getPidButton.addActionListener(e -> {
try {
int index = indexDropdown.getSelectedIndex();
Robot.PID pid = robot.getPID(index);
robot.getPID(index).then(pid -> {
kpSlider.setValue((int) (pid.kp() * SCALE));
kiSlider.setValue((int) (pid.ki() * SCALE));
kdSlider.setValue((int) (pid.kd() * SCALE));
@ -92,25 +91,23 @@ public class PidPanel extends JPanel {
} else {
reverseButton.setSelected(true);
}
} catch (Exception ex) {
}).error(ex -> {
ex.printStackTrace();
JOptionPane.showMessageDialog(this, "Failed to get PID: " + ex.getMessage());
}
});
});
setPidButton.addActionListener(e -> {
try {
int index = indexDropdown.getSelectedIndex();
float kp = (float) (kpSlider.getValue() / SCALE);
float ki = (float) (kiSlider.getValue() / SCALE);
float kd = (float) (kdSlider.getValue() / SCALE);
int direction = directButton.isSelected() ? 0 : 1;
robot.setPID(index, kp, ki, kd, direction);
} catch (Exception ex) {
robot.setPID(index, kp, ki, kd, direction).error(ex -> {
ex.printStackTrace();
JOptionPane.showMessageDialog(this, "Failed to set PID: " + ex.getMessage());
}
});
});
getPidButton.doClick();
indexDropdown.addActionListener(a -> {

85
jdbg/src/Robot.java
View File

@ -2,53 +2,86 @@ import java.io.IOException;
import java.net.*;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.rmi.server.ExportException;
import java.util.HashMap;
import java.util.function.Consumer;
import java.util.function.Function;
public class Robot{
DatagramSocket socket = new DatagramSocket();
private int sequence = 0;
final String ip;
final int port;
final InetAddress address;
private final ByteBuffer buf = ByteBuffer.allocate(4096).order(ByteOrder.LITTLE_ENDIAN);
private final ByteBuffer outBuf = ByteBuffer.allocate(4096).order(ByteOrder.LITTLE_ENDIAN);
private final HashMap<Integer, PFuture<?>> seqMap = new HashMap<>();
public static abstract class PFuture<T> extends Future<T, Exception>{
protected abstract void resolve(ByteBuffer bb);
}
public Robot() throws SocketException, UnknownHostException {
this("192.168.5.11", 42069);
this("192.168.4.1", 42069);
}
public Robot(String ip, int port) throws SocketException, UnknownHostException {
this.ip = ip;
address = InetAddress.getByName(ip);
this.port = port;
}
public synchronized <T> T packet(int id, Consumer<ByteBuffer> setup, Function<ByteBuffer, T> func) throws IOException {
setup.accept(buf.clear().limit(buf.capacity()).putInt(id));
DatagramPacket packet = new DatagramPacket(buf.array(), buf.position(), address, port);
socket.send(packet);
buf.clear();
DatagramPacket receivePacket = new DatagramPacket(buf.array(), buf.capacity());
socket.setSoTimeout(1000);
new Thread(() -> {
var inBuf = ByteBuffer.allocate(4096).order(ByteOrder.LITTLE_ENDIAN);
while(true){
try{
inBuf.clear();
DatagramPacket receivePacket = new DatagramPacket(inBuf.array(), inBuf.capacity());
socket.receive(receivePacket);
buf.limit(receivePacket.getLength());
inBuf.limit(receivePacket.getLength());
if(buf.getInt() != id)throw new IOException("ID miss match");
var seq = buf.getInt();
return func.apply(buf);
int squ = inBuf.getInt();
var future = seqMap.remove(squ);
if(future!=null) future.resolve(inBuf);
}catch (Exception ignore){}
}
}).start();
}
public synchronized <T> PFuture<T> packet(int id, Consumer<ByteBuffer> setup, Function<ByteBuffer, T> func) {
var f = new PFuture<T>(){
@Override
protected synchronized void resolve(ByteBuffer bb) {
try{
if(id!=bb.getInt())
super.resolve(new Exception("ID Miss Match"));
else
super.resolve(func.apply(bb));
}catch (Exception e){
super.resolve(e);
}
}
};
seqMap.put(sequence, f);
try{
setup.accept(outBuf.clear().limit(outBuf.capacity()).putInt(sequence++).putInt(id));
DatagramPacket packet = new DatagramPacket(outBuf.array(), outBuf.position(), address, port);
socket.send(packet);
}catch (Exception e){
f.resolve(e);
}
return f;
}
public void sendZero() throws IOException{
packet(0, bb -> {}, bb -> null);
public PFuture<Void> sendZero() {
return packet(0, bb -> {}, bb -> null);
}
public record DataPacket(float yaw, float distance, float x, float y){}
public DataPacket getData() throws IOException {
public PFuture<DataPacket> getData() {
return packet(1, bb -> {}, bb -> {
float yaw = bb.getFloat();
float distance = bb.getFloat();
@ -59,11 +92,11 @@ public class Robot{
});
}
public void sendTargetPos(float x, float y) throws IOException{
packet(2, bb -> bb.putFloat(x).putFloat(y), bb -> null);
public PFuture<Void> sendTargetPos(float x, float y) {
return packet(2, bb -> bb.putFloat(x).putFloat(y), bb -> null);
}
public float[] getEverything() throws IOException{
public PFuture<float[]> getEverything() {
return packet(3, bb -> {}, bb -> {
float[] arr = new float[bb.remaining()/4];
for(int i = 0; i < arr.length; i ++)
@ -75,7 +108,7 @@ public class Robot{
public record DataPacketPlus(float yaw, float t_yaw, float distance, float x, float y, float t_x, float t_y){}
public DataPacketPlus getDataPlus() throws IOException {
public PFuture<DataPacketPlus> getDataPlus() {
return packet(4,
bb -> {},
bb -> {
@ -92,15 +125,15 @@ public class Robot{
}
public record PID(float kp, float ki, float kd, int direction){}
public PID getPID(int index) throws IOException{
public PFuture<PID> getPID(int index) {
return packet(5,
bb -> bb.putInt(index),
bb -> new PID(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getInt())
);
}
public void setPID(int index, float kp, float ki, float kd, int direction) throws IOException{
packet(6,
public PFuture<Void> setPID(int index, float kp, float ki, float kd, int direction) {
return packet(6,
bb -> bb
.putInt(index)
.putFloat(kp)

1
robot/distance.cpp
View File

@ -5,7 +5,6 @@ Adafruit_VL53L0X lox = Adafruit_VL53L0X();
float distanceReading = 0;
void initDistance(){
if (!lox.begin(0x29, false, &Wire1, Adafruit_VL53L0X::VL53L0X_SENSE_LONG_RANGE)) {
Serial.println(F("Failed to boot VL53L0X"));
while(1);

25
robot/encoder.cpp
View File

@ -62,19 +62,36 @@ void updateEncoder(){
float displacement = (d_left + d_right)/2;
float oldAng = odom.angle;
odom.angle += (d_left-d_right)/(WHEEL_DISTANCE);
odom.angle += (d_right-d_left)/(WHEEL_DISTANCE);
float ang = (odom.angle+oldAng)/2;
currentYaw=odom.angle*180/M_PI;
float dispx = desiredPos.x-odom.x;
float dispy = desiredPos.y-odom.y;
posInput = sqrt(dispx*dispx+dispy*dispy);
desiredYaw = atan2(dispy, dispx)*180/PI;
if(abs(fmod(currentYaw-desiredYaw, 180.0))>90){
desiredYaw = fmod(desiredYaw+180.0, 360.0);
desiredYaw = fmod(desiredYaw+360, 360);
currentYaw = fmod(fmod(currentYaw, 360)+360, 360);
double yd = desiredYaw-currentYaw;
if (yd >= 180)
yd -= 360;
else if (yd <= -180)
yd += 360;
//display
desiredYaw = currentYaw+yd;
turnInput = yd;
if (abs(yd) > 90) {
turnInput = fmod((yd<0?1:-1)*180+yd, 180);
posInput = -posInput;
//display
desiredYaw += 180;
}
// Serial.println(desiredYaw);
posInput *= cos(turnInput*PI/180);
odom.x += (float)(cos(ang)*displacement);
odom.y += (float)(sin(ang)*displacement);

2
robot/headers.h
View File

@ -1,5 +1,5 @@
#define LOOP_INTERVAL_MS 20
#define LOOP_INTERVAL_MS 10
struct FVec2{
float x,y;

11
robot/pid.cpp
View File

@ -6,9 +6,9 @@
//double turnKp=2, turnKi=0.0, turnKd=0.0;
double angKp=4.0, angKi=106.75, angKd=0.0472;
double posKp=0.96, posKi=1.28, posKd=1.0;
double turnKp=1, turnKi=5.0, turnKd=0.17;
double angKp=4, angKi=60.0, angKd=0.0958;
double posKp=0.5, posKi=0.0, posKd=0.5;
double turnKp=1.25, turnKi=3.0, turnKd=0.0;
double angleInput, angleOutput, angleSetpoint;
@ -18,7 +18,7 @@ double posInput, posOutput, posSetpoint;
PID posPID(&posInput, &posOutput, &posSetpoint, posKp, posKi, posKd, P_ON_E, DIRECT);
double turnInput, turnOutput, turnSetpoint;
PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_M, DIRECT);
PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_E, DIRECT);
PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID};
@ -46,10 +46,9 @@ Speeds updatePID(){
angleSetpoint = posOutput;
anglePID.Compute();
turnSetpoint = 0;
float maxTurn = max(0.0f, 25.0f-abs((float)angleOutput));
turnPID.SetOutputLimits(-maxTurn, maxTurn);
turnSetpoint = 0;
turnInput = fmod(currentYaw-desiredYaw, 180);
turnPID.Compute();
Speeds speeds;

55
robot/robot.ino
View File

@ -1,6 +1,5 @@
#include "headers.h"
#define PWM_FREQ 400
void wire1(){
Wire1.begin(SDA, SCL);
@ -12,12 +11,7 @@ void wire2(){
Wire2.setClock(400000);
}
#include <Servo.h>
Servo left;
Servo right;
float angleOffset = 4.620817395210266;
float angleOffset = 2.5033416018486023;
float desiredYaw = 0.0;
float currentYaw = 0.0;
@ -33,8 +27,10 @@ void initSerial(){
}
void initMotors(){
left.attach(D3);
right.attach(D4);
pinMode(D3, OUTPUT);
pinMode(D4, OUTPUT);
analogWriteFreq(PWM_FREQ);
}
void initI2C(){
@ -46,14 +42,16 @@ void initI2C(){
void setup() {
initSerial();
initWifi(false);
initServer();
initMotors();
initPID();
initI2C();
initDistance();
initGyro();
initEncoder();
initWifi(true);
initServer();
initGyro();
}
void loop() {
@ -65,25 +63,48 @@ void loop() {
angle -= 180;
angleInput = angle - angleOffset;
}
long gyro = millis();
updateEncoder();
currentYaw=odom.angle*180/M_PI;
long encoder = millis();
updateDistance();
long distance = millis();
updateServer();
long server = millis();
Speeds speeds = updatePID();
long pid = millis();
speeds.left = min(90.0f-10, max(-90.0f+10, speeds.left));
speeds.right = min(90.0f-10, max(-90.0f+10, speeds.right));
left.write(90+(int)speeds.left);
right.write(90+(int)speeds.right);
const int MAX_FOR = (int)(0.0010/(1.0/PWM_FREQ)*255);
const int MAX_REV = (int)(0.0020/(1.0/PWM_FREQ)*255);
analogWrite(D3, map((int)speeds.left, 90-10, -90+10, MAX_REV, MAX_FOR));
analogWrite(D4, map((int)speeds.right, 90-10, -90+10, MAX_REV, MAX_FOR));
long end = millis();
if(end-start>LOOP_INTERVAL_MS){
Serial.print("Overran ");
Serial.println(end-start);
Serial.print(gyro-start);
Serial.print(" ");
Serial.print(encoder-gyro);
Serial.print(" ");
Serial.print(distance-encoder);
Serial.print(" ");
Serial.print(server-distance);
Serial.print(" ");
Serial.print(pid-server);
Serial.print(" ");
Serial.println(end-pid);
}else{
delay(LOOP_INTERVAL_MS-(end-start));
}

7
robot/webserver.cpp
View File

@ -179,6 +179,7 @@ struct SetPIDPacket{
};
struct Packet{
uint32_t sequence;
uint32_t id;
union{
ZeroPacket zero;
@ -282,16 +283,14 @@ bool handleUDP(){
constexpr size_t buffer_size = 256;
alignas(alignof(float)) char buffer[buffer_size];
if (size>=4){
if (size>=8){
int len = Udp.read(buffer, buffer_size);
Packet* packet = (Packet*)buffer;
static uint32_t sequence = 0;
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write((const char*)&packet->sequence, sizeof(packet->sequence));
Udp.write((const char*)&packet->id, sizeof(packet->id));
Udp.write((const char*)&sequence, sizeof(sequence));
sequence++;
switch(packet->id){
case ZeroPacket::ID: