short
PidControllerUpdate( pidController *p, long sensor_value )
{
if( p == NULL )
return(0);
if( p->enabled )
{
p->sensor_value = sensor_value;
p->error = p->target_value - p->sensor_value;
// force error to 0 if below threshold
if( abs(p->error) < p->error_threshold )
p->error = 0;
// integral accumulation
if( p->Ki != 0 )
{
p->integral += p->error;
// limit to avoid windup
if( abs( p->integral ) > p->integral_limit )
p->integral = sgn(p->integral) * p->integral_limit;
}
else
p->integral = 0;
// derivative
p->derivative = p->error - p->last_error;
p->last_error = p->error;
// calculate drive - no delta T in this version
p->drive = (p->Kp * p->error) + (p->Ki * p->integral) + (p->Kd * p->derivative) + p->Kbias;
// drive should be in the range +/- 1.0
if( abs( p->drive ) > 1.0 )
p->drive = sgn(p->drive);
// final motor output
p->drive_raw = p->drive * 127.0;
}
else
{
// Disabled - all 0
p->error = 0;
p->last_error = 0;
p->integral = 0;
p->derivative = 0;
p->drive = 0.0;
p->drive_raw = 0;
}
// linearize - be careful this is a macro
p->drive_cmd = _LinearizeDrive( p->drive_raw );
// return the thing we are really interested in
return( p->drive_cmd );
}
int16_t
PidControllerUpdate( pidController *p )
{
if( p == NULL )
return(0);
if( p->enabled )
{
// check for sensor port
// otherwise externally calculated error
if( p->sensor_port >= 0 )
{
// Get raw position value, may be pot or encoder
p->sensor_value = vexSensorValueGet( p->sensor_port );
// A reversed sensor ?
if( p->sensor_reverse )
{
if( vexSensorIsAnalog( p->sensor_port) )
// reverse pot
p->sensor_value = 4095 - p->sensor_value;
else
// reverse encoder
p->sensor_value = -p->sensor_value;
}
p->error = p->target_value - p->sensor_value;
}
// force error to 0 if below threshold
if( fabs(p->error) < p->error_threshold )
p->error = 0;
// integral accumulation
if( p->Ki != 0 )
{
p->integral += p->error;
// limit to avoid windup
if( fabs( p->integral ) > p->integral_limit )
p->integral = sgn(p->integral) * p->integral_limit;
}
else
p->integral = 0;
// derivative
p->derivative = p->error - p->last_error;
p->last_error = p->error;
// calculate drive - no delta T in this version
p->drive = (p->Kp * p->error) + (p->Ki * p->integral) + (p->Kd * p->derivative) + p->Kbias;
// drive should be in the range +/- 1.0
if( fabs( p->drive ) > 1.0 )
p->drive = sgn(p->drive);
// final motor output
p->drive_raw = p->drive * 127.0;
}
else
{
// Disabled - all 0
p->error = 0;
p->last_error = 0;
p->integral = 0;
p->derivative = 0;
p->drive = 0.0;
p->drive_raw = 0;
}
// linearize - be careful this is a macro
p->drive_cmd = _LinearizeDrive( p->drive_raw );
// return the thing we are really interested in
return( p->drive_cmd );
}