Hey guys,
im stuck with this one. Im trying to control an inverted pendulum via Python where i use the Arduino only as I/O-Device and to do the complex calculations in Python.
The Idea is that every 50ms the Arduino calculates values and sends them to python. Python receives the values and sends a control value U back every then 50ms aswell. The arduino takes the value executes it and waits for its 50ms cycle to start again. So the idea is to have to independent loops who (asynchron) are sending/receiving data.
The values that the Arduino sends aswell as the calculated value in Python are correct (i did a bunch of debugging on that hoping to find the mistake there).
Problem:
I kinda works in the beginning for small values of theta. But the reaction time is somewhat shitty and the motor turns sometimes in weird directions with weird speeds and shows in general often glitchy/unexpected and stuttering behaviour. I used the same sketch with a different loop for speed tests and it was super smooth.
Ideas:
I think the communication doesnt work all the time so weird values get parsed etc.. Also just for testing the I and D values are not set for testing purposes.
Does anyone has an Idea what should i do better or what is the problem?
here is the (hardcore) shortened Arduino code + the Python code
#include <DueTimer.h>
#include <Wire.h>
#include <AS5600.h>
//AS5600
AS5600 as5600(&Wire);
#define BAUDRATE 115200 // has to be the same as in python // not used currently bcs i use natove portg
//Variables
volatile bool i2c_request = true;
constexpr unsigned long communication_time = 50; // has to be the same as in python
//PID
constexpr int PWM_MAX_CMD = 100;
constexpr unsigned CMD_TIMEOUT_MS = 100; // wenn länger nichts kommt -> Stop
static int last_pwm_cmd = 0;
static unsigned long last_cmd_ms = 0;
static char rxbuf[32];
static uint8_t rxidx = 0;
constexpr int PWM_HW_MAX = 100; // max PWM
constexpr int CMD_ABS_MAX = 100; // max expected value from Python
//Helpers///////////////////////////////////////////////////////////////
inline void setMotorSignedPWM_direct(float u_cmd) {
// Richtung aus Vorzeichen
int dir = (u_cmd >= 0.0f) ? +1 : -1;
dir *= MOTOR_DIR_POL;
// |u| -> PWM linear 0..255
float au = fabsf(u_cmd);
if (au > CMD_ABS_MAX) au = CMD_ABS_MAX;
int pwm = (int)roundf(au * (PWM_HW_MAX / (float)CMD_ABS_MAX));
if (pwm < 0) pwm = 0;
if (pwm > PWM_HW_MAX) pwm = PWM_HW_MAX;
// Ausgänge setzen
digitalWrite(DIR_PIN, (dir > 0) ? LOW : HIGH);
analogWrite(PWM_PIN, pwm);
// Debug (optional)
last_cmd_ms = millis();
}
inline void applyCommandLine(const char* s) {
if (tripped) return;
// erwartet DezimalPUNKT und '\n' am Ende
float u = atof(s);
setMotorSignedPWM_direct(u);
}
///////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(BAUDRATE);
while (!Serial) {}
//Magnetic Encoder
pinMode(20, INPUT_PULLUP);
pinMode(21, INPUT_PULLUP);
//Motor PINS
pinMode(PWM_PIN, OUTPUT);
pinMode(DIR_PIN, OUTPUT);
//Magnetic Encoder init
Wire.begin();
Wire.setClock(100000);
if (!as5600.begin()) { //test if the sensor go recognized
Serial.println("AS5600 not found on Wire. Check wiring.");
while (1) { delay(1000); }
}
delay(5000);
}
void loop() {
static unsigned long next_ms = 0;
unsigned long now = millis();
//collect line from python
while (Serial.available() > 0) {
char c = (char)Serial.read();
if (c == '\n') {
if (rxidx > 0) {
rxbuf[rxidx] = '\0';
applyCommandLine(rxbuf); // sets pwm and dir_pin
rxidx = 0;
}
} else if (c == '\r') {
} else if (rxidx < sizeof(rxbuf) - 1) {
rxbuf[rxidx++] = c;
} else {
rxidx = 0;
}
}
// 10ms rate
if ((long)(now - next_ms) >= 0) {
// if we are to late:
do { next_ms += communication_time; } while ((long)(now - next_ms) >= 0);
//
if (i2c_request) {
i2c_request = false;
uint16_t raw = readRaw();
angle_unwrapped += (float)delta_from_raw(raw, last_raw) * step_rad;
last_raw = raw;
theta_new = angle_unwrapped;
}
// Position/States bilden
long counts = absolute_counts(); // im loop ok
float x_m = counts_to_meters(counts);
float theta = wrap_to_pi(angle_unwrapped);
//
Serial.print(x_m, 1); Serial.print(',');
Serial.print(v_filtered, 1); Serial.print(',');
Serial.print(theta, 3); Serial.print(',');
Serial.println(omega_filtered, 2);
}
// failsafe (optional)
if (last_cmd_ms != 0 && (millis() - last_cmd_ms > CMD_TIMEOUT_MS)) {
analogWrite(PWM_PIN, 0);
}
}
and here the python code
import serial, time
from PID import PIDAngle, PIDParams
PORT = "/dev/ttyACM0"
BAUD = 115200
communication_time = 0.05
ser = serial.Serial(PORT, BAUD, timeout=1)
time.sleep(2)
ser.reset_input_buffer()
params = PIDParams(Kp=300.0, Ki=0.0, Kd=0.0, u_min=-100.0, u_max=100.0)
pid = PIDAngle(params, dt=0.01)
theta_ref = 0.0
def read_latest_line(ser):
latest = None
# Solange Bytes im Puffer sind, Zeilen lesen und jeweils die letzte behalten
while ser.in_waiting:
line = ser.readline().decode(errors="ignore").strip()
if line:
latest = line
# Falls wir nichts im Buffer hatten, versuche normal eine Zeile zu lesen
if latest is None:
latest = ser.readline().decode(errors="ignore").strip()
return latest if latest else ""
def main():
try:
next_t = time.monotonic()
while True:
next_t += communication_time
# read sensor values
line = read_latest_line(ser)
if not line:
#no line
pass
else:
try:
x, v , thetampc, thetadotmpc = map(float, line.split(","))
except ValueError:
# skip broken line
thetampc = thetadotmpc = None
# calculate PID and send it
if thetampc is not None:
u = pid.step((thetampc, thetadotmpc))
ser.write(f"{u:.3f}\n".encode())
#print(f"{u:.1f}")
print(f"theta={thetampc:.3f}, thetaDot={thetadotmpc:.3f}, {u:.1f}")
# --- Takt einhalten ---
remain = next_t - time.monotonic()
if remain > 0:
time.sleep(remain)
else:
next_t = time.monotonic()
except KeyboardInterrupt:
pass
finally:
ser.close()
if __name__ == "__main__":
main()