diff --git a/.gitignore b/.gitignore index e740d28..3b0d4e0 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,5 @@ .DS_Store __pycache__/ sim_results/ -sim_results_multi/ \ No newline at end of file +sim_results_multi/ +tuningTrials/ \ No newline at end of file diff --git a/AdditiveControlCode/AdditiveControlCode.ino b/AdditiveControlCode/AdditiveControlCode.ino index b67d924..96228ad 100644 --- a/AdditiveControlCode/AdditiveControlCode.ino +++ b/AdditiveControlCode/AdditiveControlCode.ino @@ -5,23 +5,19 @@ #include "FastPWM.hpp" // K, Ki, Kd Constants -Constants repelling = {1000, 0, 10000}; -Constants attracting = {1000, 0, 10000}; +Constants repelling = {250, 0, 1000}; +Constants attracting = {250, 0, 1000}; -Constants RollLeftUp = {500, 0, 10000}; -Constants RollLeftDown = {500, 0, 10000}; +Constants RollLeftUp = {0, 0, 100}; +Constants RollLeftDown = {0, 0, 100}; -Constants RollFrontUp = {500, 0, 10000}; -Constants RollFrontDown = {500, 0, 10000}; +Constants RollFrontUp = {0, 0, 500}; +Constants RollFrontDown = {0, 0, 500}; // Reference values for average dist, -float avgRef = 12.0; // TBD: what is our equilibrium height with this testrig? -float LRDiffRef = 0.0; // TBD: what is our left-right balance equilibrium? Positive -> left is above right -float FBDiffRef = 2; // TBD: what is front-back balance equilibrium? Positive -> front above back. - -float slewRateLimit = 10000.0; // max PWM change per control cycle (determined by 1 second / sampling rate) -// this was implemented by Claude and we can see if it helps. -// Set it at or above 255 to make it have no effect. +float avgRef = 11.0; // TBD: what is our equilibrium height with this testrig? +float LRDiffRef = -2.0; // TBD: what is our left-right balance equilibrium? Positive -> left is above right +float FBDiffRef = 0.0; // TBD: what is front-back balance equilibrium? Positive -> front above back. // Might be useful for things like jitter or lag. #define sampling_rate 1000 // Hz @@ -40,7 +36,7 @@ FullConsts fullConsts = { {RollFrontDown, RollFrontUp} }; -FullController controller(indL, indR, indF, indB, fullConsts, avgRef, LRDiffRef, FBDiffRef, slewRateLimit); +FullController controller(indL, indR, indF, indB, fullConsts, avgRef, LRDiffRef, FBDiffRef); const int dt_micros = 1e6/sampling_rate; @@ -49,7 +45,7 @@ const int dt_micros = 1e6/sampling_rate; int ON = 0; void setup() { - Serial.begin(115200); + Serial.begin(2000000); setupADC(); setupFastPWM(); @@ -72,11 +68,96 @@ void setup() { void loop() { if (Serial.available() > 0) { - // this might need to be changed if we have trouble getting serial to read. - char c = Serial.read(); - while(Serial.available()) Serial.read(); // flush remaining - - controller.outputOn = (c != '0'); + String cmd = Serial.readStringUntil('\n'); + cmd.trim(); + + // Check if it's a reference update command (format: REF,avgRef,lrDiffRef,fbDiffRef) + if (cmd.startsWith("REF,")) { + int firstComma = cmd.indexOf(','); + int secondComma = cmd.indexOf(',', firstComma + 1); + int thirdComma = cmd.indexOf(',', secondComma + 1); + + if (firstComma > 0 && secondComma > 0 && thirdComma > 0) { + float newAvgRef = cmd.substring(firstComma + 1, secondComma).toFloat(); + float newLRDiffRef = cmd.substring(secondComma + 1, thirdComma).toFloat(); + float newFBDiffRef = cmd.substring(thirdComma + 1).toFloat(); + + avgRef = newAvgRef; + LRDiffRef = newLRDiffRef; + FBDiffRef = newFBDiffRef; + + controller.updateReferences(avgRef, LRDiffRef, FBDiffRef); + Serial.print("Updated References: Avg="); + Serial.print(avgRef); + Serial.print(", LR="); + Serial.print(LRDiffRef); + Serial.print(", FB="); + Serial.println(FBDiffRef); + } + } + // Check if it's a PID tuning command (format: PID,mode,kp,ki,kd) + else if (cmd.startsWith("PID,")) { + int firstComma = cmd.indexOf(','); + int secondComma = cmd.indexOf(',', firstComma + 1); + int thirdComma = cmd.indexOf(',', secondComma + 1); + int fourthComma = cmd.indexOf(',', thirdComma + 1); + + if (firstComma > 0 && secondComma > 0 && thirdComma > 0 && fourthComma > 0) { + int mode = cmd.substring(firstComma + 1, secondComma).toInt(); + float kp = cmd.substring(secondComma + 1, thirdComma).toFloat(); + float ki = cmd.substring(thirdComma + 1, fourthComma).toFloat(); + float kd = cmd.substring(fourthComma + 1).toFloat(); + + Constants newConst = {kp, ki, kd}; + + // Mode mapping: + // 0: Repelling + // 1: Attracting + // 2: RollLeftDown + // 3: RollLeftUp + // 4: RollFrontDown + // 5: RollFrontUp + + switch(mode) { + case 0: // Repelling + repelling = newConst; + controller.updateAvgPID(repelling, attracting); + Serial.println("Updated Repelling PID"); + break; + case 1: // Attracting + attracting = newConst; + controller.updateAvgPID(repelling, attracting); + Serial.println("Updated Attracting PID"); + break; + case 2: // RollLeftDown + RollLeftDown = newConst; + controller.updateLRPID(RollLeftDown, RollLeftUp); + Serial.println("Updated RollLeftDown PID"); + break; + case 3: // RollLeftUp + RollLeftUp = newConst; + controller.updateLRPID(RollLeftDown, RollLeftUp); + Serial.println("Updated RollLeftUp PID"); + break; + case 4: // RollFrontDown + RollFrontDown = newConst; + controller.updateFBPID(RollFrontDown, RollFrontUp); + Serial.println("Updated RollFrontDown PID"); + break; + case 5: // RollFrontUp + RollFrontUp = newConst; + controller.updateFBPID(RollFrontDown, RollFrontUp); + Serial.println("Updated RollFrontUp PID"); + break; + default: + Serial.println("Invalid mode"); + break; + } + } + } else { + // Original control on/off command + controller.outputOn = (cmd.charAt(0) != '0'); + } } tDiffMicros = micros() - tprior; diff --git a/MAGLEV_DIGITALTWIN_PYTHON/controller.py b/MAGLEV_DIGITALTWIN_PYTHON/additiveController.py similarity index 98% rename from MAGLEV_DIGITALTWIN_PYTHON/controller.py rename to MAGLEV_DIGITALTWIN_PYTHON/additiveController.py index 246958e..715c584 100644 --- a/MAGLEV_DIGITALTWIN_PYTHON/controller.py +++ b/MAGLEV_DIGITALTWIN_PYTHON/additiveController.py @@ -6,7 +6,7 @@ Ported from decentralizedPIDcontroller.m import numpy as np -class DecentralizedPIDController: +class AdditivePIDController: """ Decentralized PID controller for quadrotor/maglev control. Controls altitude, roll, and pitch using gap sensor feedback. @@ -151,6 +151,6 @@ def decentralized_pid_controller(R, S, P, controller=None): 4-element vector with voltages applied to each yoke """ if controller is None: - controller = DecentralizedPIDController() + controller = AdditivePIDController() return controller.control(R, S, P) diff --git a/MAGLEV_DIGITALTWIN_PYTHON/simulate.py b/MAGLEV_DIGITALTWIN_PYTHON/simulate.py index d809dbf..dc19cac 100644 --- a/MAGLEV_DIGITALTWIN_PYTHON/simulate.py +++ b/MAGLEV_DIGITALTWIN_PYTHON/simulate.py @@ -7,7 +7,7 @@ import numpy as np from scipy.integrate import solve_ivp from utils import euler2dcm, dcm2euler from dynamics import quad_ode_function_hf -from controller import DecentralizedPIDController +from MAGLEV_DIGITALTWIN_PYTHON.additiveController import AdditivePIDController def simulate_maglev_control(R, S, P): @@ -84,7 +84,7 @@ def simulate_maglev_control(R, S, P): xk = x0 # Create controller instance to maintain state - controller = DecentralizedPIDController() + controller = AdditivePIDController() for k in range(N - 1): # loop through each time step tspan = np.arange(S['tVec'][k], S['tVec'][k+1] + dt/2, dt) diff --git a/PID_TUNING_GUIDE.md b/PID_TUNING_GUIDE.md new file mode 100644 index 0000000..940ae67 --- /dev/null +++ b/PID_TUNING_GUIDE.md @@ -0,0 +1,203 @@ +# PID Tuning & Reference Control System - Implementation Guide + +## Overview +This system allows real-time tuning of all 6 PID control modes and reference setpoints through a GUI interface. Changes are transmitted via serial communication and applied immediately to the controller. + +## Control Modes + +The system supports 6 distinct PID control modes: + +1. **Repelling** (Mode 0) - Average height control when pushing away +2. **Attracting** (Mode 1) - Average height control when pulling in +3. **RollLeftDown** (Mode 2) - Left-right balance when left side needs to go down +4. **RollLeftUp** (Mode 3) - Left-right balance when left side needs to go up +5. **RollFrontDown** (Mode 4) - Front-back balance when front needs to go down +6. **RollFrontUp** (Mode 5) - Front-back balance when front needs to go up + +## Reference Values + +The system also supports updating three reference setpoints: + +1. **Average Height Reference (avgRef)** - Target height for levitation (mm) +2. **Left-Right Difference Reference (LRDiffRef)** - Target balance between left and right sensors (mm) + - Positive value: left side should be higher than right + - Negative value: right side should be higher than left +3. **Front-Back Difference Reference (FBDiffRef)** - Target balance between front and back sensors (mm) + - Positive value: front should be higher than back + - Negative value: back should be higher than front + +## Serial Protocol + +### PID Command Format +``` +PID,,,,\n +``` + +### Reference Command Format +``` +REF,,,\n +``` + +### Parameters +**PID Command:** +- **mode**: Integer 0-5 (see control modes above) +- **kp**: Float - Proportional gain +- **ki**: Float - Integral gain +- **kd**: Float - Derivative gain + +**Reference Command:** +- **avgRef**: Float - Average height target in mm +- **lrDiffRef**: Float - Left-right balance difference in mm +- **fbDiffRef**: Float - Front-back balance difference in mm + +### Examples +``` +PID,0,250,0,1000\n # Set Repelling: Kp=250, Ki=0, Kd=1000 +REF,11.0,-2.0,0.0\n # Set avgRef=11mm, LRDiff=-2mm, FBDiff=0mm +PID,3,0,0,100\n # Set RollLeftUp: Kp=0, Ki=0, Kd=100 +REF,12.5,0.0,0.5\n # Set avgRef=12.5mm, LRDiff=0mm, FBDiff=0.5mm +``` + +## Implementation Details + +### Arduino Side (AdditiveControlCode.ino) + +#### Default Values +```cpp +Constants repelling = {250, 0, 1000}; +Constants attracting = {250, 0, 1000}; +Constants RollLeftUp = {0, 0, 100}; +Constants RollLeftDown = {0, 0, 100}; +Constants RollFrontUp = {0, 0, 500}; +Constants RollFrontDown = {0, 0, 500}; +``` + +#### Serial Command Processing +The main loop now parses incoming serial commands: +- Commands starting with "PID," are parsed for PID tuning +- Single character commands ('0' or '1') control the system on/off state +- Upon receiving a PID command, the corresponding Constants struct is updated +- The controller's internal PID values are updated via setter methods + +### Controller (Controller.hpp/cpp) + +#### New Methods +```cpp +// PID update methods +void updateAvgPID(Constants repel, Constants attract); +void updateLRPID(Constants down, Constants up); +void updateFBPID(Constants down, Constants up); + +// Reference update method +void updateReferences(float avgReference, float lrDiffReference, float fbDiffReference); +``` + +These methods update the controller's internal values: +- **PID Constants:** + - `avgConsts` - Controls average height (repelling/attracting) + - `LConsts` - Controls left-right balance (RollLeftDown/RollLeftUp) + - `FConsts` - Controls front-back balance (RollFrontDown/RollFrontUp) +- **Reference Values:** + - `AvgRef` - Target average height + - `LRDiffRef` - Target left-right balance difference + - `FBDiffRef` - Target front-back balance difference + +### Python GUI (serial_plotter.py) + +#### Features +- **Individual PID Entry**: Each control mode has dedicated Kp, Ki, Kd input fields +- **Reference Value Controls**: Set avgRef, LRDiffRef, FBDiffRef targets +- **Send Button**: Each mode has its own "Send" button for individual updates +- **Send All**: A "Send All PID Values" button transmits all 6 modes at once +- **Send References**: Update all reference values with one click +- **Default Values**: GUI is pre-populated with the default values from the Arduino code +- **Scrollable Interface**: All PID controls are in a scrollable panel for easy access + +#### Control Flow - PID Updates +1. User enters PID values in the GUI +2. Clicks "Send" for individual mode or "Send All" for batch update +3. Python formats the command: `PID,,,,\n` +4. Command is sent via serial to Arduino +5. Arduino parses the command and updates the controller +6. Changes take effect immediately in the control loop + +#### Control Flow - Reference Updates +1. User enters reference values in the GUI (Avg Height, LR Diff, FB Diff) +2. Clicks "Send References" +3. Python formats the command: `REF,,,\n` +4. Command is sent via serial to Arduino +5. Arduino updates the reference variables and controller +6. New setpoints take effect immediately + +## Usage Instructions + +### Starting the GUI +```bash +python serial_plotter.py --port /dev/cu.usbmodemXXXX +``` + +Or for testing without hardware: +```bash +python serial_plotter.py --mock +``` + +### Tuning Workflow +1. **Start the application** - GUI launches with current PID values +2. **Connect to Arduino** - Serial connection established automatically +3. **Adjust values** - Modify Kp, Ki, Kd for desired control mode +4. **Send to Arduino** - Click "Send" for that mode or "Send All" +5. **Observe behavior** - Watch real-time plots to see the effect +6. **Iterate** - Adjust and resend as needed + +### Tips for Tuning +- Start with one mode at a time +- Use "Send All" to load a complete tuning set +- Watch the real-time plots to observe the effect of changes +- The system updates immediately - no restart required + +## Data Flow Summary + +**PID Updates:** +``` +GUI Input → Serial Command → Arduino Parse → Constants Update → +Controller Setter → Internal PID Update → Control Loop → PWM Output +``` + +**Reference Updates:** +``` +GUI Input → Serial Command → Arduino Parse → Reference Variables Update → +Controller Setter → Internal Reference Update → Control Loop → PWM Output +``` + +## Compatibility Notes + +- The GUI requires `tkinter` (included with most Python installations) +- Serial communication uses standard pyserial library +- Arduino code uses String parsing (ensure sufficient memory) +- Commands are processed in the main loop between control cycles + +## Troubleshooting + +### GUI doesn't send commands +- Check serial connection +- Verify port selection +- Look for error messages in console + +### Arduino doesn't respond +- Ensure Arduino code is uploaded +- Check Serial Monitor for confirmation messages +- Verify baud rate (115200) + +### Values don't take effect +- Confirm command format is correct +- Check that mode number (0-5) is valid +- Ensure values are within reasonable ranges + +## Future Enhancements + +Possible additions: +- Save/load PID profiles to/from file +- Preset tuning configurations +- Auto-tuning algorithms +- Real-time response visualization +- Logging of PID changes with timestamps diff --git a/lib/Controller.cpp b/lib/Controller.cpp index 76adbdb..3d94b8c 100644 --- a/lib/Controller.cpp +++ b/lib/Controller.cpp @@ -39,16 +39,17 @@ void FullController::sendOutputs() { zeroPWMs(); } - // The following assumes 0 direction drives repulsion and 1 direction drives attraction. - // Using direct register writes to maintain fast PWM mode set by setupFastPWM() + // The following assumes 0 direction drives repulsion and 1 direction drives + // attraction. Using direct register writes to maintain fast PWM mode set by + // setupFastPWM() digitalWrite(dirFL, FLPWM < 0); - OCR2A = abs(FLPWM); // Pin 11 -> Timer 2A + OCR2A = abs(FLPWM); // Pin 11 -> Timer 2A digitalWrite(dirBL, BLPWM < 0); - OCR1A = abs(BLPWM); // Pin 9 -> Timer 1A + OCR1A = abs(BLPWM); // Pin 9 -> Timer 1A digitalWrite(dirFR, FRPWM < 0); - OCR2B = abs(FRPWM); // Pin 3 -> Timer 2B + OCR2B = abs(FRPWM); // Pin 3 -> Timer 2B digitalWrite(dirBR, BRPWM < 0); - OCR1B = abs(BRPWM); // Pin 10 -> Timer 1B + OCR1B = abs(BRPWM); // Pin 10 -> Timer 1B } void FullController::avgControl() { @@ -58,7 +59,8 @@ void FullController::avgControl() { avgError.eDiff = eCurr - avgError.e; if (!oor) { avgError.eInt += eCurr; - avgError.eInt = constrain(avgError.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); + avgError.eInt = + constrain(avgError.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); } avgError.e = eCurr; @@ -67,14 +69,18 @@ void FullController::avgControl() { void FullController::LRControl() { float diff = Right.mmVal - Left.mmVal; // how far above the right is the left? - float eCurr = diff - LRDiffRef; // how different is that from the reference? positive -> Left repels, Right attracts. - K_MAP rConsts = {LConsts.attracting, LConsts.repelling}; // apply attracting to repelling and vice versa. + float eCurr = diff - LRDiffRef; // how different is that from the reference? + // positive -> Left repels, Right attracts. + K_MAP rConsts = { + LConsts.attracting, + LConsts.repelling}; // apply attracting to repelling and vice versa. LRDiffErr.eDiff = eCurr - LRDiffErr.e; if (!oor) { LRDiffErr.eInt += eCurr; - LRDiffErr.eInt = constrain(LRDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); + LRDiffErr.eInt = + constrain(LRDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); } LRDiffErr.e = eCurr; @@ -85,16 +91,18 @@ void FullController::LRControl() { void FullController::FBControl() { float diff = Back.mmVal - Front.mmVal; // how far above the back is the front? - float eCurr = diff - FBDiffRef; // how different is that from ref? pos.->Front must repel, Back must attract + float eCurr = diff - FBDiffRef; // how different is that from ref? pos.->Front + // must repel, Back must attract K_MAP bConsts = {FConsts.attracting, FConsts.repelling}; FBDiffErr.eDiff = eCurr - FBDiffErr.e; if (!oor) { FBDiffErr.eInt += eCurr; - FBDiffErr.eInt = constrain(FBDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); + FBDiffErr.eInt = + constrain(FBDiffErr.eInt, -MAX_INTEGRAL_TERM, MAX_INTEGRAL_TERM); } - + FBDiffErr.e = eCurr; FDiffPWM = pwmFunc(FConsts, FBDiffErr); @@ -102,35 +110,55 @@ void FullController::FBControl() { } int16_t FullController::pwmFunc(K_MAP consts, Errors errs) { - if (oor) return 0; + if (oor) + return 0; Constants constants = (errs.e < 0) ? consts.attracting : consts.repelling; - return (int)constrain(constants.kp*errs.e + constants.ki*errs.eInt + constants.kd*errs.eDiff, -(float)CAP,(float)CAP); + return (int)(constants.kp * errs.e + constants.ki * errs.eInt + + constants.kd * errs.eDiff); } void FullController::report() { - Serial.print("SENSORS - Left: "); + // CSV Format: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn Serial.print(Left.mmVal); - Serial.print("mm, Right: "); + Serial.print(","); Serial.print(Right.mmVal); - Serial.print("mm, Front: "); + Serial.print(","); Serial.print(Front.mmVal); - Serial.print("mm, Back: "); + Serial.print(","); Serial.print(Back.mmVal); - Serial.print("mm,\n"); - Serial.print("AVG - "); - Serial.println(avg); + Serial.print(","); + Serial.print(avg); + Serial.print(","); - Serial.print("PWMS - FL_PWM: "); Serial.print(FLPWM); - Serial.print(", BL_PWM: "); + Serial.print(","); Serial.print(BLPWM); - Serial.print("FR_PWM: "); + Serial.print(","); Serial.print(FRPWM); - Serial.print("BR_PWM: "); + Serial.print(","); Serial.print(BRPWM); - Serial.print("\n"); + Serial.print(","); - Serial.print("CONTROL ON - "); - Serial.print(outputOn); - Serial.print("\n"); + Serial.println(outputOn); +} + +void FullController::updateAvgPID(Constants repel, Constants attract) { + avgConsts.repelling = repel; + avgConsts.attracting = attract; +} + +void FullController::updateLRPID(Constants down, Constants up) { + LConsts.repelling = down; + LConsts.attracting = up; +} + +void FullController::updateFBPID(Constants down, Constants up) { + FConsts.repelling = down; + FConsts.attracting = up; +} + +void FullController::updateReferences(float avgReference, float lrDiffReference, float fbDiffReference) { + AvgRef = avgReference; + LRDiffRef = lrDiffReference; + FBDiffRef = fbDiffReference; } \ No newline at end of file diff --git a/lib/Controller.hpp b/lib/Controller.hpp index 5291dd5..6295aa2 100644 --- a/lib/Controller.hpp +++ b/lib/Controller.hpp @@ -45,7 +45,7 @@ class FullController { bool outputOn; FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b, - FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef, float slewRate) + FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef) : Left(l), Right(r), Front(f), Back(b), AvgRef(avgRef), LRDiffRef(lrDiffRef), FBDiffRef(fbDiffRef), avgConsts(fullConsts.avg), LConsts(fullConsts.lColl), FConsts(fullConsts.fColl), avgError({0,0,0}), LRDiffErr({0,0,0}), @@ -55,13 +55,20 @@ class FullController { void zeroPWMs(); void sendOutputs(); void report(); + + // PID tuning methods + void updateAvgPID(Constants repel, Constants attract); + void updateLRPID(Constants down, Constants up); + void updateFBPID(Constants down, Constants up); + + // Reference update methods + void updateReferences(float avgReference, float lrDiffReference, float fbDiffReference); private: void avgControl(); void LRControl(); void FBControl(); int16_t pwmFunc(K_MAP consts, Errors errs); - int16_t slewLimit(int16_t target, int16_t prev); IndSensor& Front; IndSensor& Back; diff --git a/loadCellCode/loadCellCode.ino b/loadCellCode/loadCellCode.ino new file mode 100644 index 0000000..e5e3889 --- /dev/null +++ b/loadCellCode/loadCellCode.ino @@ -0,0 +1,99 @@ +// +// FILE: HX_calibration.ino +// AUTHOR: Rob Tillaart +// PURPOSE: HX711 calibration finder for offset and scale +// URL: https://github.com/RobTillaart/HX711 + + +#include "HX711.h" + +HX711 myScale; + +// adjust pins if needed. +uint8_t dataPin = 2; +uint8_t clockPin = 3; + + +void setup() +{ + Serial.begin(115200); + Serial.println(); + Serial.println(__FILE__); + Serial.print("HX711_LIB_VERSION: "); + Serial.println(HX711_LIB_VERSION); + Serial.println(); + + myScale.begin(dataPin, clockPin); + myScale.set_offset(26784); + myScale.set_scale(106.557518); +} + +void loop() +{ + // Serial.println(myScale.get_units()); + calibrate(); +} + + + +void calibrate() +{ + Serial.println("\n\nCALIBRATION\n==========="); + Serial.println("remove all weight from the loadcell"); + // flush Serial input + while (Serial.available()) Serial.read(); + + Serial.println("and enter any message into serial.\n"); + while (Serial.available() == 0); + + Serial.println("Determine zero weight offset"); + // average 20 measurements. + myScale.tare(20); + int32_t offset = myScale.get_offset(); + + Serial.print("OFFSET: "); + Serial.println(offset); + Serial.println(); + + + Serial.println("place a weight on the loadcell"); + // flush Serial input + while (Serial.available()) Serial.read(); + + Serial.println("enter the weight in (whole) grams and enter 'e'"); + int16_t weight = 0; + bool neg = false; + while (Serial.peek() != 'e') + { + if (Serial.available()) + { + char ch = Serial.read(); + if (isdigit(ch)) + { + weight *= 10; + weight = weight + (ch - '0'); + } + else if (ch == '-') neg = true; + } + } + if (neg) weight *= -1; + Serial.print("WEIGHT: "); + Serial.println(weight); + myScale.calibrate_scale(weight, 20); + float scale = myScale.get_scale(); + + Serial.print("SCALE: "); + Serial.println(scale, 6); + + Serial.print("\nuse scale.set_offset("); + Serial.print(offset); + Serial.print("); and scale.set_scale("); + Serial.print(scale, 6); + Serial.print(");\n"); + Serial.println("in the setup of your project"); + + Serial.println("\n\n"); +} + + +// -- END OF FILE -- \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index 7cdcf2f..a2b1207 100644 --- a/requirements.txt +++ b/requirements.txt @@ -12,3 +12,4 @@ python-dateutil==2.9.0.post0 scipy==1.16.3 six==1.17.0 sympy==1.14.0 +pyserial==3.5 diff --git a/serial_plotter.py b/serial_plotter.py new file mode 100644 index 0000000..40130a0 --- /dev/null +++ b/serial_plotter.py @@ -0,0 +1,653 @@ +import serial +import serial.tools.list_ports +import matplotlib +matplotlib.use('TkAgg') # Use TkAgg backend explicitly +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg +from collections import deque +import argparse +import time +import random +import threading +import sys +from datetime import datetime +import queue +import tkinter as tk +from tkinter import ttk + +# Constants +BAUD_RATE = 2000000 +MAX_POINTS = 100 # Number of points to display on the plot + +# Save queue for handling saves in main thread +save_queue = queue.Queue() + +# Data storage +data = { + "Left": deque(maxlen=MAX_POINTS), + "Right": deque(maxlen=MAX_POINTS), + "Front": deque(maxlen=MAX_POINTS), + "Back": deque(maxlen=MAX_POINTS), + "Avg": deque(maxlen=MAX_POINTS), + "FLPWM": deque(maxlen=MAX_POINTS), + "BLPWM": deque(maxlen=MAX_POINTS), + "FRPWM": deque(maxlen=MAX_POINTS), + "BRPWM": deque(maxlen=MAX_POINTS), + "ControlOn": deque(maxlen=MAX_POINTS), + "Time": deque(maxlen=MAX_POINTS) +} + +# Recording data storage (no max length limit) +recording_data = { + "Left": [], + "Right": [], + "Front": [], + "Back": [], + "Avg": [], + "FLPWM": [], + "BLPWM": [], + "FRPWM": [], + "BRPWM": [], + "ControlOn": [], + "Time": [] +} + +start_time = time.time() +recording = False +recording_start_time = None + +def get_serial_port(): + ports = list(serial.tools.list_ports.comports()) + if not ports: + return None + + print("Available ports:") + for i, p in enumerate(ports): + print(f"{i}: {p.device} - {p.description}") + + if len(ports) == 1: + return ports[0].device + + try: + selection = int(input("Select port index: ")) + return ports[selection].device + except (ValueError, IndexError): + print("Invalid selection.") + return None + +def mock_data_generator(): + """Generates mock data in the expected CSV format.""" + while True: + # Simulate sensor readings (around 12.0mm with noise) + left = 12.0 + random.uniform(-0.5, 0.5) + right = 12.0 + random.uniform(-0.5, 0.5) + front = 12.0 + random.uniform(-0.5, 0.5) + back = 12.0 + random.uniform(-0.5, 0.5) + avg = (left + right + front + back) / 4.0 + + # Simulate PWM values (around 0 with noise) + fl_pwm = random.randint(-50, 50) + bl_pwm = random.randint(-50, 50) + fr_pwm = random.randint(-50, 50) + br_pwm = random.randint(-50, 50) + + control_on = 1 + + # CSV Format: Left,Right,Front,Back,Avg,FLPWM,BLPWM,FRPWM,BRPWM,ControlOn + line = f"{left:.2f},{right:.2f},{front:.2f},{back:.2f},{avg:.2f},{fl_pwm},{bl_pwm},{fr_pwm},{br_pwm},{control_on}" + yield line + time.sleep(0.01) # 100Hz + +def read_serial(ser, mock=False): + """Reads data from serial port or mock generator.""" + global recording, recording_start_time + generator = mock_data_generator() if mock else None + + while True: + try: + if mock: + line = next(generator) + else: + if ser.in_waiting: + line = ser.readline().decode('utf-8').strip() + else: + continue + + parts = line.split(',') + if len(parts) == 10: + current_time = time.time() - start_time + + left = float(parts[0]) + right = float(parts[1]) + front = float(parts[2]) + back = float(parts[3]) + avg = float(parts[4]) + flpwm = float(parts[5]) + blpwm = float(parts[6]) + frpwm = float(parts[7]) + brpwm = float(parts[8]) + control_on = int(parts[9]) + + # Update live display data + data["Left"].append(left) + data["Right"].append(right) + data["Front"].append(front) + data["Back"].append(back) + data["Avg"].append(avg) + data["FLPWM"].append(flpwm) + data["BLPWM"].append(blpwm) + data["FRPWM"].append(frpwm) + data["BRPWM"].append(brpwm) + data["ControlOn"].append(control_on) + data["Time"].append(current_time) + + # If recording, store in unlimited buffer + if recording: + recording_time = current_time - recording_start_time + recording_data["Left"].append(left) + recording_data["Right"].append(right) + recording_data["Front"].append(front) + recording_data["Back"].append(back) + recording_data["Avg"].append(avg) + recording_data["FLPWM"].append(flpwm) + recording_data["BLPWM"].append(blpwm) + recording_data["FRPWM"].append(frpwm) + recording_data["BRPWM"].append(brpwm) + recording_data["ControlOn"].append(control_on) + recording_data["Time"].append(recording_time) + + except ValueError: + pass # Ignore parse errors + except Exception as e: + print(f"Error reading data: {e}") + if not mock: + break + +def save_recording(metadata=None): + """Save the recorded data to a high-quality PNG file.""" + global recording_data + + if not recording_data["Time"]: + print("No data recorded to save.") + return + + # Generate filename with timestamp + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + + # Create directory if it doesn't exist + import os + save_dir = "tuningTrials" + os.makedirs(save_dir, exist_ok=True) + + filename = f"{save_dir}/trial_{timestamp}.png" + + # Create a completely independent figure with Agg backend + import matplotlib + from matplotlib.figure import Figure + from matplotlib.backends.backend_agg import FigureCanvasAgg + + save_fig = Figure(figsize=(14, 10)) + canvas = FigureCanvasAgg(save_fig) + save_fig.suptitle(f'Levitation Control Trial - {timestamp}', fontsize=16, fontweight='bold') + + # Plot Sensors + ax1 = save_fig.add_subplot(2, 1, 1) + ax1.plot(recording_data["Time"], recording_data["Left"], label="Left", linewidth=2) + ax1.plot(recording_data["Time"], recording_data["Right"], label="Right", linewidth=2) + ax1.plot(recording_data["Time"], recording_data["Front"], label="Front", linewidth=2) + ax1.plot(recording_data["Time"], recording_data["Back"], label="Back", linewidth=2) + ax1.plot(recording_data["Time"], recording_data["Avg"], label="Avg", linestyle='--', color='black', linewidth=2.5) + ax1.set_ylabel("Distance (mm)", fontsize=12, fontweight='bold') + ax1.legend(loc='upper right', fontsize=10) + ax1.set_title("Sensor Readings", fontsize=14, fontweight='bold') + ax1.grid(True, alpha=0.3) + ax1.set_xlim([0, recording_data["Time"][-1]]) + + # Plot PWMs + ax2 = save_fig.add_subplot(2, 1, 2) + ax2.plot(recording_data["Time"], recording_data["FLPWM"], label="FL", linewidth=2) + ax2.plot(recording_data["Time"], recording_data["BLPWM"], label="BL", linewidth=2) + ax2.plot(recording_data["Time"], recording_data["FRPWM"], label="FR", linewidth=2) + ax2.plot(recording_data["Time"], recording_data["BRPWM"], label="BR", linewidth=2) + ax2.set_ylabel("PWM Value", fontsize=12, fontweight='bold') + ax2.set_xlabel("Time (s)", fontsize=12, fontweight='bold') + ax2.legend(loc='upper right', fontsize=10) + ax2.set_title("PWM Outputs", fontsize=14, fontweight='bold') + ax2.grid(True, alpha=0.3) + ax2.set_xlim([0, recording_data["Time"][-1]]) + + save_fig.tight_layout() + + # Save with high DPI using the canvas + canvas.print_figure(filename, dpi=300, bbox_inches='tight') + + print(f"\n{'='*60}") + print(f"Trial data saved to: {filename}") + print(f"Duration: {recording_data['Time'][-1]:.2f} seconds") + print(f"Data points: {len(recording_data['Time'])}") + + # Save metadata if provided + if metadata: + txt_filename = f"{save_dir}/trial_{timestamp}.txt" + try: + with open(txt_filename, 'w') as f: + f.write(f"Trial Timestamp: {timestamp}\n") + f.write(f"Duration: {recording_data['Time'][-1]:.2f} seconds\n") + f.write(f"Data Points: {len(recording_data['Time'])}\n\n") + + f.write("=== Reference Values ===\n") + if "references" in metadata: + for key, value in metadata["references"].items(): + f.write(f"{key}: {value}\n") + f.write("\n") + + f.write("=== PID Parameters ===\n") + if "pid" in metadata: + for mode, values in metadata["pid"].items(): + f.write(f"[{mode}]\n") + f.write(f" Kp: {values['Kp']}\n") + f.write(f" Ki: {values['Ki']}\n") + f.write(f" Kd: {values['Kd']}\n") + f.write("\n") + print(f"Metadata saved to: {txt_filename}") + except Exception as e: + print(f"Failed to save metadata: {e}") + + print(f"{'='*60}\n") + + +def update_plot(frame): + global recording + + # Check if there's a save request + try: + while not save_queue.empty(): + item = save_queue.get_nowait() + if isinstance(item, dict): + save_recording(metadata=item) + else: + save_recording() + except: + pass + + plt.clf() + + # Add recording indicator + fig = plt.gcf() + if recording: + fig.patch.set_facecolor('#ffe6e6') # Light red background when recording + else: + fig.patch.set_facecolor('white') + + # Plot Sensors + plt.subplot(2, 1, 1) + plt.plot(data["Time"], data["Left"], label="Left") + plt.plot(data["Time"], data["Right"], label="Right") + plt.plot(data["Time"], data["Front"], label="Front") + plt.plot(data["Time"], data["Back"], label="Back") + plt.plot(data["Time"], data["Avg"], label="Avg", linestyle='--', color='black') + plt.ylabel("Distance (mm)") + plt.legend(loc='upper right') + title = "Sensor Readings" + if recording: + title += " [RECORDING]" + plt.title(title) + plt.grid(True) + + # Plot PWMs + plt.subplot(2, 1, 2) + plt.plot(data["Time"], data["FLPWM"], label="FL") + plt.plot(data["Time"], data["BLPWM"], label="BL") + plt.plot(data["Time"], data["FRPWM"], label="FR") + plt.plot(data["Time"], data["BRPWM"], label="BR") + plt.ylabel("PWM Value") + plt.xlabel("Time (s)") + plt.legend(loc='upper right') + title = "PWM Outputs" + if recording: + title += " [RECORDING]" + plt.title(title) + plt.grid(True) + + plt.tight_layout() + +def main(): + parser = argparse.ArgumentParser(description='Serial Plotter for Levitation Control') + parser.add_argument('--port', type=str, help='Serial port to connect to') + parser.add_argument('--mock', action='store_true', help='Use mock data instead of serial') + args = parser.parse_args() + + ser = None + if not args.mock: + port = args.port or get_serial_port() + if not port: + print("No serial port found or selected. Use --mock to test without hardware.") + return + + try: + ser = serial.Serial(port, BAUD_RATE, timeout=1) + print(f"Connected to {port} at {BAUD_RATE} baud.") + except serial.SerialException as e: + print(f"Error connecting to serial port: {e}") + return + + # Start data reading thread + thread = threading.Thread(target=read_serial, args=(ser, args.mock), daemon=True) + thread.start() + + # Send initial start command + if ser: + try: + print("Sending start command...") + ser.write(b'1') + except Exception as e: + print(f"Failed to send start command: {e}") + + # Create Tkinter window + root = tk.Tk() + root.title("Levitation Control - Serial Plotter") + + # Create main container + main_container = ttk.Frame(root) + main_container.pack(fill=tk.BOTH, expand=True) + + # Left panel for controls + control_panel = ttk.Frame(main_container, width=400) + control_panel.pack(side=tk.LEFT, fill=tk.BOTH, padx=5, pady=5) + + # Right panel for plot + plot_panel = ttk.Frame(main_container) + plot_panel.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True, padx=5, pady=5) + + # === CONTROL BUTTONS === + btn_frame = ttk.LabelFrame(control_panel, text="Control", padding=10) + btn_frame.pack(fill=tk.X, pady=5) + + def start_control(): + if ser: + ser.write(b'1') + print("Control started") + global recording, recording_start_time, recording_data + recording = True + recording_start_time = time.time() - start_time + for key in recording_data: + recording_data[key] = [] + + def stop_control(): + if ser: + ser.write(b'0') + print("Control stopped") + global recording + if recording: + recording = False + + # Collect metadata + metadata = { + "references": { + "Avg Height": avg_ref_entry.get(), + "LR Diff": lr_diff_entry.get(), + "FB Diff": fb_diff_entry.get() + }, + "pid": {} + } + + # Collect PID values + for name, _, _, _, _ in pid_modes: + kp_e, ki_e, kd_e = pid_entries[name] + metadata["pid"][name] = { + "Kp": kp_e.get(), + "Ki": ki_e.get(), + "Kd": kd_e.get() + } + + save_queue.put(metadata) + + ttk.Button(btn_frame, text="Start Control & Record", command=start_control, width=25).pack(pady=2) + ttk.Button(btn_frame, text="Stop Control & Save", command=stop_control, width=25).pack(pady=2) + + # === REFERENCE VALUES === + ref_frame = ttk.LabelFrame(control_panel, text="Reference Values", padding=10) + ref_frame.pack(fill=tk.X, pady=5) + + # Average Reference + ttk.Label(ref_frame, text="Avg Height (mm):", width=15).grid(row=0, column=0, sticky=tk.W, pady=2) + avg_ref_entry = ttk.Entry(ref_frame, width=12) + avg_ref_entry.insert(0, "11.0") + avg_ref_entry.grid(row=0, column=1, padx=5, pady=2) + + # Left-Right Diff Reference + ttk.Label(ref_frame, text="LR Diff (mm):", width=15).grid(row=1, column=0, sticky=tk.W, pady=2) + lr_diff_entry = ttk.Entry(ref_frame, width=12) + lr_diff_entry.insert(0, "-2.0") + lr_diff_entry.grid(row=1, column=1, padx=5, pady=2) + + # Front-Back Diff Reference + ttk.Label(ref_frame, text="FB Diff (mm):", width=15).grid(row=2, column=0, sticky=tk.W, pady=2) + fb_diff_entry = ttk.Entry(ref_frame, width=12) + fb_diff_entry.insert(0, "0.0") + fb_diff_entry.grid(row=2, column=1, padx=5, pady=2) + + def send_references(): + try: + avg_ref = float(avg_ref_entry.get()) + lr_diff = float(lr_diff_entry.get()) + fb_diff = float(fb_diff_entry.get()) + cmd = f"REF,{avg_ref},{lr_diff},{fb_diff}\n" + if ser: + ser.write(cmd.encode('utf-8')) + print(f"Sent References: Avg={avg_ref}, LR={lr_diff}, FB={fb_diff}") + else: + print(f"Mock mode - would send: {cmd.strip()}") + except ValueError as e: + print(f"Error: Invalid reference values - {e}") + + ttk.Button(ref_frame, text="Send References", command=send_references, width=25).grid(row=3, column=0, columnspan=2, pady=10) + + # === PID TUNING === + pid_frame = ttk.LabelFrame(control_panel, text="PID Tuning", padding=10) + pid_frame.pack(fill=tk.BOTH, expand=True, pady=5) + + # Create scrollable frame + canvas = tk.Canvas(pid_frame, height=500) + scrollbar = ttk.Scrollbar(pid_frame, orient="vertical", command=canvas.yview) + scrollable_frame = ttk.Frame(canvas) + + scrollable_frame.bind( + "", + lambda e: canvas.configure(scrollregion=canvas.bbox("all")) + ) + + canvas.create_window((0, 0), window=scrollable_frame, anchor="nw") + canvas.configure(yscrollcommand=scrollbar.set) + + # PID modes with default values from the .ino file + pid_modes = [ + ("Repelling", 0, 250, 0, 1000), + ("Attracting", 1, 250, 0, 1000), + ("RollLeftDown", 2, 0, 0, 100), + ("RollLeftUp", 3, 0, 0, 100), + ("RollFrontDown", 4, 0, 0, 500), + ("RollFrontUp", 5, 0, 0, 500), + ] + + pid_entries = {} + + for name, mode, default_kp, default_ki, default_kd in pid_modes: + mode_frame = ttk.LabelFrame(scrollable_frame, text=name, padding=5) + mode_frame.pack(fill=tk.X, pady=5, padx=5) + + # Kp + ttk.Label(mode_frame, text="Kp:", width=5).grid(row=0, column=0, sticky=tk.W) + kp_entry = ttk.Entry(mode_frame, width=12) + kp_entry.insert(0, str(default_kp)) + kp_entry.grid(row=0, column=1, padx=5) + + # Ki + ttk.Label(mode_frame, text="Ki:", width=5).grid(row=1, column=0, sticky=tk.W) + ki_entry = ttk.Entry(mode_frame, width=12) + ki_entry.insert(0, str(default_ki)) + ki_entry.grid(row=1, column=1, padx=5) + + # Kd + ttk.Label(mode_frame, text="Kd:", width=5).grid(row=2, column=0, sticky=tk.W) + kd_entry = ttk.Entry(mode_frame, width=12) + kd_entry.insert(0, str(default_kd)) + kd_entry.grid(row=2, column=1, padx=5) + + # Send button + def make_send_func(mode_num, kp_e, ki_e, kd_e, mode_name): + def send_pid(): + try: + kp = float(kp_e.get()) + ki = float(ki_e.get()) + kd = float(kd_e.get()) + cmd = f"PID,{mode_num},{kp},{ki},{kd}\n" + if ser: + ser.write(cmd.encode('utf-8')) + print(f"Sent {mode_name}: Kp={kp}, Ki={ki}, Kd={kd}") + else: + print(f"Mock mode - would send: {cmd.strip()}") + except ValueError as e: + print(f"Error: Invalid PID values - {e}") + return send_pid + + send_btn = ttk.Button(mode_frame, text="Send", + command=make_send_func(mode, kp_entry, ki_entry, kd_entry, name), + width=10) + send_btn.grid(row=3, column=0, columnspan=2, pady=5) + + pid_entries[name] = (kp_entry, ki_entry, kd_entry) + + # Send All button + def send_all_pid(): + for name, mode, _, _, _ in pid_modes: + kp_e, ki_e, kd_e = pid_entries[name] + try: + kp = float(kp_e.get()) + ki = float(ki_e.get()) + kd = float(kd_e.get()) + cmd = f"PID,{mode},{kp},{ki},{kd}\n" + if ser: + ser.write(cmd.encode('utf-8')) + time.sleep(0.05) # Small delay between commands + print(f"Sent {name}: Kp={kp}, Ki={ki}, Kd={kd}") + except ValueError as e: + print(f"Error in {name}: {e}") + + send_all_frame = ttk.Frame(scrollable_frame) + send_all_frame.pack(fill=tk.X, pady=10, padx=5) + ttk.Button(send_all_frame, text="Send All PID Values", command=send_all_pid, width=25).pack() + + canvas.pack(side=tk.LEFT, fill=tk.BOTH, expand=True) + scrollbar.pack(side=tk.RIGHT, fill=tk.Y) + + # === PLOT SETUP === + fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 8)) + fig.tight_layout(pad=3.0) + canvas_plot = FigureCanvasTkAgg(fig, master=plot_panel) + canvas_plot.get_tk_widget().pack(fill=tk.BOTH, expand=True) + + # Setup axes once + ax1.set_ylabel("Distance (mm)") + ax1.grid(True) + ax1.set_title("Sensor Readings") + + ax2.set_ylabel("PWM Value") + ax2.set_xlabel("Time (s)") + ax2.grid(True) + ax2.set_title("PWM Outputs") + + # Initialize line objects for efficient updating + line_left, = ax1.plot([], [], label="Left") + line_right, = ax1.plot([], [], label="Right") + line_front, = ax1.plot([], [], label="Front") + line_back, = ax1.plot([], [], label="Back") + line_avg, = ax1.plot([], [], label="Avg", linestyle='--', color='black', linewidth=1.5) + + line_fl, = ax2.plot([], [], label="FL") + line_bl, = ax2.plot([], [], label="BL") + line_fr, = ax2.plot([], [], label="FR") + line_br, = ax2.plot([], [], label="BR") + + # Create legends after line objects + ax1.legend(loc='upper right') + ax2.legend(loc='upper right') + + # Track recording state to minimize updates + _was_recording = False + + def update_plot(frame): + global recording + nonlocal _was_recording + + # Check if there's a save request + try: + while not save_queue.empty(): + item = save_queue.get_nowait() + if isinstance(item, dict): + save_recording(metadata=item) + else: + save_recording() + except: + pass + + # Only update background color when recording state changes + if recording != _was_recording: + if recording: + fig.patch.set_facecolor('#ffe6e6') + ax1.set_title("Sensor Readings [RECORDING]") + ax2.set_title("PWM Outputs [RECORDING]") + else: + fig.patch.set_facecolor('white') + ax1.set_title("Sensor Readings") + ax2.set_title("PWM Outputs") + _was_recording = recording + + # Update line data efficiently (no clear/replot) + if len(data["Time"]) > 0: + time_data = list(data["Time"]) + + line_left.set_data(time_data, list(data["Left"])) + line_right.set_data(time_data, list(data["Right"])) + line_front.set_data(time_data, list(data["Front"])) + line_back.set_data(time_data, list(data["Back"])) + line_avg.set_data(time_data, list(data["Avg"])) + + line_fl.set_data(time_data, list(data["FLPWM"])) + line_bl.set_data(time_data, list(data["BLPWM"])) + line_fr.set_data(time_data, list(data["FRPWM"])) + line_br.set_data(time_data, list(data["BRPWM"])) + + # Auto-scale axes + ax1.relim() + ax1.autoscale_view() + ax2.relim() + ax2.autoscale_view() + + canvas_plot.draw_idle() + + def on_close(): + print("Window closed. Exiting script.") + if ser: + ser.close() + root.quit() + root.destroy() + sys.exit() + + root.protocol("WM_DELETE_WINDOW", on_close) + + # Start animation with slower interval for smoother GUI (100ms = 10 FPS is sufficient) + ani = animation.FuncAnimation(fig, update_plot, interval=100, cache_frame_data=False, blit=False) + + print("\n" + "="*60) + print("SERIAL PLOTTER - GUI MODE") + print("="*60) + print(" Use buttons to start/stop control and recording") + print(" Adjust PID values and click 'Send' to update Arduino") + print("="*60 + "\n") + + root.mainloop() + +if __name__ == "__main__": + main()