Hey guys I’ve been having problems with the PID on my conveyor. It moves fine when handled by an operator but when it is set to a pre-set value it starts to drift each time I try it. For instance if I started the conveyor at 0 and then set the conveyor input to -950 it moves to around -970, which is close enough. However if I move the conveyor back to 0 and then try the -950 pre-set again it goes to (-970)-(-1000) but in reality it’s moved down to -1100. If I keep going to 0 then -950 the keeps building, the optical shaft encoder thinks it is at -970 but really it just keeps going past increasing by -100 each time-ish. I tried going the other way (going to -1500 and going back to -950) and it does the same thing but increase by +100. I’m wondering if possibly we stripped the gears but I doubt it. Additionally I’m wondering if possibly the quad encoders are missing steps? I always start the conveyor in the same position every time. Here is the code.
#include "Main.h"
void OperatorControl ( unsigned long ulTime )
{
float error;
int btn;
int pot;
int btn2;
int input;
float output;
float dterm;
int lasterror;
int btn3;
int btn4;
long encoder;
float errorEncoder;
float dtermEncoder;
int lasterrorEncoder;
float outputEncoder;
int inputEncoder;
int btn5;
int btn6;
float x;
int preset;
InitLCD ( 1 ) ;
SetLCDLight ( 1 , 1 ) ;
StartQuadEncoder ( 1 , 2 , 0 ) ;
PresetQuadEncoder ( 1 , 2 , 0) ;
input = 250 ;
inputEncoder = 0 ;
while ( 1 ) // Insert Your RC Code Below
{
if ( (btn == 0) & (btn ==0) )
{
x = 3 ;
}
btn = GetJoystickDigital ( 2 , 6 , 2 ) ;
btn2 = GetJoystickDigital ( 2 , 6 , 1 ) ;
pot = GetAnalogInput ( 1 ) ;
if ( btn == 1 ) // Lift PID start
{
input = input + 4 ;
if ( input > 770 )
{
input = 770 ;
}
Wait ( 15 ) ;
}
else if ( btn2 == 1 )
{
input = input - 4 ;
if ( input < 250 )
{
input = 250 ;
}
Wait ( 15 ) ;
}
btn3 = GetJoystickDigital ( 2 , 8 , 3 ) ;
if ( btn3 == 1 )
{
input = 312 ;
inputEncoder = -950 ; // This is the line that sets the input on the conveyor to the pre-set
}
btn4 = GetJoystickDigital ( 2 , 8 , 1 ) ;
if ( btn4 == 1 )
{
input = 250 ;
}
error = (input - pot) ;
dterm = error - lasterror ;
lasterror = error ;
output = (0.6 * error) + (0.4 * dterm) ; // Output Lift
if ( output > 127 )
{
output = 127 ;
}
else if ( output < -127 )
{
output = -127 ;
}
SetMotor ( 8 , -output ) ;
SetMotor ( 9 , output ) ;
SetMotor ( 4 , output ) ;
SetMotor ( 5 , -output ) ; // Lift PID end
encoder = GetQuadEncoder ( 1 , 2 ) ; // Encoder PID start
btn5 = GetJoystickDigital ( 2 , 5 , 2 ) ;
btn6 = GetJoystickDigital ( 2 , 5 , 1 ) ;
if ( btn5 == 1 )
{
inputEncoder = inputEncoder + 4 ;
Wait ( 5 ) ;
}
else if ( btn6 == 1 )
{
inputEncoder = inputEncoder - 4 ;
Wait ( 5 ) ;
}
errorEncoder = (inputEncoder - encoder) ;
dtermEncoder = errorEncoder - lasterrorEncoder ;
lasterrorEncoder = errorEncoder ;
outputEncoder = (0.4 * errorEncoder) + (0.6 * dtermEncoder) ; // Output Encoder
if ( outputEncoder > 127 )
{
outputEncoder = 127 ;
}
else if ( outputEncoder < -127 )
{
outputEncoder = -127 ;
}
SetMotor ( 6 , -outputEncoder ) ; // Encoder PID end
Arcade4 ( 1 , 3 , 1 , 3 , 7 , 1 , 10 , 0 , 0 , 1 , 0 ) ;
JoystickToMotor ( 1 , 4 , 2 , 0 ) ;
Wait ( 1 ) ;
}
}