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

added robot code

main
ParkerTenBroeck 2025-04-09 11:14:20 -04:00
parent a8442a2ec0
commit c31889efd9
7 changed files with 655 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
robot/distance.h Normal file
View File

@ -0,0 +1,19 @@
#include "Adafruit_VL53L0X.h"
Adafruit_VL53L0X lox = Adafruit_VL53L0X();
void initDistance(){
if (!lox.begin()) {
Serial.println(F("Failed to boot VL53L0X"));
while(1);
}
lox.startRangeContinuous();
}
void updateDistance(){
if (lox.isRangeComplete()) {
Serial.print("Distance in mm: ");
Serial.println(lox.readRange());
}
}

50
robot/encoder.h Normal file
View File

@ -0,0 +1,50 @@
struct Encoder{
int rotations;
int currentAngle;
int estAngle;
bool threashhold;
float position(){
return rotations + (estAngle / 360.0);
}
};
Encoder encoder;
void initEncoder(){
pinMode(A0, INPUT);
encoder.currentAngle = 0;
encoder.rotations = 0;
encoder.estAngle=0;
encoder.threashhold=false;
}
void updateEncoder(){
int rawIn = analogRead(A0);
// static long lastRan = 0;
// if(50>millis()-lastRan) return;
// lastRan = millis();
int rotation = map(rawIn, 4, 348, 0, 360);
int diff = rotation-encoder.currentAngle;
if(!encoder.threashhold && (abs(diff)>=50)){
encoder.threashhold=true;
if(diff<0){
encoder.rotations++;
encoder.estAngle = 0;
}
if(diff>0){
encoder.rotations--;
encoder.estAngle = 360;
}
}
if(abs(diff)<30){
encoder.threashhold=false;
encoder.estAngle = rotation;
}
encoder.currentAngle = rotation;
}

120
robot/gyro.h Normal file
View File

@ -0,0 +1,120 @@
#include "MPU6050_6Axis_MotionApps612.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaRealLast; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void ICACHE_RAM_ATTR dmpDataReady() {
mpuInterrupt = true;
}
#define INTERRUPT_PIN D7
void initGyro(){
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready
// Serial.println(F("\nSend any character to begin DMP programming and demo: "));
// while (Serial.available() && Serial.read()); // empty buffer
// while (!Serial.available()); // wait for data
// while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXAccelOffset(-6009);
mpu.setYAccelOffset(-5577);
mpu.setZAccelOffset(7961);
mpu.setXGyroOffset(98);
mpu.setYGyroOffset(85);
mpu.setZGyroOffset(81);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
// mpu.CalibrateAccel(6);
// mpu.CalibrateGyro(6);
Serial.println();
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
delay(5000);
}
bool updateGyro(){
if (!dmpReady) return false;
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
// Xf = 1/4*(aaRealLast.x + aaReal.x)t^2 + Vot + Xo
// aaRealLast = aaReal;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGyro(&gy, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu.dmpGetEuler(euler, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
return true;
}
return false;
}

162
robot/page_html.h Normal file
View File

