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

worked on report, fixed distance value being reported incorrectly, cleaned network/pid code up

main
ParkerTenBroeck 2025-04-26 22:40:25 -04:00
parent 31617fb86b
commit 5021afa0ed
10 changed files with 193 additions and 290 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
doc/doc.pdf
View File

Binary file not shown.

28
doc/doc.tex
View File

@ -44,7 +44,7 @@
\makeatletter
\begin{titlepage}
\def \LOGOPATH {brock.jpg} % Path to Brock logo
\def \LOGOPATH {brock.jpg}
\def \UNIVERSITY {Brock University}
\def \FACULTY {Faculty of Mathematics \& Science}
\def \DEPARTMENT {Department of Computer Science}
@ -89,17 +89,34 @@ What if you wanted to make a robot to map out a room but you only had two wheels
With only two wheels its purely dynamically stable, will fall over due to its own stupidity but its very cute while it does so! This little guy can wiz around at the speed of a snail your house and (poorly) map out a room to your hearts desire
\section{Instructions}
\begin{itemize}
\item Hold robot in a vertical upright position clear of obsticals
\item Place battery in top compartment
\item Plug battery in and wait for robot to initialize
\item Once robot has booted let go and step away
\item Connect computer to network \texttt{MEOW}
\item launch vidualization and control software
\end{itemize}
\section{Problem Set}
\subsection{Keeping Upright}
\subsection{Keeping A Position}
\subsection{Balancing}
We have a vertical two wheeled robot I hope its fairly obvious when I say that it is only dynamically stable. This presents a challance because we need some system which keeps the system upright in a variaty of situations and responds dynamically to changes it cannot predict.
\subsection{Odometry}
Knowing that our system is very dynamic and having the forsight to know that we will need to keep in constant motion to keep upright the odometry system will need to handle that.
\subsection{Target Positions}
The problem of target positions like odometry is made more challenging due to the fact we are almost constantly in motion. We simply cannot "not move" when we are in the location we want to be as we'd fall over. This means the target position will need to be a "best effort" battle to keep in the same place as best as we can.
\subsection{Communication}
The only source of computation used is a single \texttt{ESP8266} microcontroller, this in combination with the strict timing requirements on the duration of the control loop means the communication between the robot and the control/mapping software needs to be quick and efficient.
\section{Approaches}
\subsection{Balancing}
\subsection{Odometry}
\subsection{Target Positions}
The target position
\subsection{Communication}
\section{Challanages}
@ -110,7 +127,7 @@ Keeping track of our position and angle was another challange that required care
\subsection{Target Positions}
Because the robot is in constant motion keeping itself balanced without a "push" towards a single position it will drift around. To solve this we use the odometry system as a input to the movement system. By setting the desired heading of the robot to the vector from its position to the target position, and by biasing the direction the robot will travel to be the direction to the target position we get a crude way of staying in a single position.
\subsection{Efficient Communication}
\subsection{Communication}
Since we have limited processing power and time per loop iteration we need to be smart in how we receive and transmit data to our mapping software. For this reason we designed a stateless UDP based network protocol overtop the esp8266 Wifi \& UDP libraries. \cite{wifi_lib}
\section{Resources Used}
@ -121,6 +138,7 @@ Since we have limited processing power and time per loop iteration we need to be
\item The AS5600 library was used to interface with the AS5600 magnetic encoders on each wheel. \cite{enc_lib}
\item The MPU6050 library was used to interface and interpret the accelerometer and gyroscope data. \cite{gyro_lib}. It required modification to work with our hardware as it was a knockoff and the device ID was different than what it was expecting.
\item A blog by the author of the PID library used was very helpful when tuning and configuring the PID controls in the robot. \cite{pid_help}
\item A paper on odometry for robots with differential steering which we based our odometry system off of. \cite{odom_help}
\end{itemize}

25
doc/doc.toc
View File

