Self stabilising spoon

Projects General Guidance
1

So, i made some changes but the servos seem to be moving sluggish despite the fact that Kp was raised . Any recommendations about improvements?


#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>

Servo servox;
Servo servoy;

const int servox_pin = 5;
const int servoy_pin = 6;

class PID {
  public:
    // Constructor
    PID(float Kp, float Ki, float Kd, float setpoint = 0) {
      this->Kp = Kp;
      this->Ki = Ki;
      this->Kd = Kd;
      this->setpoint = setpoint;
      previous_error = 0;
      integral = 0;
    }

    // Update method to calculate PID output
    float update(float measured_value, float dt) {
      float error = setpoint - measured_value;
      integral += error * dt;
      float derivative = (error - previous_error) / dt;
      float output = Kp * error + Ki * integral + Kd * derivative;
      previous_error = error;
      return output;
    }

    // Set the setpoint value
    void setSetpoint(float sp) {
      setpoint = sp;
    }

  private:
    // PID coefficients
    float Kp, Ki, Kd;
    
    // Internal variables
    float setpoint;
    float previous_error;
    float integral;
};

// Global vaariables for PID control
PID pitch_PID (4, 0.5 , 5);
PID roll_PID (4 , 0.5 , 5);



long gyroX, gyroY, gyroZ;
float gyroX_cal, gyroY_cal, gyroZ_cal; 
float accelX, accelY, accelZ ;
float angleX_cal, angleY_cal, angleZ_cal ;
float accelX_cal, accelY_cal, accelZ_cal ;
float angle_pitch, angle_roll;
float accel_pitch, accel_roll;


int servoXpos = 90;
int servoYpos = 90;
int count = 0;

MPU6050 sensor;


void setupMPU() {
  // Initialize MPU6050
  Wire.beginTransmission(0b1101000);
  Wire.write(0x6B); //leei to sygkekrimeno adress tou register pu thelume (power management settings)
  Wire.write(0b00000000); // Configure MPU6050
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1B); // register gia thn eyaisthisia toy gyroscopiou
  Wire.write(0x08); // Set gyro sensitivity to +/- 500 degrees/sec
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000);
  Wire.write(0x1C);//register gia eyaisthisia accelerometer
  Wire.write(0x10); // Set accelerometer sensitivity to +/- 8g
  Wire.endTransmission();
}

  void recordAccelRegisters() {
  Wire.beginTransmission(0x68); 
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) {
    Serial.println("Failed to communicate with MPU6050");
    return;
  }

  Wire.requestFrom(0x68, 6);
  unsigned long startTime = millis();
  while (Wire.available() < 6) {
    if (millis() - startTime > 1000) { // Timeout after 1000 ms
      Serial.println("Timeout reading accelerometer data");
      return;
    }
  }

  accelX = Wire.read() << 8 | Wire.read(); 
  accelY = Wire.read() << 8 | Wire.read(); 
  accelZ = Wire.read() << 8 | Wire.read(); 
}

void recordGyroRegisters() {
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  if (Wire.endTransmission(false) != 0) {
    Serial.println("Failed to communicate with MPU6050");
    return;
  }

  Wire.requestFrom(0x68, 6);
  unsigned long startTime = millis();
  while (Wire.available() < 6) {
    if (millis() - startTime > 1000) { // Timeout after 1000 ms
      Serial.println("Timeout reading gyro data");
      return;
    }
  }

  gyroX = Wire.read() << 8 | Wire.read();
  gyroY = Wire.read() << 8 | Wire.read();
  gyroZ = Wire.read() << 8 | Wire.read();
}

void calibrateSensors() {
  // Reset calibration values
  gyroX_cal = gyroY_cal = gyroZ_cal = 0;
  accelX_cal = accelY_cal = accelZ_cal = 0;

  Serial.println("Calibrating gyro...");
  for (int i = 0; i < 500; i++) {
    recordGyroRegisters();
    gyroX_cal += gyroX;
    gyroY_cal += gyroY;
    gyroZ_cal += gyroZ;
    delay(2); // Small delay to not overload the I2C bus
  }
  gyroX_cal /= 500;
  gyroY_cal /= 500;
  gyroZ_cal /= 500;

  Serial.println("Calibrating accelerometer...");
  for (int i = 0; i < 500; i++) {
    recordAccelRegisters();
    accelX_cal += accelX;
    accelY_cal += accelY;
    accelZ_cal += accelZ;
    delay(2); // Small delay to not overload the I2C bus
  }
  accelX_cal /= 500;
  accelY_cal /= 500;
  accelZ_cal /= 500;
}


void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.setClock(400000); // Set I2C frequency to 400kHz
  Wire.setWireTimeout(3000, true); // Set a timeout for I2C communication

  servox.attach(servox_pin);
  servoy.attach(servoy_pin);

    pitch_PID.setSetpoint(0);
  roll_PID.setSetpoint(0);


