Hey guys,
i´ve been building an inverted pendulum out of an old printer. Im using Quadrature Encoders for the angle of pendulum and the linear position of the cart.
Right now im at the point where I got the system set up so that it will balance the pendulum using the PID library. The PID Controller is solely looking at the angle of the pendulum and does not use the linear position signal in any way. I haven´t implemented an automated swing up and dont´t intend to do so far.
I´d now like to find a way to balance the pendulum at a certain linear position along the rail.
Besides it would be just cool to do that I actually need that feature for stable system. What happens at the moment is that the pendulum is stabilized upright but the cart is still moving eventually hitting the end of travel. The cart will move with a constant speed everlasting trying to catch up with the pendulum that still has some residue velocity at the top left and try´s to tip over therefore.
Because it´s just a desktop printer i run out of travel very fast so though the pendulum is stabilized it will only work for about two seconds before in runs into the end of travel.
I need some way to control the carts position along with the angle. That seems to be tricky because I need control two variablses with just one input(motor) into the system. Furthermore the linear control would basically need overshoot first to get the cart to reverse its direction. That is somewhat of the contrary of what the "angle"-controller is trying to do.
Can anyone give me some clues how to realize a linear control of the system?
Iv´e been googeling like crazy but can only find academic papers that are just beyond me.
Thanks in advance guys.
Kevin
The Code for anyone whos interested.
It´s not particularly pretty I must admit.
#include <PID_v1.h>
#include <Encoder.h>
//max value for linear encoder until it hits physical stop Encoder count =6716
byte pin_photoint=13;
byte pin_pwm=6;
byte pin_left=7; //Left and right might change depending how you connected the dc motor. CHECK THIS!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
byte pin_right=8;
byte pin_lin_enc1=2;
byte pin_lin_enc2=3;
byte pin_rot_enc1=18;
byte pin_rot_enc2=19;
byte setup_speed=105;
byte rot_deadband=1; //In counts in each direction
int limit_lin_min=500; //min Limit (in direction torwards limit switch--> right)
int limit_lin_max=6200; //max Limit (left)
double limit_angle_left=4000;
double limit_angle_right=3200;
double angle;
double lin_position;
Encoder linEnc(pin_lin_enc1,pin_lin_enc2); //Setup of linear Encoder, overall length about 6700 counts
Encoder rotEnc(pin_rot_enc1,pin_rot_enc2); //Setup dof rotary Encoder, one revolution=7200 counts
//PID Variables
double Output, rot_Setpoint, lin_Setpoint;
double kp=5, ki=0.0, kd=0.01;
PID Controller(&angle,&Output,&rot_Setpoint,kp,ki,kd,DIRECT);
void setup() {
Serial.begin(115200);
pinMode(pin_photoint,INPUT);
linEnc.write(10000); //Choose so that linEnc.read() cant become 0 Zero on intitial reference run, that means bigger than maximum number of possible steps in whole travel
rotEnc.write(0); //when pendulum ist hanging straight down due to gravity
delay(500);
//set Setpoint --> offset by 180 degrees (3600 counts)
rot_Setpoint=3600;
//give it a little bump to beginn cause setup_speed is enough power to maintain the movement but not to start it initially
go_right(255);
delay(10);
//Driving towards the limit switch to reference linear position
while (linEnc.read()!=0){
go_right(setup_speed);
byte photoint=digitalRead(pin_photoint);
if (photoint==0){
break;
}
}
go_stop();
delay(300);
linEnc.write(0);
//Drive to the middel of the travelrange
//little bump to start with
go_left(255);
delay(10);
while (linEnc.read()<3000){ //3000 ist about the middle
go_left(setup_speed);
}
go_stop();
delay(500);
//PID Setup
Controller.SetMode(AUTOMATIC);
Controller.SetOutputLimits(-255,255);
Controller.SetSampleTime(1);
}
void loop() {
// put your main code here, to run repeatedly:
angle=rotEnc.read();
lin_position=linEnc.read();
Controller.Compute();
/*if(millis()>5000){
Controller.SetTunings(kp, 150, kd);
Serial.println("I active");
}*/
/*Serial.print("angle=");
Serial.print(rot_Setpoint-angle);
Serial.print("\t");
Serial.print("Output=");
Serial.println(Output);
*/
//Serial.println(read_angle());
if (lin_position>limit_lin_min &&lin_position<limit_lin_max){ //perform as long carriage is within the linear limits
if (angle<limit_angle_left &&angle>limit_angle_right){ //perform as long as its at least reasonably upright, no point in in trying to balance it when its already hanging down...
//actual controller part
//Serial.println(Output);
if(Output<0){
go_left(map(abs(Output),0,255,setup_speed-10,255));
//go_left(abs(Output));
//Serial.print("L");
//Serial.print("\t");
//Serial.println(map(abs(Output),0,255,setup_speed-10,255));
}
if(Output>0){
go_right(map(abs(Output),0,255,setup_speed-20,255));
//go_right(abs(Output));
//Serial.print("R");
//Serial.print("\t");
//Serial.println(map(abs(Output),0,255,setup_speed-20,255));
}
if(Output==0){
go_stop;
}
}
else{
go_stop(); //stop carrigage if travel limit is hit
}
}
else{ //drive back into linear limits when they were exeeded
go_stop(); //stop carriage
delay(600);
if(linEnc.read()<limit_lin_min){ //if outside of right limit
while(linEnc.read()<limit_lin_min+100){ //drive it back into the travel range, just a little bit over the actual limit
go_left(setup_speed+10);
}
go_stop();
}
if(linEnc.read()>limit_lin_max){ //if outside of left limit
while(linEnc.read()>limit_lin_max-100){ //drive it back into the travel range, just a little bit over the actual limit
go_right(setup_speed+10);
}
go_stop();
}
}
}
void go_left(int velocity){ //go left with 'velocity'
digitalWrite(pin_right,LOW);
digitalWrite(pin_left,HIGH);
analogWrite(pin_pwm,velocity);
}
void go_right(int velocity){ //go right with 'velocity'
digitalWrite(pin_right,HIGH);
digitalWrite(pin_left,LOW);
analogWrite(pin_pwm,velocity);
}
void go_stop(){ //stop carriage
analogWrite(pin_pwm,0);
}