@ -0,0 +1,162 @@
const char index_html[] PROGMEM = R"rawliteral(
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>PID Controller</title>
<style>
body {
margin: 0;
background-color: black;
color: greenyellow;
font-family: monospace;
display: flex;
height: 100vh;
overflow: hidden;
}
#jsonOutput {
flex: 1;
padding: 20px;
white-space: pre-wrap;
word-wrap: break-word;
overflow: auto;
}
#controls {
width: 50vw;
padding: 20px;
background: #222;
color: white;
display: flex;
flex-direction: column;
gap: 10px;
}
label, button {
font-size: 16px;
}
input {
width: 100%;
}
.slider-value {
display: inline-block;
width: 40px;
text-align: right;
}
</style>
</head>
<body>
<pre id="jsonOutput">Loading...</pre>
<div id="controls">
<button onclick="setHome()">Set Home</button>
<h3>PID</h3>
<label for="pidIndex">Index:</label>
<select id="pidIndex">
<option value="0">angle</option>
<option value="1">pos</option>
<option value="2">turn</option>
</select>
<button onclick="fetchPID()">Get PID</button>
<label>Kp: <input type="range" id="kp" min="0" max="20" step="0.0001" oninput="updateValue('kp_value', this.value)"><span id="kp_value" class="slider-value">0</span></label>
<label>Ki: <input type="range" id="ki" min="0" max="120" step="0.0001" oninput="updateValue('ki_value', this.value)"><span id="ki_value" class="slider-value">0</span></label>
<label>Kd: <input type="range" id="kd" min="0" max="0.3" step="0.0001" oninput="updateValue('kd_value', this.value)"><span id="kd_value" class="slider-value">0</span></label>
<div>
<label>Direction:</label>
<label><input type="radio" name="direction" value="0" checked> Direct</label>
<label><input type="radio" name="direction" value="1"> Reverse</label>
</div>
<button onclick="setPID()">Set PID</button>
<label>Desired Yaw: <input type="range" id="gyro_kp" min="-180" max="180" step="5" oninput="updateValue('desired_yaw', this.value);setDesiredYaw(this.value)"><span id="desired_yaw" class="slider-value">0</span></label>
</div>
<script>
async function fetchData(){
try{
const res = await fetch("get_stuff");
document.getElementById("jsonOutput").textContent = JSON.stringify(await res.json(), null, 2);
}catch(error){
document.getElementById("jsonOutput").textContent = "Error: " + error.message;
}
}
async function loopFetch(){
while(true){
await fetchData();
}
}
loopFetch();
document.getElementById("pidIndex").addEventListener("change", (event) => {
fetchPID();
});
async function fetchPID() {
const index = parseInt(document.getElementById("pidIndex").value);
const res = await fetch("/get_pid", {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify({index})
});
const data = await res.json();
document.getElementById("kp").value = data.kp;
document.getElementById("ki").value = data.ki;
document.getElementById("kd").value = data.kd;
updateValue('kp_value', data.kp);
updateValue('ki_value', data.ki);
updateValue('kd_value', data.kd);
document.querySelector(`input[name="direction"][value="${data.direction}"]`).checked = true;
}
window.onload = () => {
fetchPID();
}
async function setPID() {
const index = parseInt(document.getElementById("pidIndex").value);
const direction = document.querySelector('input[name="direction"]:checked').value;
const data = {
index,
kp: parseFloat(document.getElementById("kp").value),
ki: parseFloat(document.getElementById("ki").value),
kd: parseFloat(document.getElementById("kd").value),
direction: parseInt(direction)
};
await fetch("/set_pid", {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify(data)
});
}
function setDesiredYaw(yaw){
fetch("/set_desired_yaw", {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify({yaw})
});
}
function setGyroPID() {
const direction = document.querySelector('input[name="gyro_direction"]:checked').value;
const data = {
kp: parseFloat(document.getElementById("gyro_kp").value),
ki: parseFloat(document.getElementById("gyro_ki").value),
kd: parseFloat(document.getElementById("gyro_kd").value),
direction: parseInt(direction)
};
fetch("/set_gyro_pid", {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify(data)
});
}
function updateValue(id, value) {
document.getElementById(id).textContent = value;
}
</script>
</body>
</html>
)rawliteral";

66
robot/pid.h Normal file
View File

@ -0,0 +1,66 @@
#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.0, angKi=106.75, angKd=0.0472;
double posKp=0.96, posKi=1.28, posKd=0.5;
double turnKp=1, turnKi=5.0, turnKd=0.17;
double angleInput, angleOutput, angleSetpoint;
PID anglePID(&angleInput, &angleOutput, &angleSetpoint, angKp, angKi, angKd, AUTOMATIC);
double posInput, posOutput, posSetpoint;
PID posPID(&posInput, &posOutput, &posSetpoint, posKp, posKi, posKd, AUTOMATIC);
double turnInput, turnOutput, turnSetpoint;
PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, AUTOMATIC);
PID* pids[] = {&anglePID, &posPID, &turnPID};
void initPID(){
angleSetpoint = 0;
anglePID.SetOutputLimits(-180, 180);
anglePID.SetMode(AUTOMATIC);
anglePID.SetSampleTime(5);
posSetpoint = 0.5;
posPID.SetOutputLimits(-1, 1);
posPID.SetMode(AUTOMATIC);
posPID.SetSampleTime(5);
posPID.SetControllerDirection(DIRECT);
turnSetpoint = 0;
turnPID.SetOutputLimits(-15, 15);
turnPID.SetMode(AUTOMATIC);
turnPID.SetSampleTime(5);
}
struct Speeds{
float left;
float right;
};
Speeds updatePID(){
posPID.Compute();
angleSetpoint = posOutput;
anglePID.Compute();
float maxTurn = max(0.0f, 15.0f-abs((float)angleOutput));
turnPID.SetOutputLimits(-maxTurn, maxTurn);
turnSetpoint = desiredYaw;
turnInput = currentYaw;
turnPID.Compute();
Speeds speeds;
speeds.left = (float)(angleOutput + turnOutput);
speeds.right = (float)(-angleOutput + turnOutput);
if(angleInput>20 || angleInput<-20){
speeds.left = 0;
speeds.right = 0;
}
return speeds;
}

