diff --git a/control/.gitignore b/control/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/control/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/control/.vscode/extensions.json b/control/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/control/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/control/.vscode/settings.json b/control/.vscode/settings.json new file mode 100644 index 0000000..3f00f17 --- /dev/null +++ b/control/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "cmake.configureOnOpen": true +} \ No newline at end of file diff --git a/control/include/README b/control/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/control/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/control/lib/README b/control/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/control/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/control/platformio.ini b/control/platformio.ini new file mode 100644 index 0000000..e9aec46 --- /dev/null +++ b/control/platformio.ini @@ -0,0 +1,18 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32-c3-devkitc-02] +platform = espressif32 +board = esp32dev +framework = arduino +lib_deps = + khoih-prog/TimerInterrupt_Generic@^1.13.0 + adafruit/Adafruit MPU6050@^2.2.4 +monitor_speed = 115200 diff --git a/control/src/main.cpp b/control/src/main.cpp new file mode 100644 index 0000000..5af5e86 --- /dev/null +++ b/control/src/main.cpp @@ -0,0 +1,159 @@ +#include +#include +#include +#include +#include +#include + +// The Stepper pins +#define STEPPER1_DIR_PIN 16 //Arduino D9 +#define STEPPER1_STEP_PIN 17 //Arduino D8 +#define STEPPER2_DIR_PIN 4 //Arduino D11 +#define STEPPER2_STEP_PIN 14 //Arduino D10 +#define STEPPER_EN 15 //Arduino D12 + +// Diagnostic pin for oscilloscope +#define TOGGLE_PIN 32 //Arduino A4 + +const int PRINT_INTERVAL = 50; +const double LOOP_INTERVAL = 5; +const int STEPPER_INTERVAL_US = 10; + +//Global objects +ESP32Timer ITimer(3); +Adafruit_MPU6050 mpu; //Default pins for I2C are SCL: IO22/Arduino D3, SDA: IO21/Arduino D4 + +step step1(STEPPER_INTERVAL_US,STEPPER1_STEP_PIN,STEPPER1_DIR_PIN ); +step step2(STEPPER_INTERVAL_US,STEPPER2_STEP_PIN,STEPPER2_DIR_PIN ); + + +//Interrupt Service Routine for motor update +//Note: ESP32 doesn't support floating point calculations in an ISR +bool TimerHandler(void * timerNo) +{ + static bool toggle = false; + + //Update the stepper motors + step1.runStepper(); + step2.runStepper(); + + //Indicate that the ISR is running + digitalWrite(TOGGLE_PIN,toggle); + toggle = !toggle; + return true; +} + +void setup() +{ + Serial.begin(115200); + pinMode(TOGGLE_PIN,OUTPUT); + + // Try to initialize Accelerometer/Gyroscope + if (!mpu.begin()) { + Serial.println("Failed to find MPU6050 chip"); + while (1) { + delay(10); + } + } + Serial.println("MPU6050 Found!"); + + mpu.setAccelerometerRange(MPU6050_RANGE_2_G); + mpu.setGyroRange(MPU6050_RANGE_250_DEG); + mpu.setFilterBandwidth(MPU6050_BAND_44_HZ); + + //Attach motor update ISR to timer to run every STEPPER_INTERVAL_US μs + if (!ITimer.attachInterruptInterval(STEPPER_INTERVAL_US, TimerHandler)) { + Serial.println("Failed to start stepper interrupt"); + while (1) delay(10); + } + Serial.println("Initialised Interrupt for Stepper"); + + //Set motor acceleration values + step1.setAccelerationRad(30.0); + step2.setAccelerationRad(30.0); + + //Enable the stepper motor drivers + pinMode(STEPPER_EN,OUTPUT); + digitalWrite(STEPPER_EN, false); + +} + +void loop() +{ + //Static variables are initialised once and then the value is remembered betweeen subsequent calls to this function + static unsigned long printTimer = 0; //time of the next print + static unsigned long loopTimer = 0; //time of the next control update + static double tiltx = 0.0; + static double gyrox = 0.0; //current tilt angle + static double gyroangle = 0.0; + static double theta_n = 0.0; + + + float kp = 2; + float ki = 0.002; //potentially don't need if we get the setpoint right (correct offset calibration) + float kd = 1; + float setpoint = -0.51; + double error, previous_error = 0, integral = 0, derivative = 0, previous_derivative = 0, Pout, Iout, Dout, motor_out; + + //Run the control loop every LOOP_INTERVAL ms + if (millis() > loopTimer) { + loopTimer += LOOP_INTERVAL; + + // Fetch data from MPU6050 + sensors_event_t a, g, temp; + mpu.getEvent(&a, &g, &temp); + + //Calculate Tilt using accelerometer and sin x = x approximation for a small tilt angle + tiltx = asin(a.acceleration.z/9.81)-0.05; + + gyrox = g.gyro.y - 0.04; + gyroangle = gyroangle + (gyrox*(LOOP_INTERVAL)); + + theta_n = 0.04*(tiltx*100) + 0.96*((gyrox*LOOP_INTERVAL)/10 + theta_n); + + //PIDeez Nuts + + + error = setpoint - theta_n; + + Pout = kp*error; + + integral = integral + error*(LOOP_INTERVAL/1000); + Iout = ki*integral; + + derivative = ((error - previous_error)/LOOP_INTERVAL)*1000; + derivative = previous_derivative + 0.2*(derivative-previous_derivative); + previous_derivative = derivative; + + Dout = kd*derivative; + + motor_out = Pout + Iout + Dout; + + previous_error = error; + + //Set target motor speed + + step1.setTargetSpeedRad(motor_out); + step2.setTargetSpeedRad(-motor_out); + } + + //Print updates every PRINT_INTERVAL ms + // if (millis() > printTimer) { + // printTimer += PRINT_INTERVAL; + + // Serial.print(error); + // Serial.print(' '); + // Serial.print(motor_out); + // Serial.println(); + + // // Serial.print(tiltx*100); + // // Serial.print(' '); + // // Serial.print(step1.getSpeedRad()); + // // Serial.print(' '); + // // Serial.print(gyroangle/10); + // // Serial.print(' '); + // // Serial.print(theta_n); + // // Serial.println(); + // } + +} \ No newline at end of file diff --git a/control/src/step.h b/control/src/step.h new file mode 100644 index 0000000..a75c2a3 --- /dev/null +++ b/control/src/step.h @@ -0,0 +1,156 @@ +#include + +class step { + +public: + + const int MAX_SPEED = 10000; //Maximum motor speed (steps/s) + const int MAX_SPEED_INTERVAL_US = 1000; //Maximum interval between speed updates (μs) + const int SPEED_SCALE = 2000; //Integer speed units are in steps per SPEED_SCALE seconds + const int MICROSTEPS = 16; //Number of microsteps per physical step + const int STEPS = 200; //Number of physical steps per revolution + const float STEP_ANGLE = (2.0 * PI)/(STEPS * MICROSTEPS); //Angle per microstep (rad) + int32_t accel = 0; //current acceleration (steps/s) + int32_t tSpeed = 0; //current speed (steps/(SPEED_SCALE * s)) + + //Initialise the stepper with interval and pin numbers + step(int i, int8_t sp, int8_t dp) : interval(i), stepPin(sp), dirPin(dp) { + pinMode(stepPin, OUTPUT); + pinMode(dirPin, OUTPUT); + } + + //Update the stepper motor, performing a step and updating the speed as necessary. Call every interval μs + void runStepper(){ + //Note: ESP32 doesn't support floating point calculations in an ISR, so this function only uses integer operations + + //Increment speed calculation interval timer + speedTimer += interval; + + //Check for stepping active + if (step_period != 0) { + + //Increment step timer + stepTimer += interval; + + //Check for step period elapsed + if (stepTimer > step_period) { + + //Start pulse + digitalWrite(stepPin, HIGH); + + //Roll back step timer by one period + stepTimer -= step_period; + + //Recaculate step interval + updateSpeed(); + + //Set step direction for next step + digitalWrite(dirPin, speed > 0); + + //Increment/decrement position counter + position += (speed > 0) ? 1 : -1; + + //End pulse + digitalWrite(stepPin, LOW); + } + + } else { + //Do nothing if stepping inactive + stepTimer = 0; + } + + //Recalculate step interval if the last update was longer ago than MAX_SPEED_INTERVAL_US + if (speedTimer > MAX_SPEED_INTERVAL_US) + updateSpeed(); + + } + + //Set acceleration in rad/s/s. Do not call from ISR + void setAccelerationRad(float accelRad){ + accel = static_cast(accelRad / STEP_ANGLE); + } + + //Set acceleration in microsteps/s/s + void setAcceleration(int newAccel){ + accel = newAccel; + } + + //Set target speed in rad/s. Do not call from ISR + void setTargetSpeedRad(float speedRad){ + tSpeed = static_cast(speedRad * SPEED_SCALE / STEP_ANGLE); + } + + // Set target speed in microsteps/(SPEED_SCALE * s) + void setTargetSpeed(int speed){ + tSpeed = speed; + } + + // Get position in microsteps + int getPosition() { + return position; + } + + //Get position in rads. Do not call from ISR + float getPositionRad() { + return static_cast(position) * STEP_ANGLE; + } + + //Get current speed in microsteps/(SPEED_SCALE * s) + float getSpeed() { + return speed; + } + + //Get current speed in rad/s. Do not call from ISR + float getSpeedRad() { + return static_cast(speed) * STEP_ANGLE / SPEED_SCALE; + } + + private: + + int32_t stepTimer = 0; //time since last step (μs) + int32_t speedTimer = 0; //time since last speed update (μs) + int32_t step_period = 0; //current time between steps (μs) + int32_t position = 0; //current accumulated steps (steps) + int8_t stepPin; //output pin number for step + int8_t dirPin; //output pin number for direction + int32_t speed = 0; //current steps per SPEED_SCALE seconds (steps) + int32_t interval; //interval between calls to runStepper (μs) + + //Update the motor speed and step interval + void updateSpeed(){ + + if (accel < 0) accel = -accel; + + //Calculate change to speed + if (speed < tSpeed){ + speed += accel * speedTimer / (1000000/SPEED_SCALE); + if (speed > tSpeed){ + speed = tSpeed; + } + if (speed > MAX_SPEED * SPEED_SCALE) { + speed = MAX_SPEED * SPEED_SCALE; + } + } + else { + speed -= accel * speedTimer / (1000000/SPEED_SCALE); + if (speed < tSpeed){ + speed = tSpeed; + } + if (speed < -MAX_SPEED * SPEED_SCALE) { + speed = -MAX_SPEED * SPEED_SCALE; + } + } + + //Reset speed calculation timer + speedTimer = 0; + + //Calculate step period + if (speed == 0) + step_period = 0; + else if (speed > 0) + step_period = 1000000 * SPEED_SCALE / speed; + else + step_period = -1000000 * SPEED_SCALE / speed; + } + +}; \ No newline at end of file diff --git a/control/test/README b/control/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/control/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html