diff --git a/robot/distance.h b/robot/distance.h new file mode 100644 index 0000000..bbfd025 --- /dev/null +++ b/robot/distance.h @@ -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()); + } +} diff --git a/robot/encoder.h b/robot/encoder.h new file mode 100644 index 0000000..139fccc --- /dev/null +++ b/robot/encoder.h @@ -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; +} diff --git a/robot/gyro.h b/robot/gyro.h new file mode 100644 index 0000000..3f35628 --- /dev/null +++ b/robot/gyro.h @@ -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; +} diff --git a/robot/page_html.h b/robot/page_html.h new file mode 100644 index 0000000..3d483be --- /dev/null +++ b/robot/page_html.h @@ -0,0 +1,162 @@ + +const char index_html[] PROGMEM = R"rawliteral( + + + + + + PID Controller + + + + 
Loading...
+
+ + +

PID

+ + + + + + +
+ + + +
+ + + +
+ + + +)rawliteral"; diff --git a/robot/pid.h b/robot/pid.h new file mode 100644 index 0000000..2b026d0 --- /dev/null +++ b/robot/pid.h @@ -0,0 +1,66 @@ +#include + +//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; +} diff --git a/robot/robot.ino b/robot/robot.ino new file mode 100644 index 0000000..51850a6 --- /dev/null +++ b/robot/robot.ino @@ -0,0 +1,81 @@ + +#include "I2Cdev.h" + + +#include +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); +} diff --git a/robot/webserver.h b/robot/webserver.h new file mode 100644 index 0000000..c08a4c6 --- /dev/null +++ b/robot/webserver.h @@ -0,0 +1,157 @@ +#include +#include +#include +#include +#include +#include + +#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(); +}