@ -1,13 +1,18 @@
\contentsline {section}{\numberline {1}Introduction}{2}{section.1}%
\contentsline {section}{\numberline {2}Instructions}{2}{section.2}%
\contentsline {section}{\numberline {3}Problem Set}{2}{section.3}%
\contentsline {subsection}{\numberline {3.1}Keeping Upright}{2}{subsection.3.1}%
\contentsline {subsection}{\numberline {3.2}Keeping A Position}{2}{subsection.3.2}%
\contentsline {subsection}{\numberline {3.3}Communication}{2}{subsection.3.3}%
\contentsline {section}{\numberline {4}Approaches}{2}{section.4}%
\contentsline {section}{\numberline {5}Challanages}{2}{section.5}%
\contentsline {subsection}{\numberline {5.1}Balancing}{2}{subsection.5.1}%
\contentsline {subsection}{\numberline {5.2}Odometry}{2}{subsection.5.2}%
\contentsline {subsection}{\numberline {5.3}Target Positions}{3}{subsection.5.3}%
\contentsline {subsection}{\numberline {5.4}Efficient Communication}{3}{subsection.5.4}%
\contentsline {section}{\numberline {6}Resources Used}{3}{section.6}%
\contentsline {subsection}{\numberline {3.1}Balancing}{2}{subsection.3.1}%
\contentsline {subsection}{\numberline {3.2}Odometry}{2}{subsection.3.2}%
\contentsline {subsection}{\numberline {3.3}Target Positions}{3}{subsection.3.3}%
\contentsline {subsection}{\numberline {3.4}Communication}{3}{subsection.3.4}%
\contentsline {section}{\numberline {4}Approaches}{3}{section.4}%
\contentsline {subsection}{\numberline {4.1}Balancing}{3}{subsection.4.1}%
\contentsline {subsection}{\numberline {4.2}Odometry}{3}{subsection.4.2}%
\contentsline {subsection}{\numberline {4.3}Target Positions}{3}{subsection.4.3}%
\contentsline {subsection}{\numberline {4.4}Communication}{3}{subsection.4.4}%
\contentsline {section}{\numberline {5}Challanages}{3}{section.5}%
\contentsline {subsection}{\numberline {5.1}Balancing}{3}{subsection.5.1}%
\contentsline {subsection}{\numberline {5.2}Odometry}{4}{subsection.5.2}%
\contentsline {subsection}{\numberline {5.3}Target Positions}{4}{subsection.5.3}%
\contentsline {subsection}{\numberline {5.4}Communication}{4}{subsection.5.4}%
\contentsline {section}{\numberline {6}Resources Used}{4}{section.6}%

9
doc/references.bib
View File

@ -33,3 +33,12 @@
date = "June 20, 2017",
note = "\url{http://brettbeauregard.com/blog/2017/06/introducing-proportional-on-measurement/}",
}
@misc{odom_help,
key = "odom_help",
title = {{A Tutorial and Elementary Trajectory Model
for the Differential Steering System
of Robot Wheel Actuators}},
author = {{G.W. Lucas}},
note = "\url{https://rossum.sourceforge.net/papers/DiffSteer/}",
}

3
jdbg/src/Gui.java
View File

