Dear All, I need help with a little project of mine that supposes to level/trim a rc boat to the proper/desired waterline using two installed ballast tanks, one in stern and on the bow section.
The pump moves water from one syringe to another until it reaches 0 degrees setpoint, using PID.
For this purpose I have create a prototype using the following components:
- Peristaltic pump - Kamoer 6V, 5W;
- Adafruit DRV8871;
- IMU 6050;
- Battery 2xLion 18650 3,7V
- Arduino UNO R3;
- Lego.
Picture here:
This project serves as a base/playground for further development of my RC submarine that will use 2 gear pumps and drives 2 pistons used for both diving and trimming.
Code:
// Include Wire Library for I2C
#include <Wire.h>
#include <PID_v1.h>
#include <MPU6050_tockn.h>
MPU6050 mpu6050(Wire);
//Declare motor direction pins
#define MotorPowerForward 5
#define MotorPowerBackward 6
//declare direction variable for motor function
int dir;
//the angle where the boat is level
double Setpoint = 0;
double Input, Output;
//PID controllers
double Kp = 10;
double Kd = 15;
double Ki = 0;
//instance of class PID
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVERSE);
double angle_pitch_output;
void setup() {
//setting the motors pinmodes
pinMode(MotorPowerForward, OUTPUT);
pinMode(MotorPowerBackward, OUTPUT);
//Starts with motors off
digitalWrite(MotorPowerForward, LOW);
digitalWrite(MotorPowerBackward, LOW);
//setting PID parameters
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-255, 255);
myPID.SetSampleTime(20);
Wire.begin();
mpu6050.begin();
mpu6050.calcGyroOffsets(true);
// Start Serial Monitor
Serial.begin(9600);
}
void loop(){
mpu6050.update();
angle_pitch_output=mpu6050.getAngleX();
Input = angle_pitch_output;
myPID.Compute();
if (Output > 0) {
dir = 1;
setMotor(dir,abs(Output),MotorPowerForward,MotorPowerBackward);
}
else if (Output < 0) {
dir = -1;
setMotor(dir,abs(Output),MotorPowerForward,MotorPowerBackward);
}
//Serial.print("Angle:");
//Serial.println(angle_pitch_output);
Serial.print("\tPID_Input:");
Serial.print(Input);
Serial.print("\tPID_Setpoint:");
Serial.print(Setpoint);
Serial.print("\tPID_Output:");
Serial.print(Output);
Serial.println();
}//void loop
//Function for running the motor/pump
void setMotor(int dir, int pwmVal, int in1, int in2){
if (pwmVal<128){
pwmVal=127;
}
if(dir == 1){
analogWrite(in1, pwmVal);
analogWrite(in2, LOW);
}
else {
analogWrite(in1, LOW);
analogWrite(in2, pwmVal);
}
}
Symptoms: When reaching the setpoint, the pump never stops and hunts for target position.
Video here: Pump hunting for target position.
It was a challenge to find the kp,kd and ki constants, maybe an autopid library will help but I find it difficult to grasp. Why autopid? The prototype now run in the air but finally it will run in water, different medium all together, I guess the constants will need to be adjusted again.
Please help me improve my code and reach target/setpoint but in the same time stop the pump and save battery. Some say that I need to add deadband to the code but I do not know where and how.
Thank you.
Bogdan