81
robot/robot.ino Normal file
View File

@ -0,0 +1,81 @@
#include "I2Cdev.h"
#include <Servo.h>
Servo left;
Servo right;
float angleOffset = -2.4241745;
float desiredYaw = 0.0;
float currentYaw = 0.0;
struct DebugState{
int motorTargetAngle;
};
DebugState dbgState;
#include "distance.h"
#include "encoder.h"
#include "gyro.h"
#include "pid.h"
#include "webserver.h"
void initSerial(){
Serial.begin(115200);
while (!Serial);
delay(300);
Serial.print("\n\n\n\n\n\n\n\n\n\n\n");
}
void initMotors(){
left.attach(D3);
right.attach(D4);
}
void initI2C(){
digitalWrite(D2, LOW);
digitalWrite(D1, LOW);
delay(100);
Wire.begin();
Wire.setClock(400000);
}
void setup() {
initSerial();
initWifi(false);
initServer();
initMotors();
initPID();
initI2C();
initEncoder();
initDistance();
initGyro();
}
void loop() {
if (updateGyro()) { //gyro data
currentYaw=ypr[0]*180/M_PI;
double angle = ypr[1] * 180 / M_PI;
if(angle>180)
angle -= 180;
angleInput = angle + angleOffset;
}
{// encoder data
updateEncoder();
posInput = encoder.position();
}
updateDistance();
Speeds speeds = updatePID();
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);
delay(5);
}

157
robot/webserver.h Normal file
View File

@ -0,0 +1,157 @@
#include <ESP8266WiFi.h>
#include <ESPAsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <ArduinoJson.h>
#include <AsyncJson.h>
#include <AsyncMessagePack.h>
#include "page_html.h"
AsyncWebServer server(80);
void initWifi(bool host){
if(host){
WiFi.softAP("MEOW");
IPAddress IP = WiFi.softAPIP();
Serial.print("AP IP address: ");
Serial.println(IP);
}else{
Serial.print("Connecting to ");
Serial.println("Michael Loves CP");
WiFi.enableInsecureWEP(false);
WiFi.mode(WIFI_STA);
WiFi.setPhyMode(WIFI_PHY_MODE_11G);
WiFi.begin("Michael Loves CP", "cockandpavly20");
while(WiFi.status() != WL_CONNECTED){
delay(500);
Serial.print('.');
}
Serial.println();
}
Serial.println(WiFi.localIP());
}
struct FVec3{
float x,y,z;
};
struct FVec4{
float x,y,z;
};
struct Everything{
float motorTargetAngle;
float position;
FVec3 anglePID;
FVec3 posPID;
FVec3 ypr;
FVec3 euler;
FVec3 gravity;
FVec4 q;
FVec3 aa;
FVec3 gy;
FVec3 aaReal;
FVec3 aaWorld;
};
void initServer(){
server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
request->send_P(200, "text/html", index_html);
});
server.on("/get_stuff_bin", HTTP_GET, [](AsyncWebServerRequest *request){
float arr[] = {
dbgState.motorTargetAngle,
encoder.position(),
angleSetpoint, angleInput, angleOutput,
posSetpoint, posInput, posOutput,
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/text", (const uint8_t*)arr, sizeof(arr)*4);
});
server.on("/get_stuff", HTTP_GET, [](AsyncWebServerRequest *request){
char buff[1024];
int ret = snprintf(buff, sizeof(buff),
R"({
"motorTargetAngle": %d,
"position": %f,
"anglePID": {"setpoint": %lf, "input": %lf, "output": %lf},
"posPID": {"setpoint": %lf, "input": %lf, "output": %lf},
"turnPID": {"setpoint": %lf, "input": %lf, "output": %lf},
"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}
})",
dbgState.motorTargetAngle,
encoder.position(),
angleSetpoint, angleInput, angleOutput,
posSetpoint, posInput, posOutput,
turnSetpoint, turnInput, turnOutput,
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_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();
}