diff --git a/examples/06_Drone/06_Drone.ino b/examples/06_Drone/06_Drone.ino new file mode 100644 index 0000000..62470c1 --- /dev/null +++ b/examples/06_Drone/06_Drone.ino @@ -0,0 +1,547 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ControlLoop.h" +#define MPU9250_ADDR 0x68 +MPU9250_WE mpu = MPU9250_WE(MPU9250_ADDR); + +// helper functions +int minMax(int value, int min_value, int max_value) { + if (value > max_value) { + value = max_value; + } else if (value < min_value) { + value = min_value; + } + return value; +} + +// esc declaration +Servo escFL,escFR,escBL,escBR; +const int escPinFL = 14, escPinFR = 12, escPinBL = 13, escPinBR = 15; + +// operational Params +Madgwick filter; +xyzFloat accAngle , gyrAngle; +xyzFloat actualOrientation; +xyzFloat angularVelocity; +xyzFloat pid; +unsigned long timerPrint , timerMpu , timerEsc, timerRadio; + +// radio input +float throttle,pitch,roll,yaw; +int mFL,mFR,mBL,mBR; +bool enableMotor=false; + +// editable params +// const int offsetFL = 180,offsetFR = 90 , offsetBL = 30,offsetBR = 30; +const int offsetFL = 200,offsetFR = 200 , offsetBL = 200,offsetBR = 200; +unsigned int MIN_THROTTLE=1100; +unsigned int MAX_THROTTLE=1800; +const int ESC_UPDATE_INTERVAL = 20000; // update every 20000us / 50hz +const int MPU_UPDATE_INTERVAL = 20; // update every 20us / 20Kz +const int RADIO_INTERVAL = 25000; // update every 25000us / 40Hz +const int PRINT_UPDATE_INTERVAL = 20000; // update every 200ms / 50Hz +const int RATE_UPDATE_INTERVAL = 200; // 8ms +const float GYRO_RANGE_DIVIDER = 131; //for gyro range 250 +const float PID_MIN = -50; +const float PID_MAX = 50; +const int OUTER_LOOP_FACTOR = 5; +xyzFloat kp = {0.5f, 0.5f, 0.0f}; +xyzFloat ki = {0.02f,0.02f, 0.0f}; +xyzFloat kd = {1.0f, 1.0f, 0.0f}; +xyzFloat outerKp = {0.1f, 0.1f, 0.0f}; +xyzFloat outerKi = {0.02f,0.02f, 0.0f}; +xyzFloat outerKd = {0.05f, 0.05f, 0.0f}; +xyzFloat deadBand = {2.0f,2.0f,2.0f}; +xyzFloat angleSetpoint = {0.0f,0.0f,0.0f}; + +WebSocketsServer webSocket = WebSocketsServer(81); + +// print output +bool toPrint = true; +bool toCalibrate = false; + +// Function to read xyzFloat from EEPROM at a given address +xyzFloat readXYZFromEEPROM(int address) { + xyzFloat data; + EEPROM.get(address, data); // Retrieve the data from EEPROM + return data; +} + +// Function to write xyzFloat to EEPROM at a given address +void writeXYZToEEPROM(int address, xyzFloat data) { + EEPROM.put(address, data); + EEPROM.commit(); // Make sure the data is written +} + +class PitchRateDataSource : public DataSource { +public: + virtual float get() { + // Return the rate data from IMU (e.g., angular velocity in degrees/second) + return angularVelocity.y; // Replace getRateX with the actual method to get the rate + } +}; + +class RollRateDataSource : public DataSource { +public: + virtual float get() { + // Return the rate data from IMU (e.g., angular velocity in degrees/second) + return angularVelocity.x; // Replace getRateX with the actual method to get the rate + } +}; + +class YawRateDataSource : public DataSource { +public: + virtual float get() { + // Return the rate data from IMU (e.g., angular velocity in degrees/second) + return angularVelocity.z; // Replace getRateX with the actual method to get the rate + } +}; + +class PitchAngleDataSource : public DataSource { +public: + virtual float get() { + // Return the rate data from IMU (e.g., angular velocity in degrees/second) + return actualOrientation.y; // Replace getRateX with the actual method to get the rate + } +}; + +class RollAngleDataSource : public DataSource { +public: + virtual float get() { + // Return the rate data from IMU (e.g., angular velocity in degrees/second) + return actualOrientation.x; // Replace getRateX with the actual method to get the rate + } +}; + +class YawAngleDataSource : public DataSource { +public: + virtual float get() { + // Return the rate data from IMU (e.g., angular velocity in degrees/second) + return actualOrientation.z; // Replace getRateX with the actual method to get the rate + } +}; +// Relay class for controlling actuators +class PitchPid : public RelayUpdate { +public: + virtual void update(float output) { + // Convert the PID output to a suitable format for controlling the motor (e.g., PWM) + pid.y = output; + } + void on() { + // Turn on the motor if necessary + return; + } + + void off() { + // Turn off the motor if necessary + return; + } +}; +class RollPid : public RelayUpdate { +public: + virtual void update(float output) { + // Convert the PID output to a suitable format for controlling the motor (e.g., PWM) + pid.x = output; + } + void on() { + // Turn on the motor if necessary + return; + } + + void off() { + // Turn off the motor if necessary + return; + } +}; +class YawPid : public RelayUpdate { +public: + virtual void update(float output) { + // Convert the PID output to a suitable format for controlling the motor (e.g., PWM) + pid.z = output; + } + void on() { + // Turn on the motor if necessary + return; + } + + void off() { + // Turn off the motor if necessary + return; + } +}; + +// Define the data sources +PitchRateDataSource yRateSource; +PitchAngleDataSource ySource; +PitchPid yPid; +// Define the data sources +RollRateDataSource xRateSource; +RollAngleDataSource xSource; +RollPid xPid; +// Define the data sources +YawRateDataSource zRateSource; +YawAngleDataSource zSource; +YawPid zPid; + +ControlLoop rollControlSystem(&xRateSource, &xSource, &xPid, angleSetpoint.x); +ControlLoop pitchControlSystem(&yRateSource, &ySource, &yPid, angleSetpoint.y); +ControlLoop yawControlSystem(&zRateSource, &zSource, &zPid, angleSetpoint.z); + +void setup() { + EEPROM.begin(2 * sizeof(kp)); + Serial.begin(115200); + Wire.begin(); + delay(1000); + if (!mpu.init()) { + // Serial.println("MPU connection failed."); + delay(500); + } + delay(1000); + if (toCalibrate){ + calibrateMpu(); + } else { + // Fetch the values from EEPROM into xyzFloat type + xyzFloat accelOffset = readXYZFromEEPROM(0); // Read accelOffset from address 0 + xyzFloat gyroOffset = readXYZFromEEPROM(sizeof(accelOffset)); // Read gyroOffset after accelOffset + Serial.print("gyro roll from EEPROM "); + Serial.println(gyroOffset.x); + Serial.print("gyro pitch from EEPROM "); + Serial.println(gyroOffset.y); + Serial.print("gyro yaw from EEPROM "); + Serial.println(gyroOffset.z); + + // Apply the offsets to the MPU + mpu.setAccOffsets(accelOffset); + mpu.setGyrOffsets(gyroOffset); + } + + tuneMpu(); + + // Setup ESCs + escFL.attach(escPinFL,1000,2000); + escFR.attach(escPinFR,1000,2000); + escBL.attach(escPinBL,1000,2000); + escBR.attach(escPinBR,1000,2000); + resetAllEsc(); + delay(300); + + filter.begin(MPU_UPDATE_INTERVAL*10); + timerPrint,timerMpu,timerEsc,timerRadio= micros(); + cascadePidSetup(); + throttle = MIN_THROTTLE; + apServer(); + // setupServer(htmlContent); + webSocket.begin(); + webSocket.onEvent(webSocketEvent); +} + +void loop() { + unsigned long looptTimer = micros(); + if (micros()-timerMpu > MPU_UPDATE_INTERVAL) { + getSenorValue(); + sensorFusion(); + updatePID(); + + + timerMpu = micros(); + } + + if (enableMotor){ + + if (micros()-timerEsc > ESC_UPDATE_INTERVAL) { + updateMotorSpeed(); + timerEsc = micros(); + } + + } + + + if ((toPrint) && (micros()-timerPrint > PRINT_UPDATE_INTERVAL)) { + // processingLog(); + serialPrint(); + timerPrint = micros(); + } + + if (micros()-timerRadio > RADIO_INTERVAL) { + // processingLog(); + telemetry(); + timerRadio = micros(); + } + webSocket.loop(); +} + +void apServer(){ + WiFi.disconnect(); + const char* ssid = "buidybee_drone"; + IPAddress local_IP(192, 168, 4, 1); + // We set a Gateway IP address + IPAddress gateway(192, 168, 4, 1); + IPAddress subnet(255, 255, 255, 0); + // Connecting WiFi + WiFi.softAPConfig(local_IP,gateway,subnet); + WiFi.mode(WIFI_AP); + WiFi.softAP(ssid); +} + +void tuneMpu(){ + mpu.enableGyrDLPF(); + mpu.setGyrDLPF(MPU9250_DLPF_6); + mpu.setSampleRateDivider(5); + mpu.setGyrRange(MPU9250_GYRO_RANGE_250); + mpu.setAccRange(MPU9250_ACC_RANGE_2G); + mpu.enableAccDLPF(true); + mpu.setAccDLPF(MPU9250_DLPF_6); +} + +void calibrateMpu(){ + Serial.println("Position your MPU9250 flat and don't move it - calibrating..."); + delay(1000); + mpu.autoOffsets(); + xyzFloat gyroOffset = mpu.getGyrOffsets(); + xyzFloat accelOffset = mpu.getAccOffsets(); + + Serial.print("gyro roll to EEPROM "); + Serial.println(gyroOffset.x); + Serial.print("gyro pitch to EEPROM "); + Serial.println(gyroOffset.y); + Serial.print("gyro yaw to EEPROM "); + Serial.println(gyroOffset.z); + + // Save the values in EEPROM + writeXYZToEEPROM(0, accelOffset); // Write accelOffset at EEPROM address 0 + writeXYZToEEPROM(sizeof(accelOffset), gyroOffset); // Write gyroOffset right after accelOffset + + Serial.println("Offsets written to EEPROM!"); +} + +void getSenorValue(){ + accAngle = mpu.getGValues(); + gyrAngle = mpu.getGyrValues(); + angularVelocity = (angularVelocity *0.7) + (mpu.getCorrectedGyrRawValues()/GYRO_RANGE_DIVIDER)*0.3; // filter with 0.7 - 0.3 split. +} + +void sensorFusion(){ + filter.updateIMU(gyrAngle.x, gyrAngle.y, gyrAngle.z, accAngle.x, accAngle.y, accAngle.z); + actualOrientation.x = (abs(actualOrientation.x-filter.getRoll()) > deadBand.x) ? filter.getRoll(): actualOrientation.x; + actualOrientation.y = (abs(actualOrientation.y-filter.getPitch()) > deadBand.y) ? filter.getPitch(): actualOrientation.y; + actualOrientation.z = (abs(actualOrientation.z-filter.getYaw()) > deadBand.z) ? filter.getYaw(): actualOrientation.z; + } + +void updatePID(){ + rollControlSystem.Compute(); + pitchControlSystem.Compute(); + yawControlSystem.Compute(); + } + +void updateMotorSpeed(){ + mFL = throttle + int(trunc(pid.x)) - int(trunc(pid.y)) + int(trunc(pid.z)); + mFR = throttle - int(trunc(pid.x)) - int(trunc(pid.y)) - int(trunc(pid.z)); + mBL = throttle + int(trunc(pid.x)) + int(trunc(pid.y)) - int(trunc(pid.z)); + mBR = throttle - int(trunc(pid.x)) + int(trunc(pid.y)) + int(trunc(pid.z)); + + + mFL = minMax(mFL,MIN_THROTTLE,MAX_THROTTLE) + offsetFL; + mFR = minMax(mFR,MIN_THROTTLE,MAX_THROTTLE) + offsetFR; + mBL = minMax(mBL,MIN_THROTTLE,MAX_THROTTLE) + offsetBL; + mBR = minMax(mBR,MIN_THROTTLE,MAX_THROTTLE) + offsetBR; + + escFL.write(mFL); + escFR.write(mFR); + escBL.write(mBL); + escBR.write(mBR); + } + +void resetOrientation(){ + angleSetpoint = angleSetpoint * 0; +} + +void webSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t length) { + switch (type) { + case WStype_DISCONNECTED: + enableMotor = false; + resetAllEsc(); // Stop motors when disconnected + break; + + case WStype_CONNECTED: { + IPAddress ip = webSocket.remoteIP(num); + } + break; + + case WStype_TEXT: { + String payloadStr = String((char*)payload); + timerRadio = micros(); + // Start ESC when the command is received + + // Serial.println(payloadStr); + + if (payloadStr == "startESC") { + enableMotor = true; // Enable motors + softStartEsc(); + break; + } + + // Stop ESC when the command is received + if (payloadStr == "stopESC") { + resetAllEsc(); + enableMotor = false; + break; + + } + + if (payloadStr == "REQUEST_PID") { + String response = "kpX:" + String(kp.x) + '|' + \ + "kpY:" + String(kp.y) + '|' + \ + "kpZ:" + String(kp.z) + '|' + \ + "kiX:" + String(ki.x) + '|' + \ + "kiY:" + String(ki.y) + '|' + \ + "kiZ:" + String(ki.z) + '|' + \ + "kdX:" + String(kd.x) + '|' + \ + "kdY:" + String(kd.y) + '|' + \ + "kdZ:" + String(kd.z); + // Serial.println(response); + // Send the current PID values to the client + webSocket.broadcastTXT(response); + break; // Exit early as we've handled the action + } + + StaticJsonDocument<200> doc; + DeserializationError error = deserializeJson(doc, payloadStr); + + + if (error) { + Serial.print(F("deserializeJson() failed: ")); + Serial.println(error.f_str()); + return; + } + + + + // Extract joystick data (throttle, yaw, pitch, roll) if present + if (doc.containsKey("throttle")) { + throttle = MIN_THROTTLE + int(doc["throttle"]); + // angleSetpoint.z = float(doc["yaw"]); + break;; + } + if (doc.containsKey("roll")) { + angleSetpoint.x = float(doc["roll"]); + angleSetpoint.y = float(doc["pitch"]); + break; + } + + if (doc.containsKey("kpX")) { + kp.x = float(doc["kpX"]); + kp.y = float(doc["kpY"]); + kp.z = float(doc["kpZ"]); + ki.x = float(doc["kiX"]); + ki.y = float(doc["kiY"]); + ki.z = float(doc["kiZ"]); + kd.x = float(doc["kdX"]); + kd.y = float(doc["kdY"]); + kd.z = float(doc["kdZ"]); + retunePid(); + break; + // Serial.println("Pids updated"); + } + + // Update radio timer + + break; + } + + default: + if ((timerRadio > 0) && (micros() - timerRadio > RADIO_INTERVAL * 1000)) { + resetOrientation(); + timerRadio = 0; + } + break; + } +} + + +void resetAllEsc(){ + escFL.write(1000); + escFR.write(1000); + escBL.write(1000); + escBR.write(1000); +} + +void softStartEsc(){ + throttle = MIN_THROTTLE; + escFL.write(MIN_THROTTLE + offsetFL); + escFR.write(MIN_THROTTLE + offsetFR); + escBL.write(MIN_THROTTLE + offsetBL); + escBR.write(MIN_THROTTLE + offsetBR); +} + +void serialPrint(){ + Serial.print("mFL:"); Serial.print(mFL); Serial.print(","); + Serial.print("mFR:"); Serial.print(mFR); Serial.print(","); + Serial.print("mBL:"); Serial.print(mBL); Serial.print(","); + Serial.print("mBR:"); Serial.print(mBR); Serial.print(","); + Serial.print("Roll:"); Serial.print(actualOrientation.x,2); Serial.print(","); + Serial.print("Pitch:"); Serial.print(actualOrientation.y,2); Serial.print(","); + Serial.print("Yaw:"); Serial.print(actualOrientation.z,2); Serial.print(","); + Serial.print("Roll:"); Serial.print(actualOrientation.x,2); Serial.print(","); + Serial.print("Pitch:"); Serial.print(actualOrientation.y,2); Serial.print(","); + Serial.print("Yaw:"); Serial.print(actualOrientation.z,2); Serial.print(","); + Serial.print("pidX:"); Serial.print(pid.x); Serial.print(","); + Serial.print("pidY:"); Serial.print(pid.y); Serial.print(","); + Serial.print("pidZ:"); Serial.print(pid.z); Serial.print(","); + Serial.print("rateRollX:"); Serial.print(angularVelocity.x); Serial.print(","); + Serial.print("ratePitchY:"); Serial.print(angularVelocity.y); Serial.print(","); + Serial.print("rateYawZ:"); Serial.println(angularVelocity.y); + } + +void processingLog(){ + Serial.print(actualOrientation.x,3); + Serial.print(" | "); + Serial.println(actualOrientation.y,3); + } + +void telemetry(){ + String telemetryTx = String(int(actualOrientation.y)) + "|" + String(int(actualOrientation.x)) + "|" + String(int(actualOrientation.z)); + webSocket.broadcastTXT(telemetryTx); +} + +void retunePid(){ + rollControlSystem.setTunings(ControlLoop::INNER,kp.x,ki.x,kd.x); + pitchControlSystem.setTunings(ControlLoop::INNER,kp.y,ki.y,kd.y); + yawControlSystem.setTunings(ControlLoop::INNER,kp.z,ki.z,kd.z); + rollControlSystem.setTunings(ControlLoop::OUTER,outerKp.x,outerKi.x,outerKd.x); + pitchControlSystem.setTunings(ControlLoop::OUTER,outerKp.y,outerKi.y,outerKd.y); + yawControlSystem.setTunings(ControlLoop::OUTER,outerKp.z,outerKi.z,outerKd.z); +} + +void cascadePidSetup(){ + rollControlSystem.setTunings(ControlLoop::INNER,kp.x,ki.x,kd.x); + rollControlSystem.setTunings(ControlLoop::OUTER,outerKp.x,outerKi.x,outerKd.x); + rollControlSystem.setOutputLimits(ControlLoop::INNER, PID_MIN, PID_MAX); + rollControlSystem.setOutputLimits(ControlLoop::OUTER, PID_MIN, PID_MAX); + rollControlSystem.setControlType(ControlLoop::CASCADE); + rollControlSystem.setSampleTime(RATE_UPDATE_INTERVAL); + rollControlSystem.setOuterSampleFactor(OUTER_LOOP_FACTOR); + rollControlSystem.setOn(); + pitchControlSystem.setTunings(ControlLoop::INNER,kp.y,ki.y,kd.y); + pitchControlSystem.setTunings(ControlLoop::OUTER,outerKp.y,outerKi.y,outerKd.y); + pitchControlSystem.setOutputLimits(ControlLoop::INNER, PID_MIN, PID_MAX); + pitchControlSystem.setOutputLimits(ControlLoop::OUTER, PID_MIN, PID_MAX); + pitchControlSystem.setControlType(ControlLoop::CASCADE); + pitchControlSystem.setSampleTime(RATE_UPDATE_INTERVAL); + pitchControlSystem.setOuterSampleFactor(OUTER_LOOP_FACTOR); + pitchControlSystem.setOn(); + yawControlSystem.setTunings(ControlLoop::INNER,kp.z,ki.z,kd.z); + yawControlSystem.setTunings(ControlLoop::OUTER,outerKp.z,outerKi.z,outerKd.z); + yawControlSystem.setOutputLimits(ControlLoop::INNER, PID_MIN, PID_MAX); + yawControlSystem.setOutputLimits(ControlLoop::OUTER, PID_MIN, PID_MAX); + yawControlSystem.setControlType(ControlLoop::CASCADE); + yawControlSystem.setSampleTime(RATE_UPDATE_INTERVAL); + yawControlSystem.setOuterSampleFactor(OUTER_LOOP_FACTOR); + yawControlSystem.setOn(); +} diff --git a/examples/06_Drone/ControlLoop.cpp b/examples/06_Drone/ControlLoop.cpp new file mode 100644 index 0000000..14de421 --- /dev/null +++ b/examples/06_Drone/ControlLoop.cpp @@ -0,0 +1,319 @@ +#include "ControlLoop.h" + +//#define _SHOW_COMPUTE_TRACE(...) Serial.printf(__VA_ARGS__ ) +#define _SHOW_COMPUTE_TRACE(...) + + + +ControlLoop::ControlLoop(DataSource* inDS, DataSource* otDS, RelayUpdate* r, float sp) : + outer(&outerIn, &outerOutInnerSet, &outerSet, 1.0, 0.0, 0.0, DIRECT), + inner(&innerIn, &innerOut, &outerOutInnerSet, 1.0, 0.0, 0.0, DIRECT) { + + _innerDataSource = inDS; + _outerDataSource = otDS; + _relay = r; + _setpoint = sp; + + _outMin = 0.0; + _outMax = 1.0; + + inner.SetOutputLimits(_outMin, _outMax); + outer.SetOutputLimits(0.0, 90.0); + + _isOn = false; + inner.SetMode(MANUAL); + outer.SetMode(MANUAL); + + inner.SetSampleTime(_sampleTimeMS); + outer.SetSampleTime(_sampleTimeMS*_sampleFactor); + + setControlType(STD); + updateInputs(); +} + + +PID* ControlLoop::getController(int c) { + if ( c == ControlLoop::OUTER ) { + return &outer; + } + return &inner; +} + +void ControlLoop::updateInputs() { + switch (this->_ControlState) { + case ControlLoop::CASCADE: + this->outerIn = _outerDataSource->get(); + // Handle case if CASCADE is set without a second data source. + if (_innerDataSource == NULL) { + this->innerIn = this->outerIn; + } else { + this->innerIn = _innerDataSource->get(); + } + break; + + case ControlLoop::STD: + case ControlLoop::ONOFF: + this->outerIn = _outerDataSource->get(); + this->innerIn = this->outerIn; + break; + + default: + Serial.print(F("ERROR: unknown control type ")); + Serial.println(this->_ControlState); + } +} + +bool ControlLoop::Compute() { + + bool updated = false; + + updateInputs(); + + _SHOW_COMPUTE_TRACE("Control: %i bb: %i sp: %f, %f %f ", this->_ControlState, _bangBangOn, _setpoint, _setpointLower, _setpointUpper); + + // Jump out as off. + if (! _isOn) { + _SHOW_COMPUTE_TRACE("not on, returning\n"); + return false; + } + + + if ( this->_ControlState == ControlLoop::ONOFF ) { + _SHOW_COMPUTE_TRACE("onoff "); + + if ( _bangBangOn ) { + _SHOW_COMPUTE_TRACE("bb "); + updated = true; + + if ( innerIn < _setpointLower ) { + _SHOW_COMPUTE_TRACE("max "); + innerOut = _outMax; + + } else if ( innerIn > _setpointUpper ) { + _SHOW_COMPUTE_TRACE("min "); + innerOut = _outMin; + + } else { + _SHOW_COMPUTE_TRACE("n/a "); + + // No else to handle bewteen setpts + // will continue previous setting + // e.g. rising will continue + // failing will continue + ; + + } + + } else { + _SHOW_COMPUTE_TRACE("!bb "); + + updated = true; + if ( innerIn < _setpoint) { + _SHOW_COMPUTE_TRACE("max "); + innerOut = _outMax; + + } else if ( innerIn > _setpoint ) { + _SHOW_COMPUTE_TRACE("min "); + innerOut = _outMin; + + } else { + // State is equal, keep doing previous. + _SHOW_COMPUTE_TRACE("n/a "); + ; + } + + } + + + + } else if ( this->_ControlState == ControlLoop::STD ) { + _SHOW_COMPUTE_TRACE("std "); + + if ( _bangBangOn && innerIn < _setpointLower ) { + _SHOW_COMPUTE_TRACE("bb/low "); + updated = true; + innerOut = _outMax; + + } else if ( _bangBangOn && innerIn > _setpointUpper ) { + _SHOW_COMPUTE_TRACE("bb/high "); + updated = true; + innerOut = _outMin; + + } else { + _SHOW_COMPUTE_TRACE("compute "); + updated = inner.Compute(); + } + + } else if ( this->_ControlState == ControlLoop::CASCADE ) { + _SHOW_COMPUTE_TRACE("cas "); + if ( _bangBangOn && innerIn < _setpointLower ) { + _SHOW_COMPUTE_TRACE("bb/low "); + updated = true; + innerOut = _outMax; + + } else if ( _bangBangOn && innerIn > _setpointUpper ) { + _SHOW_COMPUTE_TRACE("bb/high "); + updated = true; + innerOut = _outMin; + + } else { + _SHOW_COMPUTE_TRACE("compute "); + bool o = outer.Compute(); + bool i = inner.Compute(); + updated = i || o; + } + + } else { + Serial.print(F("ERROR: unknown control type ")); + Serial.println(this->_ControlState); + updated = false; + + } + + if ( updated ) { + _relay->update(innerOut); + } + + _SHOW_COMPUTE_TRACE("upd rt: %i\n", updated); + return updated; +} + +void ControlLoop::setControlType(int ct) { + + this->_ControlState = ct; + + if ( this->_ControlState == ControlLoop::ONOFF ) { + inner.SetMode(MANUAL); + outer.SetMode(MANUAL); + + } else if ( this->_ControlState == ControlLoop::STD ) { + inner.SetMode(AUTOMATIC); + outer.SetMode(MANUAL); + outerOutInnerSet = _setpoint; + + } else if ( this->_ControlState == ControlLoop::CASCADE ) { + inner.SetMode(AUTOMATIC); + outer.SetMode(AUTOMATIC); + outerSet = _setpoint; + outerOutInnerSet = _setpoint; + + } else { + Serial.print(F("ERROR: unknown control type ")); + Serial.println(this->_ControlState); + } + +} + +void ControlLoop::setPoint(float sp) { + + _setpoint = sp; + _setpointLower = sp - _bangBangLower; + _setpointUpper = sp + _bangBangUpper; + setControlType(_ControlState); +} + + +float ControlLoop::getInnerSetPoint() { + if (this->_ControlState == CASCADE ) { + return outerOutInnerSet; + } + return -1.0; +} + +void ControlLoop::setBangBangRange(float lower, float upper) { + if ( lower > 0 && upper > 0 ) { + _bangBangLower = lower; + _bangBangUpper = upper; + setPoint(_setpoint); + } +} + + + +void ControlLoop::setSampleTime(int ms) { + _sampleTimeMS = ms; + inner.SetSampleTime(_sampleTimeMS); + outer.SetSampleTime(_sampleTimeMS*_sampleFactor); +} + +void ControlLoop::setOuterSampleFactor(int factor) { + _sampleFactor = factor; + outer.SetSampleTime(_sampleTimeMS*_sampleFactor); +} + + +void ControlLoop::setOnOff(bool turnOn) { + + inner.SetMode(MANUAL); + outer.SetMode(MANUAL); + _isOn = turnOn; + + if ( _isOn ) { + _relay->on(); + if ( this->_ControlState == ControlLoop::ONOFF ) { + ; + } else if ( this->_ControlState == ControlLoop::STD ) { + inner.SetMode(AUTOMATIC); + + } else if ( this->_ControlState == ControlLoop::CASCADE ) { + inner.SetMode(AUTOMATIC); + outer.SetMode(AUTOMATIC); + + } else { + Serial.print(F("ERROR: unknown control type ")); + Serial.println(this->_ControlState); + } + } else { + _relay->off(); + } + + +} + + +void ControlLoop::setOutputLimits(int c, float _min, float _max) { + PID* thePid = this->getController(c); + thePid->SetOutputLimits(_min, _max); + + if ( c == ControlLoop::INNER ) { + _outMin = _min; + _outMax = _max; + } + +} + + +void ControlLoop::setDirectionIncrease(int c, bool dir) { + PID *thePid = this->getController(c); + if ( dir ) { + thePid->SetControllerDirection(DIRECT); + } else { + thePid->SetControllerDirection(REVERSE); + } +} + +bool ControlLoop::getDirectionIncrease(int c) { + PID *thePid = this->getController(c); + return thePid->GetDirection() == DIRECT; +} + +void ControlLoop::setTunings(int c, float p, float i, float d) { + PID *thePid = this->getController(c); + thePid->SetTunings(p, i, d); +} + +float ControlLoop::getKp(int c) { + PID *thePid = this->getController(c); + return thePid->GetKp(); +} + +float ControlLoop::getKi(int c) { + PID *thePid = this->getController(c); + return thePid->GetKi(); +} + +float ControlLoop::getKd(int c) { + PID *thePid = this->getController(c); + return thePid->GetKd(); +} + diff --git a/examples/06_Drone/ControlLoop.h b/examples/06_Drone/ControlLoop.h new file mode 100644 index 0000000..4b436ad --- /dev/null +++ b/examples/06_Drone/ControlLoop.h @@ -0,0 +1,126 @@ + +#ifndef _CONTROLLOOP_H +#define _CONTROLLOOP_H + +#include +#include "PID_v1.h" + +class RelayUpdate { + public: + virtual void on() = 0; + virtual void off() = 0; + virtual void update(float) = 0; +}; + +class DataSource { + public: + virtual float get() = 0; +}; + + + +class ControlLoop { + + public: + ControlLoop(DataSource*, DataSource*, RelayUpdate*, float); + ControlLoop(DataSource* data, RelayUpdate* update, float setpoint) : ControlLoop(NULL, data, update, setpoint) {;}; + + + static const bool PID_DEBUG = true; + + static const int INNER = 0; + static const int OUTER = 1; + + static const int ONOFF = 10; + static const int STD = 11; + static const int CASCADE = 12; + + bool Compute(); + + void setPoint(float); + float getSetPoint() { return _setpoint; } + float getInnerSetPoint(); + + + + void enableBangBang() { _bangBangOn = true; } + void disableBangBang() { _bangBangOn = false; } + bool isBangBangOn() { return _bangBangOn; } + void setBangBangRange(float x) { setBangBangRange(-x, x); } + void setBangBangRange(float, float); + float getBangBangLower() { return _bangBangLower; } + float getBangBangUpper() { return _bangBangUpper;} + + void setControlType(int); + bool isControlOnOff() { return this->_ControlState == ControlLoop::ONOFF; } + bool isControlStandardPID() { return this->_ControlState == ControlLoop::STD; } + bool isControlCascadePID() { return this->_ControlState == ControlLoop::CASCADE; } + int getControlType(){ return this->_ControlState; } + + + void setSampleTime(int); + void setOuterSampleFactor(int); + + bool isOn() { return _isOn; } + void setOn() { setOnOff(true); } + void setOff() { setOnOff(false); } + + void setOutputLimits(int, float, float); + void setDirectionIncrease(int, bool); + bool getDirectionIncrease(int); + + void setTunings(float p, float i, float d) { setTunings(ControlLoop::INNER, p, i, d);} + void setTunings(int, float, float, float); + float getKp(int); + float getKi(int); + float getKd(int); + + + protected: + + void updateInputs(); + + void setOnOff(bool); + + private: + + float outerIn = 0.0; + float outerOutInnerSet = 0.0; + float outerSet = 0.0; + // Setpoint is shared with inner Out + + float innerIn = 0.0; + float innerOut = 0.0; + + float _outMin = 0.0; + float _outMax = 1.0; + + + PID outer; + PID inner; + + PID *getController(int c); + + long _sampleTimeMS = 2500; + long _sampleFactor = 4; // Recommended to be 3-5 times + + int _ControlState = STD; + + float _setpoint; + float _setpointLower; + float _setpointUpper; + + bool _isOn = false; + bool _bangBangOn = false; + float _bangBangLower = 0.0; + float _bangBangUpper = 0.0; + + + DataSource *_innerDataSource = NULL; + DataSource *_outerDataSource = NULL; + RelayUpdate *_relay = NULL; + +}; + +#endif + diff --git a/examples/06_Drone/PID_v1.cpp b/examples/06_Drone/PID_v1.cpp new file mode 100644 index 0000000..bcb03a7 --- /dev/null +++ b/examples/06_Drone/PID_v1.cpp @@ -0,0 +1,224 @@ +/********************************************************************************************** + * Arduino PID Library - Version 1.1.1 + * by Brett Beauregard brettbeauregard.com + * + * This Library is licensed under a GPLv3 License + **********************************************************************************************/ + +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#include "PID_v1.h" + +/*Constructor (...)********************************************************* + * The parameters specified here are those for for which we can't set up + * reliable defaults, so we need to have the user set them. + ***************************************************************************/ +PID::PID(float* Input, float* Output, float* Setpoint, + float Kp, float Ki, float Kd, int POn, int ControllerDirection) +{ + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + inAuto = false; + + PID::SetOutputLimits(0, 255); //default output limit corresponds to + //the arduino pwm limits + + SampleTime = 100; //default Controller Sample Time is 0.1 seconds + + PID::SetControllerDirection(ControllerDirection); + PID::SetTunings(Kp, Ki, Kd, POn); + + lastTime = millis()-SampleTime; +} + +/*Constructor (...)********************************************************* + * To allow backwards compatability for v1.1, or for people that just want + * to use Proportional on Error without explicitly saying so + ***************************************************************************/ + +PID::PID(float* Input, float* Output, float* Setpoint, + float Kp, float Ki, float Kd, int ControllerDirection) + :PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection) +{ + +} + + +/* Compute() ********************************************************************** + * This, as they say, is where the magic happens. this function should be called + * every time "void loop()" executes. the function will decide for itself whether a new + * pid Output needs to be computed. returns true when the output is computed, + * false when nothing has been done. + **********************************************************************************/ +bool PID::Compute() +{ + if(!inAuto) return false; + unsigned long now = millis(); + unsigned long timeChange = (now - lastTime); + if(timeChange>=SampleTime) + { + /*Compute all the working error variables*/ + float input = *myInput; + float error = *mySetpoint - input; + float dInput = (input - lastInput); + outputSum+= (ki * error); + + /*Add Proportional on Measurement, if P_ON_M is specified*/ + if(!pOnE) outputSum-= kp * dInput; + + if(outputSum > outMax) outputSum= outMax; + else if(outputSum < outMin) outputSum= outMin; + + /*Add Proportional on Error, if P_ON_E is specified*/ + float output; + if(pOnE) output = kp * error; + else output = 0; + + /*Compute Rest of PID Output*/ + output += outputSum - kd * dInput; + + if(output > outMax) output = outMax; + else if(output < outMin) output = outMin; + *myOutput = output; + + /*Remember some variables for next time*/ + lastInput = input; + lastTime = now; + return true; + } + else return false; +} + +/* SetTunings(...)************************************************************* + * This function allows the controller's dynamic performance to be adjusted. + * it's called automatically from the constructor, but tunings can also + * be adjusted on the fly during normal operation + ******************************************************************************/ +void PID::SetTunings(float Kp, float Ki, float Kd, int POn) +{ + if (Kp<0 || Ki<0 || Kd<0) return; + + pOn = POn; + pOnE = POn == P_ON_E; + + dispKp = Kp; dispKi = Ki; dispKd = Kd; + + float SampleTimeInSec = ((float)SampleTime)/1000; + kp = Kp; + ki = Ki * SampleTimeInSec; + kd = Kd / SampleTimeInSec; + + if(controllerDirection ==REVERSE) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } +} + +/* SetTunings(...)************************************************************* + * Set Tunings using the last-rembered POn setting + ******************************************************************************/ +void PID::SetTunings(float Kp, float Ki, float Kd){ + SetTunings(Kp, Ki, Kd, pOn); +} + +/* SetSampleTime(...) ********************************************************* + * sets the period, in Milliseconds, at which the calculation is performed + ******************************************************************************/ +void PID::SetSampleTime(int NewSampleTime) +{ + if (NewSampleTime > 0) + { + float ratio = (float)NewSampleTime + / (float)SampleTime; + ki *= ratio; + kd /= ratio; + SampleTime = (unsigned long)NewSampleTime; + } +} + +/* SetOutputLimits(...)**************************************************** + * This function will be used far more often than SetInputLimits. while + * the input to the controller will generally be in the 0-1023 range (which is + * the default already,) the output will be a little different. maybe they'll + * be doing a time window and will need 0-8000 or something. or maybe they'll + * want to clamp it from 0-125. who knows. at any rate, that can all be done + * here. + **************************************************************************/ +void PID::SetOutputLimits(float Min, float Max) +{ + if(Min >= Max) return; + outMin = Min; + outMax = Max; + + if(inAuto) + { + if(*myOutput > outMax) *myOutput = outMax; + else if(*myOutput < outMin) *myOutput = outMin; + + if(outputSum > outMax) outputSum= outMax; + else if(outputSum < outMin) outputSum= outMin; + } +} + +/* SetMode(...)**************************************************************** + * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) + * when the transition from manual to auto occurs, the controller is + * automatically initialized + ******************************************************************************/ +void PID::SetMode(int Mode) +{ + bool newAuto = (Mode == AUTOMATIC); + if(newAuto && !inAuto) + { /*we just went from manual to auto*/ + PID::Initialize(); + } + inAuto = newAuto; +} + +/* Initialize()**************************************************************** + * does all the things that need to happen to ensure a bumpless transfer + * from manual to automatic mode. + ******************************************************************************/ +void PID::Initialize() +{ + outputSum = *myOutput; + lastInput = *myInput; + if(outputSum > outMax) outputSum = outMax; + else if(outputSum < outMin) outputSum = outMin; +} + +/* SetControllerDirection(...)************************************************* + * The PID will either be connected to a DIRECT acting process (+Output leads + * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to + * know which one, because otherwise we may increase the output when we should + * be decreasing. This is called from the constructor. + ******************************************************************************/ +void PID::SetControllerDirection(int Direction) +{ + if(inAuto && Direction !=controllerDirection) + { + kp = (0 - kp); + ki = (0 - ki); + kd = (0 - kd); + } + controllerDirection = Direction; +} + +/* Status Funcions************************************************************* + * Just because you set the Kp=-1 doesn't mean it actually happened. these + * functions query the internal state of the PID. they're here for display + * purposes. this are the functions the PID Front-end uses for example + ******************************************************************************/ +float PID::GetKp(){ return dispKp; } +float PID::GetKi(){ return dispKi;} +float PID::GetKd(){ return dispKd;} +int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} +int PID::GetDirection(){ return controllerDirection;} + diff --git a/examples/06_Drone/PID_v1.h b/examples/06_Drone/PID_v1.h new file mode 100644 index 0000000..2f01b65 --- /dev/null +++ b/examples/06_Drone/PID_v1.h @@ -0,0 +1,90 @@ +#ifndef PID_v1_h +#define PID_v1_h +#define LIBRARY_VERSION 1.1.1 + +class PID +{ + + + public: + + //Constants used in some of the functions below + #define AUTOMATIC 1 + #define MANUAL 0 + #define DIRECT 0 + #define REVERSE 1 + #define P_ON_M 0 + #define P_ON_E 1 + + //commonly used functions ************************************************************************** + PID(float*, float*, float*, // * constructor. links the PID to the Input, Output, and + float, float, float, int, int);// Setpoint. Initial tuning parameters are also set here. + // (overload for specifying proportional mode) + + PID(float*, float*, float*, // * constructor. links the PID to the Input, Output, and + float, float, float, int); // Setpoint. Initial tuning parameters are also set here + + void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) + + bool Compute(); // * performs the PID calculation. it should be + // called every time loop() cycles. ON/OFF and + // calculation frequency can be set using SetMode + // SetSampleTime respectively + + void SetOutputLimits(float, float); // * clamps the output to a specific range. 0-255 by default, but + // it's likely the user will want to change this depending on + // the application + + + + //available but not commonly used functions ******************************************************** + void SetTunings(float, float, // * While most users will set the tunings once in the + float); // constructor, this function gives the user the option + // of changing tunings during runtime for Adaptive control + void SetTunings(float, float, // * overload for specifying proportional mode + float, int); + + void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT + // means the output will increase when error is positive. REVERSE + // means the opposite. it's very unlikely that this will be needed + // once it is set in the constructor. + void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which + // the PID calculation is performed. default is 100 + + + + //Display functions **************************************************************** + float GetKp(); // These functions query the pid for interal values. + float GetKi(); // they were created mainly for the pid front-end, + float GetKd(); // where it's important to know what is actually + int GetMode(); // inside the PID. + int GetDirection(); // + + private: + void Initialize(); + + float dispKp; // * we'll hold on to the tuning parameters in user-entered + float dispKi; // format for display purposes + float dispKd; // + + float kp; // * (P)roportional Tuning Parameter + float ki; // * (I)ntegral Tuning Parameter + float kd; // * (D)erivative Tuning Parameter + + int controllerDirection; + int pOn; + + float *myInput; // * Pointers to the Input, Output, and Setpoint variables + float *myOutput; // This creates a hard link between the variables and the + float *mySetpoint; // PID, freeing the user from having to constantly tell us + // what these values are. with pointers we'll just know. + + unsigned long lastTime; + float outputSum, lastInput; + + unsigned long SampleTime; + float outMin, outMax; + bool inAuto, pOnE; +}; +#endif +