Hello everyone,
I'm working on a small engineering project which consist on reducing the roll effect on boats. My system is based on the displacement of a weight to conteract the sea swell ( you can look at the photos attach to see the prototype that i have built).
The system is composed of :
- Arduino nano
- Mpu6050
- L298n
- DC Motor
- 1/16 Gearbox
- Motor
- Pulley and belt
- Two end switch
My first objective is simply to obtain a system that when disturb stabilizes itself faster around it's zero angle then with no enslavement
I have implement a PID controller in my code but my main issue is that i'm unable to determin the correct gain values for the PID. I have tried the famous Ziegler nichols method but with no success, i'm in fact able to obtain oscillations at a certain gain and the oscillation period but the differents value i obtain make the system unstable.
Here is a extract of the code that i've written (the entire code is in the files attach)
#include <Wire.h>
//////// Driver Variables ///////
const int enb = 9 ; // enb is the pwm control pin
const int in4 = 8 ; // Pin to control motor direction and brake (HIGH or LOW)
const int in3 = 7 ; // Pin to control motor direction and brake (HIGH or LOW)
////// Button variables ///////
const int buttonPing = 4; // Left end switch
const int buttonPind = 5; // Right end switch
int buttonStateg = 0;
int buttonStated = 0;
///// counter variables///////
#define STD_LOOP_TIME 10
int timeGoneBy = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
///// Variables PID/////
float Kp = 1; // (P)roportional Tuning Parameter
float Ki = 1; // (I)ntegral Tuning Parameter
float Kd = 1; // (D)erivative Tuning Parameter
float lastpitch; // Keeps track of error over time
float iTerm; // Used to accumalate error (intergral)
float targetAngle = 0; // desired angle
double thisTime = 0;
double lastTime = 0;
int PWM_sortie = 0; // PWM output signal
/*______________________________________________________________________________________________________________________________________________*/
void setup() {
///// Calibration of gyro/////
Wire.begin(); //Start I2C as master
Serial.begin(57600); //Use only for debugging
pinMode(13, OUTPUT); //Set output 13 (LED) as output
pinMode(buttonPing, INPUT);
pinMode(buttonPind, INPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
pinMode(enb,OUTPUT);
TCCR1B = TCCR1B & B11111000 | B00000100; // set timer 1 divisor to 256 for PWM frequency of 122.55 Hz
Serial.println("Debut de la Calibration");
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050 (500dfs / +/-4g) and start the gyro
calibration_gyro(); // calibrate the Gyro
Serial.println("Fin de la Calibration");
loop_timer = micros(); //Reset the loop timer
}
/*______________________________________________________________________________________________________________________________________________________*/
void loop(){
buttonStateg = digitalRead(buttonPing);
buttonStated = digitalRead(buttonPind);
lecture_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
retour_angle_pitch(); // return the angle_pitch
Serial.println(angle_pitch);
PWM_sortie = PID(angle_pitch); //PID function return the PWM output signal
if (buttonStated == HIGH && PWM_sortie > 0 ){ // Stops the motor while the right button is pressed and waits till the pwm output become negative
digitalWrite(in4,LOW);
digitalWrite(in3,LOW);
analogWrite(enb,0);
}
else if (buttonStateg == HIGH && PWM_sortie < 0){ // Stops the motor while the left button is pressed and waits till the pwm output becomes positive
digitalWrite(in4,LOW);
digitalWrite(in3,LOW);
analogWrite(enb,0);
}
else{
if(PWM_sortie < 0){ // If the PWM output is inferior to 0 move the weight in a direction
digitalWrite(in4,HIGH);
digitalWrite(in3,LOW);
analogWrite(enb,abs(PWM_sortie));
}
if(PWM_sortie > 0){ // If the PWM output is above 0 move the weight in the other direction
digitalWrite(in4,LOW);
digitalWrite(in3,HIGH);
analogWrite(enb,abs(PWM_sortie));
}
}
clock_counter();// function for a 10 ms loop
}
///////////////////////////////////////////////////////////////////////////////////
////////////////////////////// Correcteur PID//////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
int PID(float pitch) {
thisTime = millis(); // Calculate time since last time PID was called (~10ms)
double timeChange = double(thisTime - lastTime);
float error = targetAngle - pitch; // Calculate Error
float pTerm = Kp * error; // Calculate our PID terms
iTerm += Ki * error * timeChange;
if(iTerm > 255) iTerm = 255; //Limit the Integrale to 255
else if(iTerm < -255)iTerm = -255;
float dTerm = Kd * (pitch - lastpitch) / timeChange;
lastpitch = pitch;
lastTime = thisTime;
float PIDValue = pTerm + iTerm - dTerm; // Set PWM Value
if (PIDValue > 255) PIDValue = 255; // Limits PID to max motor speed
else if (PIDValue < -255) PIDValue = -255;
return int(PIDValue); // Return PID Output
}
///////////////////////////////////////////////////////////////////////////////////
////////////////////////////// counter clock ~100Hz ///////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void clock_counter() {
timeGoneBy = millis() - loopStartTime; // Calculate time since loop began
if (timeGoneBy < STD_LOOP_TIME) {
delay(STD_LOOP_TIME - timeGoneBy);
}
// Update loop timer variables
loopStartTime = millis();
}
My problem might come from the code itself, i'm open to any fix, suggestions or informations. I also wondering if the Ziegler Nichols method is suited for this type of system.
Here is a photo of the setup:
Thank for your help and time.
Code_int_grale.ino (13.8 KB)
