added robot code
ParkerTenBroeck
parent
a8442a2ec0
commit
c31889efd9
|
|
@ -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());
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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";
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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();
|
||||
}
|
||||
Loading…
Reference in New Issue