Serial.println("Initializing the sensor...");
  sensor.initialize();
  delay(100);
  if (sensor.testConnection()) {
    Serial.println("MPU6050 connection successful");
  } else {
    Serial.println("MPU6050 connection failed");
    while(1); // Stop the program if no connection
  }

  setupMPU();
    calibrateSensors(); // Function to calibrate sensors
}
  
void loop() {
float dt;
float previous_time = micros ();
  unsigned long current_time = micros();
  dt = (current_time - previous_time) / 1000000.0; // Convert microseconds to seconds
if (dt <= 0) {
  dt = 0.01; // Fallback to a safe default value (10ms)
}
  previous_time = current_time;


  recordAccelRegisters();
  recordGyroRegisters();



  gyroX -= gyroX_cal;
  gyroY -= gyroY_cal; 
  gyroZ -= gyroZ_cal;


  accelX -= accelX_cal;
  accelY -= accelY_cal; 
  accelZ -= accelZ_cal;

  angle_pitch += gyroX * 0.000122; //conversion factor. Oloklhrvnei to dω, metatrepei se Δθ, kai to prosthetei
  //sto angle pitch, etsi wste na jeroume kathe stigmh ti prosanatolismo exei. EINAI SYGKEKRIMENH THESH OXI ΔΙΑΣΤΗΜΑ
  //TO GYRO X DEN EINAI DEGRESS/SEC, ALLA ANTIPROSWPEYEI RAW VALUE!!
  angle_roll += gyroY * 0.000122;

  angle_pitch += angle_roll * sin(gyroZ * 0.000002131); //corrections factors epeidh peristrofh ston y k z axona
  //ephreasoun ton x ajona
  angle_roll -= angle_pitch * sin(gyroZ * 0.000002131);

   float accel_pitch = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / M_PI;
   float accel_roll = angle_pitch * sin(gyroZ * 0.000002131); // Apply correction for gyroZ to roll

  // Apply complementary filter to combine gyro and accelerometer data
  angle_pitch = 0.98 * (angle_pitch + accel_pitch) + 0.02 * accel_pitch; // Adjust alpha values as needed
  angle_roll = 0.98 * (angle_roll + accel_roll) + 0.02 * accel_roll; // Adjust alpha values as needed

  float controlledSignalPitch = pitch_PID.update(-controlledSignalPitch, dt)  * 2 ;
  float controlledSignalRoll = roll_PID.update(-controlledSignalPitch, dt)  * 2;
  
  // Implement deadband control
  float deadband = 2.0;  // Adjust as needed
  if (fabs(controlledSignalPitch) > deadband) {
      angle_pitch += controlledSignalPitch;
  }
  if (fabs(controlledSignalRoll) > deadband) {
      angle_roll += controlledSignalRoll;
  }

  Serial.print("Controlled Signal Pitch: ");
Serial.println(controlledSignalPitch);
Serial.print("Controlled Signal Roll: ");
Serial.println(controlledSignalRoll);


  angle_pitch += controlledSignalPitch;
  angle_roll += controlledSignalRoll;


  int servoXpos = map(angle_roll, 10.00, -10.00, 0, 180);
  int servoYpos = map(angle_pitch, -10.00, 10.00, 0, 180);

Serial.print("angle_pitch:");
Serial.println(angle_pitch);
Serial.print("angle_roll:");
Serial.println(angle_roll);


  servox.write(servoXpos);
  servoy.write(servoYpos);



}
2

Welcome to the forum

Please follow the advice given in the link below when posting code, in particular the section entitled 'Posting code and common code problems'

Use code tags (the < CODE/ > icon above the compose window) to make it easier to read and copy for examination

https://forum.arduino.cc/t/how-to-get-the-best-out-of-this-forum

Please post your sketch, using code tags when you do. This prevents parts of it being interpreted as HTML coding and makes it easier to copy for examination

In my experience the easiest way to tidy up the code and add the code tags is as follows
Start by tidying up your code by using Tools/Auto Format in the IDE to make it easier to read. Then use Edit/Copy for Forum and paste what was copied in a new reply. Code tags will have been added to the code to make it easy to read in the forum thus making it easier to provide help.

3

You never use loop_timer.

You should search and read about "pid tuning" to help stabilize the issue you describe when you stop moving the spoon.

Those servos must be tiny. Where do you find them?

1 Like
4

Thank you for your response! I found the smaller ones available but the spoon is still quite large, however it fits in the hand.
I read some things about the pid tuning and it seems to work better. However at times the servos abruptly stop moving. Also they still move uncontrollably if the mpu isnt moving.

5

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.