diff --git a/doc/doc.pdf b/doc/doc.pdf new file mode 100644 index 0000000..8dfc8a5 Binary files /dev/null and b/doc/doc.pdf differ diff --git a/robot/distance.h b/robot/distance.cpp similarity index 79% rename from robot/distance.h rename to robot/distance.cpp index f90ca28..0267705 100644 --- a/robot/distance.h +++ b/robot/distance.cpp @@ -1,10 +1,12 @@ +#include "headers.h" #include "Adafruit_VL53L0X.h" + Adafruit_VL53L0X lox = Adafruit_VL53L0X(); float distanceReading = 0; void initDistance(){ - if (!lox.begin()) { + if (!lox.begin(0x29, false, &Wire1, Adafruit_VL53L0X::VL53L0X_SENSE_LONG_RANGE)) { Serial.println(F("Failed to boot VL53L0X")); while(1); } diff --git a/robot/encoder.cpp b/robot/encoder.cpp new file mode 100644 index 0000000..a40afb6 --- /dev/null +++ b/robot/encoder.cpp @@ -0,0 +1,82 @@ +#include "headers.h" +#include "AS5600.h" + +AS5600 as5600_0(&Wire1); +AS5600 as5600_1(&Wire2); + +EncoderOdom odom; + +void zeroOdom(){ + + odom.left = 0.0; + odom.right = 0.0; + odom.x = 0.0; + odom.y = 0.0; + odom.angle = 0.0; + + wire2(); + as5600_0.resetCumulativePosition(0); + wire1(); + as5600_1.resetCumulativePosition(0); +} + + +void initEncoder(){ + odom.left = 0.0; + odom.right = 0.0; + odom.x = 0.0; + odom.y = 0.0; + odom.angle = 0.0; + + wire2(); + as5600_0.begin(); + Serial.print("Connect device 0: "); + Serial.println(as5600_0.isConnected() ? "true" : "false"); + as5600_0.resetCumulativePosition(0); + + wire1(); + as5600_1.begin(); + Serial.print("Connect device 1: "); + Serial.println(as5600_1.isConnected() ? "true" : "false"); + as5600_1.resetCumulativePosition(0); +} + +void updateEncoder(){ + #define WHEEL_CIRCUM (2*PI*4.0625) + #define WHEEL_DISTANCE (7 + 12.0/16.0) + + wire2(); + int rawL = as5600_0.getCumulativePosition(); + wire1(); + int rawR = as5600_1.getCumulativePosition(); + + float left = rawL/4096.0 * -WHEEL_CIRCUM; + float right = rawR/4096.0 * WHEEL_CIRCUM; + + float d_left = left-odom.left; + float d_right= right-odom.right; + + odom.left = left; + odom.right = right; + + float displacement = (d_left + d_right)/2; + float oldAng = odom.angle; + odom.angle += (d_left-d_right)/(2*WHEEL_DISTANCE); + float ang = (odom.angle+oldAng)/2; + + float dispx = odom.x-desiredPos.x; + float dispy = odom.y-desiredPos.y; + posInput = sqrt(dispx*dispx+dispy*dispy); + + desiredYaw = atan2(dispy, dispx)*180/PI; + if(abs(fmod(desiredYaw-currentYaw, 180.0))<90){ + desiredYaw = fmod(desiredYaw+180.0, 360.0); + if(desiredYaw>180)desiredYaw-=180.0; + + posInput = -posInput; + } +// Serial.println(desiredYaw); + + odom.x += (float)(cos(ang)*displacement); + odom.y += (float)(sin(ang)*displacement); +} diff --git a/robot/encoder.h b/robot/encoder.h deleted file mode 100644 index 139fccc..0000000 --- a/robot/encoder.h +++ /dev/null @@ -1,50 +0,0 @@ - - -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.cpp similarity index 91% rename from robot/gyro.h rename to robot/gyro.cpp index 3f35628..f7999ee 100644 --- a/robot/gyro.h +++ b/robot/gyro.cpp @@ -1,12 +1,8 @@ +#include "headers.h" #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; +MPU6050 mpu(MPU6050_ADDRESS_AD0_LOW, &Wire1); // MPU control/status vars bool dmpReady = false; // set true if DMP init was successful @@ -54,6 +50,8 @@ void initGyro(){ Serial.println(F("Initializing DMP...")); devStatus = mpu.dmpInitialize(); +// mpu.setRate(0); + // supply your own gyro offsets here, scaled for min sensitivity mpu.setXAccelOffset(-6009); @@ -102,17 +100,15 @@ void initGyro(){ 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.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); +// mpu.dmpGetEuler(euler, &q); +// mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); +// mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); return true; } diff --git a/robot/headers.h b/robot/headers.h new file mode 100644 index 0000000..3dd8ba2 --- /dev/null +++ b/robot/headers.h @@ -0,0 +1,114 @@ + +#define LOOP_INTERVAL_MS 20 + +struct FVec2{ + float x,y; +}; +struct FVec3{ + float x,y,z; +}; +struct FVec4{ + float x,y,z; +}; + +extern FVec2 desiredPos; + +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; +}; + +extern float angleOffset; +extern float desiredYaw; +extern float currentYaw; + +struct DebugState{ + int motorTargetAngle; +}; + +extern DebugState dbgState; + +//-------- wire +#include +#define Wire1 Wire +#define Wire2 Wire + +void wire1(); +void wire2(); + +//-------- gyro +#include "MPU6050_6Axis_MotionApps612.h" + +extern Quaternion q; // [w, x, y, z] quaternion container +extern VectorInt16 aa; // [x, y, z] accel sensor measurements +extern VectorInt16 gy; // [x, y, z] gyro sensor measurements +extern VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +extern VectorInt16 aaRealLast; // [x, y, z] gravity-free accel sensor measurements +extern VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +extern VectorFloat gravity; // [x, y, z] gravity vector +extern float euler[3]; // [psi, theta, phi] Euler angle container +extern float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + +void initGyro(); +bool updateGyro(); + +//-------- distance +extern float distanceReading; + +void initDistance(); +void updateDistance(); + +//-------- encoder + +struct EncoderOdom{ + float x; + float y; + float angle; + + float left; + float right; +}; + + +extern EncoderOdom odom; + +void zeroOdom(); +void initEncoder(); +void updateEncoder(); + +//--------- pid +#include +#define PID_ARR_COUNT 3 +extern PID* pids[]; + +extern double angleInput, angleOutput, angleSetpoint; +extern PID anglePID; + +extern double posInput, posOutput, posSetpoint; +extern PID posPID; + +extern double turnInput, turnOutput, turnSetpoint; +extern PID turnPID; + +struct Speeds{ + float left; + float right; +}; + +Speeds updatePID(); +void initPID(); + +//--------- webserver + +void initWifi(bool host); +void initServer(); diff --git a/robot/pid.h b/robot/pid.cpp similarity index 89% rename from robot/pid.h rename to robot/pid.cpp index c7bddb1..7f9791e 100644 --- a/robot/pid.h +++ b/robot/pid.cpp @@ -1,3 +1,4 @@ +#include "headers.h" #include //double angKp=3.5, angKi=80, angKd=0.042; @@ -17,9 +18,9 @@ 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_E, REVERSE); +PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_E, DIRECT); -PID* pids[] = {&anglePID, &posPID, &turnPID}; +PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID}; void initPID(){ angleSetpoint = 0; @@ -39,11 +40,7 @@ void initPID(){ turnPID.SetSampleTime(5); } -struct Speeds{ - float left; - float right; -}; - + Speeds updatePID(){ posPID.Compute(); angleSetpoint = posOutput; @@ -51,8 +48,8 @@ Speeds updatePID(){ float maxTurn = max(0.0f, 15.0f-abs((float)angleOutput)); turnPID.SetOutputLimits(-maxTurn, maxTurn); - turnSetpoint = desiredYaw; - turnInput = currentYaw; + turnSetpoint = 0; + turnInput = fmod(currentYaw-desiredYaw, 180); turnPID.Compute(); Speeds speeds; diff --git a/robot/robot.ino b/robot/robot.ino index 51850a6..f4feb7d 100644 --- a/robot/robot.ino +++ b/robot/robot.ino @@ -1,5 +1,16 @@ -#include "I2Cdev.h" + +#include "headers.h" + +void wire1(){ + Wire1.begin(SDA, SCL); + Wire1.setClock(400000); +} + +void wire2(){ + Wire1.begin(D7, D5); + Wire2.setClock(400000); +} #include @@ -10,19 +21,10 @@ float angleOffset = -2.4241745; float desiredYaw = 0.0; float currentYaw = 0.0; -struct DebugState{ - int motorTargetAngle; -}; +FVec2 desiredPos; DebugState dbgState; -#include "distance.h" -#include "encoder.h" -#include "gyro.h" -#include "pid.h" -#include "webserver.h" - - void initSerial(){ Serial.begin(115200); while (!Serial); @@ -36,11 +38,10 @@ void initMotors(){ } void initI2C(){ - digitalWrite(D2, LOW); - digitalWrite(D1, LOW); - delay(100); - Wire.begin(); - Wire.setClock(400000); + delay(100); + + wire2(); + wire1(); } void setup() { @@ -50,23 +51,24 @@ void setup() { initMotors(); initPID(); initI2C(); - initEncoder(); initDistance(); - initGyro(); + initGyro(); + initEncoder(); } void loop() { + long start = millis(); + 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(); - } + + updateEncoder(); + currentYaw=odom.angle*180/M_PI; + updateDistance(); @@ -76,6 +78,13 @@ void loop() { speeds.right = min(90.0f-10, max(-90.0f+10, speeds.right)); left.write(90+(int)speeds.left); right.write(90+(int)speeds.right); + + long end = millis(); - delay(5); + if(end-start>LOOP_INTERVAL_MS){ + Serial.print("Overran "); + Serial.println(end-start); + }else{ + delay(LOOP_INTERVAL_MS-(end-start)); + } } diff --git a/robot/webserver.h b/robot/webserver.cpp similarity index 82% rename from robot/webserver.h rename to robot/webserver.cpp index edc0fe7..31fe4c6 100644 --- a/robot/webserver.h +++ b/robot/webserver.cpp @@ -1,3 +1,5 @@ +#include "headers.h" + #include #include #include @@ -31,48 +33,31 @@ void initWifi(bool host){ 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){ + 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[] = { - 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 + odom.angle, + distanceReading, + odom.x, + odom.y }; - request->send(200, "application/text", (const uint8_t*)arr, sizeof(arr)*4); + 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]; @@ -84,6 +69,7 @@ void initServer(){ "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}, @@ -95,10 +81,11 @@ void initServer(){ })", (float)dbgState.motorTargetAngle, (float)distanceReading, - encoder.position(), + 0.0, //encoder.position(), angleSetpoint, angleInput, angleOutput, posSetpoint, posInput, posOutput, turnSetpoint, turnInput, turnOutput, + odom.left, odom.right, odom.x, odom.y, odom.angle, 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,