pidController *
PidControllerInit( float Kp, float Ki, float Kd, tSensors port, short sensor_reverse = 0 )
{
pidController *p;
if( nextPidControllerPtr == MAX_PID ) {
return(NULL);
}
p = (pidController *)&_pidControllers[ nextPidControllerPtr++ ];
// pid constants
p->Kp = Kp;
p->Ki = Ki;
p->Kd = Kd;
p->Kbias = 0.0;
// zero out working variables
p->error = 0;
p->last_error = 0;
p->integral = 0;
p->derivative = 0;
p->drive = 0.0;
p->drive_cmd = 0;
if(Ki != 0)
p->integral_limit = (PIDLIB_INTEGRAL_DRIVE_MAX / Ki);
else
p->integral_limit = 0;
p->error_threshold = 10;
// sensor port
p->sensor_port = port;
p->sensor_reverse = sensor_reverse;
p->sensor_type = SensorType[ port ];
p->sensor_value = 0;
p->target_value = 0;
// We need a valid sensor for pid control, pot or encoder
if( ( p->sensor_type == sensorPotentiometer ) ||
( p->sensor_type == sensorQuadEncoder ) ||
( p->sensor_type == sensorQuadEncoderOnI2CPort ) ||
( p->sensor_type == sensorGyro )
)
p->enabled = 1;
else
p->enabled = 0;
PidControllerMakeLut();
return(p);
}
pidController *
PidControllerInit( float Kp, float Ki, float Kd, tVexSensors port, int16_t sensor_reverse )
{
pidController *p;
if( nextPidControllerPtr == MAX_PID )
return(NULL);
#ifndef PIDLIB_USE_DYNAMIC
p = (pidController *)&_pidControllers[ nextPidControllerPtr++ ];
#else
p = chHeapAlloc( NULL, sizeof( pidController ) );
#endif
// pid constants
p->Kp = Kp;
p->Ki = Ki;
p->Kd = Kd;
p->Kbias = 0.0;
// zero out working variables
p->error = 0;
p->last_error = 0;
p->integral = 0;
p->derivative = 0;
p->drive = 0.0;
p->drive_cmd = 0;
if(Ki != 0)
p->integral_limit = (PIDLIB_INTEGRAL_DRIVE_MAX / Ki);
else
p->integral_limit = 0;
p->error_threshold = 10;
// sensor port
p->sensor_port = port;
p->sensor_reverse = sensor_reverse;
p->sensor_value = 0;
p->target_value = 0;
// We need a valid sensor for pid control, pot or encoder
// different sensor use in ConVEX, always enable for now
if( 1 )
p->enabled = 1;
else
p->enabled = 0;
PidControllerMakeLut();
return(p);
}