@ -4,6 +4,8 @@ import java.awt.event.*;
import java.net.SocketException;
import java.net.UnknownHostException;
import java.util.ArrayList;
import java.util.Timer;
import java.util.TimerTask;
public class Gui extends JFrame implements KeyListener, MouseWheelListener {
private VisualPanel visualPanel;
@ -50,7 +52,6 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener {
while(true){
try{
visualPanel.updateData(robot.getDataPlus().await(1000));
Thread.sleep(20);
}catch (Exception e){
e.printStackTrace();
}

4
jdbg/src/PidPanel.java
View File

@ -9,8 +9,8 @@ public class PidPanel extends JPanel {
String[] pidNames = {"angle", "pos", "turn"};
JComboBox<String> indexDropdown = new JComboBox<>(pidNames);
JSlider kpSlider = new JSlider(0, (int)(SCALE*10), 0);
JSlider kiSlider = new JSlider(0, (int)(SCALE*150), 0);
JSlider kpSlider = new JSlider(0, (int)(SCALE*1), 0);
JSlider kiSlider = new JSlider(0, (int)(SCALE*1), 0);
JSlider kdSlider = new JSlider(0, (int)(SCALE*2), 0);
JLabel kpValue = new JLabel("0.00");

2
robot/distance.cpp
View File

@ -19,5 +19,7 @@ void updateDistance(){
// if(distanceReading>2000){
// distanceReading = (float)NAN;
// }
}else{
distanceReading = 8000;
}
}

21
robot/pid.cpp
View File

@ -1,14 +1,9 @@
#include "headers.h"
#include <PID_v1.h>
//double angKp=3.5, angKi=80, angKd=0.042;
//double posKp=20, posKi=0.0, posKd=0.0;
//double turnKp=2, turnKi=0.0, turnKd=0.0;
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 angKp=0.0444, angKi=0.6666, angKd=0.001;
double posKp=0.8, posKi=0.2, posKd=1;
double turnKp=0.014, turnKi=0.0333, turnKd=0.0;
double angleInput, angleOutput, angleSetpoint;
@ -22,20 +17,22 @@ PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON
PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID};
const float MAX_TURN_SPEED = 0.15;
void initPID(){
angleSetpoint = 0;
anglePID.SetOutputLimits(-180, 180);
anglePID.SetOutputLimits(-1, 1); // speed forward/backward
anglePID.SetMode(AUTOMATIC);
anglePID.SetSampleTime(5);
posSetpoint = 0;
posPID.SetOutputLimits(-2, 2);
posPID.SetOutputLimits(-2, 2); // degrees forward/backward
posPID.SetMode(AUTOMATIC);
posPID.SetSampleTime(5);
posPID.SetControllerDirection(DIRECT);
turnSetpoint = 0;
turnPID.SetOutputLimits(-15, 15);
turnPID.SetOutputLimits(-MAX_TURN_SPEED, MAX_TURN_SPEED); // speed forward/backward
turnPID.SetMode(AUTOMATIC);
turnPID.SetSampleTime(5);
}
@ -47,7 +44,7 @@ Speeds updatePID(){
anglePID.Compute();
turnSetpoint = 0;
float maxTurn = max(0.0f, 25.0f-abs((float)angleOutput));
float maxTurn = max(0.0f, MAX_TURN_SPEED-abs((float)angleOutput)/90);
turnPID.SetOutputLimits(-maxTurn, maxTurn);
turnPID.Compute();

13
robot/robot.ino
View File

@ -1,5 +1,5 @@
#include "headers.h"
#define PWM_FREQ 400
#define PWM_FREQ 450
void wire1(){
Wire1.begin(SDA, SCL);
@ -54,6 +54,10 @@ void setup() {
initGyro();
}
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void loop() {
long start = millis();
@ -81,14 +85,11 @@ void loop() {
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));
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));
analogWrite(D3, mapfloat(speeds.left, -1, 1, MAX_FOR, MAX_REV));
analogWrite(D4, mapfloat(speeds.right, -1, 1, MAX_FOR, MAX_REV));
long end = millis();

346
robot/webserver.cpp
View File

@ -6,231 +6,69 @@
WiFiUDP Udp;
unsigned int localUdpPort = 42069;
#ifdef DO_WEB_SERVER
#include <ESPAsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <ArduinoJson.h>
#include <AsyncJson.h>
#include <AsyncMessagePack.h>
#include "page_html.h"
AsyncWebServer server(80);
void initWebServer(){
server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
request->send_P(200, "text/html", index_html);
});
server.on("/get_stuff_bin", HTTP_GET, [](AsyncWebServerRequest *request){
static float arr[4];
arr[0] = odom.angle;
arr[1] = distanceReading;
arr[2] = odom.x;
arr[3] = odom.y;
request->send(200, "application/octet-stream", (uint8_t*)(const char*)arr, sizeof(arr));
});
server.on("/fuckyou", HTTP_GET, [](AsyncWebServerRequest *request){
float arr[] = {
odom.angle,
distanceReading,
odom.x,
odom.y
};
request->send(200, "application/text", "hello");
});
server.on("/zero", HTTP_GET, [](AsyncWebServerRequest *request){
zeroOdom();
request->send(200);
});
server.on("/get_stuff", HTTP_GET, [](AsyncWebServerRequest *request){
char buff[1024];
int ret = snprintf(buff, sizeof(buff),
R"({
"motorTargetAngle": %f,
"distanceReading": %f,
"position": %f,
"anglePID": {"setpoint": %lf, "input": %lf, "output": %lf},
"posPID": {"setpoint": %lf, "input": %lf, "output": %lf},
"turnPID": {"setpoint": %lf, "input": %lf, "output": %lf},
"odom": {"left": %f, "right": %f, "x": %f, "y": %f, "angle": %f},
"ypr": {"yaw": %f, "pitch": %f, "roll": %f},
"euler": {"psi": %f, "theta": %f, "phi": %f},
"gravity": {"x": %f, "y": %f, "z": %f},
"q": {"w": %f, "x": %f, "y": %f, "z": %f},
"aa": {"x": %hd, "y": %hd, "z": %hd},
"gy": {"x": %hd, "y": %hd, "z": %hd},
"aaReal": {"x": %hd, "y": %hd, "z": %hd},
"aaWorld": {"x": %hd, "y": %hd, "z": %hd}
})",
(float)dbgState.motorTargetAngle,
(float)distanceReading,
0.0, //encoder.position(),
angleSetpoint, angleInput, angleOutput,
posSetpoint, posInput, posOutput,
turnSetpoint, turnInput, turnOutput,
odom.left, odom.right, odom.x, odom.y, odom.angle*180/M_PI,
ypr[0]*180/M_PI, ypr[1]*180/M_PI, ypr[2]*180/M_PI,
euler[0]*180/M_PI, euler[1]*180/M_PI, euler[2]*180/M_PI,
gravity.x, gravity.y, gravity.z,
q.w, q.x, q.y, q.z,
aa.x, aa.y, aa.z,
gy.x, gy.y, gy.z,
aaReal.x, aaReal.y, aaReal.z,
aaWorld.x, aaWorld.y, aaWorld.z
);
request->send(200, "application/json", buff);
});
server.on("/get_pid", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL,
[](AsyncWebServerRequest *request, uint8_t *bodyData, size_t bodyLen, size_t index, size_t total) {
StaticJsonDocument<256> json;
deserializeJson(json, &bodyData[index], bodyLen);
int idx = json["index"];
if(idx>3){
request->send(400);
return;
}
PID& pid = *pids[idx];
char buff[256];
int ret = snprintf(buff, sizeof(buff),
R"({"kp": %lf, "ki": %lf, "kd": %lf, "direction": %d})",
pid.GetKp(), pid.GetKi(), pid.GetKd(), pid.GetDirection()
);
request->send(200, "application/json", buff);
});
server.on("/set_desired_pos", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL,
[](AsyncWebServerRequest *request, uint8_t *bodyData, size_t bodyLen, size_t index, size_t total) {
StaticJsonDocument<256> json;
deserializeJson(json, &bodyData[index], bodyLen);
desiredPos.x = json["x"];
desiredPos.y = json["y"];
request->send(200);
});
server.on("/set_pid", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL,
[](AsyncWebServerRequest *request, uint8_t *bodyData, size_t bodyLen, size_t index, size_t total) {
StaticJsonDocument<256> json;
deserializeJson(json, &bodyData[index], bodyLen);
int idx = json["index"];
if(idx>3){
request->send(400);
return;
}
PID& pid = *pids[idx];
if(json.containsKey("kp"))
pid.SetTunings(json["kp"], json["ki"], json["kd"]);
if(json.containsKey("direction"))
pid.SetControllerDirection(json["direction"]);
request->send(200);
});
server.on("/set_desired_yaw", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL,
[](AsyncWebServerRequest *request, uint8_t *bodyData, size_t bodyLen, size_t index, size_t total) {
StaticJsonDocument<256> json;
deserializeJson(json, &bodyData[index], bodyLen);
desiredYaw = json["yaw"];
Serial.print(desiredYaw);
request->send(200);
});
server.begin();
}
#endif
void initServer(){
#ifdef DO_WEB_SERVER
initWebServer()
#endif
Udp.begin(localUdpPort);
}
struct ZeroPacket{
static constexpr uint32_t ID = 0;
};
struct GetDataPacket{
static constexpr uint32_t ID = 1;
};
struct SetTargetPacket{
static constexpr uint32_t ID = 2;
FVec2 pos;
};
struct EverythingPacket{
static constexpr uint32_t ID = 3;
};
struct GetDataPacketPlus{
static constexpr uint32_t ID = 4;
};
struct GetPIDPacket{
static constexpr uint32_t ID = 5;
uint32_t index;
};
struct SetPIDPacket{
static constexpr uint32_t ID = 6;
uint32_t index;
float kp,ki,kd;
uint32_t direction;
};
struct Packet{
uint32_t sequence;
uint32_t id;
union{
ZeroPacket zero;
GetDataPacket get_data;
SetTargetPacket set_target;
EverythingPacket everything;
GetDataPacketPlus get_data_plus;
GetPIDPacket get_pid_packet;
SetPIDPacket set_pid_packet;
} data;
};
struct DataPacket{
// ---------------- response packets
struct DataPacketResponse{
float yaw;
float distance;
FVec2 position;
};
struct DataPacketPlus{
struct DataPlusPacketResponse{
float yaw;
float desiredYaw;
float distance;
FVec2 position;
FVec2 targetPosition;
};
// ---------------- response packets
void respond_data_packet(){
DataPacket dp;
struct ZeroPacket{
static constexpr uint32_t ID = 0;
void handle(){
Udp.write((const char*)&ID, sizeof(ID));
zeroOdom();
desiredPos.x = 0;
desiredPos.y = 0;
}
};
struct GetDataPacket{
static constexpr uint32_t ID = 1;
void handle(){
Udp.write((const char*)&ID, sizeof(ID));
DataPacketResponse dp;
dp.yaw = currentYaw;
dp.distance = distanceReading;
dp.position.x = odom.x;
dp.position.y = odom.y;
Udp.write((const char*)&dp, sizeof(dp));
}
}
};
struct SetTargetPacket{
static constexpr uint32_t ID = 2;
FVec2 pos;
void respond_data_packet_plus(){
DataPacketPlus dp;
dp.yaw = currentYaw;
dp.desiredYaw = desiredYaw;
dp.distance = distanceReading;
dp.position.x = odom.x;
dp.position.y = odom.y;
dp.targetPosition.x = desiredPos.x;
dp.targetPosition.y = desiredPos.y;
void handle(){
desiredPos.x = this->pos.x;
desiredPos.y = this->pos.y;
Udp.write((const char*)&dp, sizeof(dp));
}
Udp.write((const char*)&ID, sizeof(ID));
}
};
struct EverythingPacket{
static constexpr uint32_t ID = 3;
void handle(){
Udp.write((const char*)&ID, sizeof(ID));
void respond_everything_packet(){
float everything[] = {
(float)dbgState.motorTargetAngle,
(float)distanceReading,
@ -250,23 +88,39 @@ void respond_everything_packet(){
};
Udp.write((const char*)everything, sizeof(everything));
}
}
};
struct GetDataPlusPacket{
static constexpr uint32_t ID = 4;
void set_pid(SetPIDPacket spid){
if(spid.index>PID_ARR_COUNT){
void handle(){
Udp.write((const char*)&ID, sizeof(ID));
DataPlusPacketResponse dp;
dp.yaw = currentYaw;
dp.desiredYaw = desiredYaw;
dp.distance = distanceReading;
dp.position.x = odom.x;
dp.position.y = odom.y;
dp.targetPosition.x = desiredPos.x;
dp.targetPosition.y = desiredPos.y;
Udp.write((const char*)&dp, sizeof(dp));
}
};
struct GetPIDPacket{
static constexpr uint32_t ID = 5;
uint32_t index;
void handle(){
if(this->index>PID_ARR_COUNT){
uint32_t e = -1;
Udp.write((const char*)&e, sizeof(e));
return;
}
PID& pid = *pids[spid.index];
pid.SetTunings(spid.kp, spid.ki, spid.kd);
pid.SetControllerDirection(spid.direction);
}
void get_pid(GetPIDPacket gpid){
if(gpid.index>PID_ARR_COUNT){
return;
}
PID& pid = *pids[gpid.index];
Udp.write((const char*)&ID, sizeof(ID));
PID& pid = *pids[this->index];
struct {float kp,ki,kd; uint32_t direction;} pidData = {
.kp=pid.GetKp(),
.ki=pid.GetKi(),
@ -275,7 +129,41 @@ void get_pid(GetPIDPacket gpid){
};
Udp.write((const char*)&pidData, sizeof(pidData));
}
}
};
struct SetPIDPacket{
static constexpr uint32_t ID = 6;
uint32_t index;
float kp,ki,kd;
uint32_t direction;
void handle(){
if(this->index>PID_ARR_COUNT){
uint32_t e = -1;
Udp.write((const char*)&e, sizeof(e));
return;
}
Udp.write((const char*)&ID, sizeof(ID));
PID& pid = *pids[this->index];
pid.SetTunings(this->kp, this->ki, this->kd);
pid.SetControllerDirection(this->direction);
}
};
struct Packet{
uint32_t sequence;
uint32_t id;
union{
ZeroPacket zero;
GetDataPacket get_data;
SetTargetPacket set_target;
EverythingPacket everything;
GetDataPlusPacket get_data_plus;
GetPIDPacket get_pid_packet;
SetPIDPacket set_pid_packet;
} kind;
};
bool handleUDP(){
int size = Udp.parsePacket();
@ -290,33 +178,15 @@ bool handleUDP(){
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write((const char*)&packet->sequence, sizeof(packet->sequence));
Udp.write((const char*)&packet->id, sizeof(packet->id));
switch(packet->id){
case ZeroPacket::ID:
zeroOdom();
desiredPos.x = 0;
desiredPos.y = 0;
break;
case GetDataPacket::ID:
respond_data_packet();
break;
case SetTargetPacket::ID:
desiredPos.x = packet->data.set_target.pos.x;
desiredPos.y = packet->data.set_target.pos.y;
break;
case EverythingPacket::ID:
respond_everything_packet();
break;
case GetDataPacketPlus::ID:
respond_data_packet_plus();
break;
case SetPIDPacket::ID:
set_pid(packet->data.set_pid_packet);
break;
case GetPIDPacket::ID:
get_pid(packet->data.get_pid_packet);
break;
case ZeroPacket::ID: packet->kind.zero.handle();break;
case GetDataPacket::ID: packet->kind.get_data.handle();break;
case SetTargetPacket::ID: packet->kind.set_target.handle();break;
case EverythingPacket::ID: packet->kind.everything.handle();break;
case GetDataPlusPacket::ID: packet->kind.get_data_plus.handle();break;
case SetPIDPacket::ID: packet->kind.set_pid_packet.handle();break;
case GetPIDPacket::ID: packet->kind.get_pid_packet.handle();break;
}
Udp.endPacket();
return true;