Hi everyone,
I'm working on a part of a project in which I have to raise a square base vertically up while keeping the model on the top horizontally level. There are 4 linear actuators, one at each corner of the base, with a model (mars) on top, as shown.
Please note:This actuator does not provide any feedback on length, nor can it be controlled by position input. It only has a 'positive' and 'negative' for the terminals of the motor.
My initial approach was to use a BNO055 mounted on the top face of the platform to measure the angle in the Y and Z axis. I treated it as two separate systems by pairing the actuators in diagonals and having the slower actuator in each pair raise at its maximum speed, while the other actuator in the pair (diagonally opposite) tried to level the platform in that axis by varying its speed via a PID controller.
The BNO055 is mounted at a 45-degree angle in the X-axis so that the Y and Z axis of the sensor are along the diagonals of the platform.
The problem I'm facing is that the actuators that are under PID control never really level with the other actuator in their pair since the actuators' max speeds are quite slow and the PID starts a bit late so even when the PID output reaches maximum speed, it takes very long for the actuator to catch up, if at all it does.
The second issue is with the difference between the speeds of the uncontrolled actuators in each pair (the ones that start rasing at max speed from the start). This difference leads to scenarios where the platform is resting level on 3 of the 4 legs with the 4th one not in contact with the ground at all (due to it being slow). But from the sensor's point of view it won't see this problem at all since the platform is level and so the angle is 0 on both Y and Z axis.
Here is the code that does what I described above:
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <PID_v1.h>
#include <EEPROM.h>
double setpoint = 0, input_z, out_z, input_y, out_y;
double z_Kp = 10, z_Ki = 8, z_Kd = 0; // Z-axis: Kp, Ki and Kd are PID tuning parameters
double y_Kp = 10, y_Ki = 8, y_Kd = 0; // Y-axis: Kp, Ki and Kd are PID tuning parameters
int z_pair[5] = {8, 9, 6, 7}; // array holding pin numbers for z-axis actuator pair in the form {IN1, IN2, EN1/PWM1, EN2/PWM2}
int y_pair[5] = {2, 4, 3, 5}; // array holding pin numbers for y-axis actuator pair in the form {IN1, IN2, EN1/PWM1, EN2/PWM2}
PID pid_z(&input_z, &out_z, &setpoint, z_Kp, z_Ki, z_Kd, DIRECT);
PID pid_y(&input_y, &out_y, &setpoint, y_Kp, y_Ki, y_Kd, DIRECT);
// BNO055 and Unified sensor lib variables
Adafruit_BNO055 IMU = Adafruit_BNO055(55);
sensors_event_t event;
adafruit_bno055_offsets_t calibrationData;
void setup() {
Serial.begin(115200);
// Set pins to output
for (int pin = 0; pin < 4; pin++) {
pinMode(z_pair[pin], OUTPUT);
pinMode(y_pair[pin], OUTPUT);
}
// Set the direction of the actuators via motor driver I/O
digitalWrite(z_pair[0], LOW);
digitalWrite(z_pair[1], HIGH);
digitalWrite(y_pair[0], LOW);
digitalWrite(y_pair[1], HIGH);
// Set PWM of uncontrolled actuator in each pair to full speed
digitalWrite(y_pair[3], HIGH);
digitalWrite(z_pair[3], HIGH);
if(!IMU.begin())
{
Serial.print("No BNO055 detected!");
while(1);
}
delay(1000);
IMU.setExtCrystalUse(true);
int eeAddress = sizeof(long);
EEPROM.get(eeAddress, calibrationData);
displaySensorOffsets(calibrationData);
Serial.println("\n\nRestoring Calibration data to the BNO055...");
IMU.setSensorOffsets(calibrationData);
Serial.println("\n\nCalibration data loaded into BNO055");
IMU.getEvent(&event);
input_z = event.orientation.z;
input_y = event.orientation.y;
setpoint = 0;
pid_z.SetOutputLimits(223, 255); // functional range is 225 to 255 (<225 ==> 0)
pid_y.SetOutputLimits(223, 255); // functional range is 225 to 255 (<225 ==> 0)
pid_z.SetMode(AUTOMATIC);
pid_y.SetMode(AUTOMATIC);
}
void loop() {
IMU.getEvent(&event);
input_z = event.orientation.z;
input_y = event.orientation.y;
pid_z.Compute();
pid_y.Compute();
if (out_z < 225) {
out_z = 0;
}
if (out_y < 225) {
out_y = 0;
}
Serial.println((String)input_z + ", " + out_z + ", " + input_y + ", " + out_y); // Debugging Info
// Set actuator speeds from PID
analogWrite(z_pair[2], out_z);
analogWrite(y_pair[2], out_y);
delay(25);
}
Any help on how to fix these issues or any other approach to tackling this challenge? Thank you so much in advance.
P.S: I'm also now considering using the VL530X on each linear actuator to use the lengths of each actuator rather than an IMU, but I'm really not sure on how to implement PID(s) with 4 sensor readings.
