/*
* Steps the controller's velocity calculation (separate from the main step function)
* Can be used to maintain velocity calculation when a full on math step isn't wanted
*/
int bangBang_StepVelocity(bangBang *bb)
{
if (bb->usingIME)
{
//Calculate timestep
getEncoderAndTimeStamp(bb->imeMotor, bb->currentPosition, bb->currentTime);
bb->dt = bb->currentTime - bb->prevTime;
bb->prevTime = bb->currentTime;
//Scrap dt if zero
if (bb->dt == 0)
{
return 0;
}
//Calculate current velcoity
bb->currentVelocity = (1000.0 / bb->dt) * (bb->currentPosition - bb->prevPosition) * 60.0 / bb->ticksPerRev;
bb->prevPosition = bb->currentPosition;
}
else if (bb->usingVar)
{
//Calculate timestep
bb->dt = nSysTime - bb->prevTime;
bb->prevTime = nSysTime;
//Scrap dt if zero
if (bb->dt == 0)
{
return 0;
}
//Calculate current velocity
bb->currentVelocity = (1000.0 / bb->dt) * (*(bb->var) - bb->prevPosition) * 60.0 / bb->ticksPerRev;
bb->prevPosition = *(bb->var);
}
else
{
//Calculate timestep
bb->dt = nSysTime - bb->prevTime;
bb->prevTime = nSysTime;
//Scrap dt if zero
if (bb->dt == 0)
{
return 0;
}
//Calculate current velocity
bb->currentVelocity = (1000.0 / bb->dt) * (SensorValue[bb->sensor] - bb->prevPosition) * 60.0 / bb->ticksPerRev;
bb->prevPosition = SensorValue[bb->sensor];
}
//Use a DEMA filter to smooth data
bb->currentVelocity = filter_DEMA(&(bb->filter), bb->currentVelocity, 0.19, 0.0526);
return bb->currentVelocity;
}
/*-----------------------------------------------------------------------------*/
task leftFwControlTask()
{
fw_controller *fw = lFly;
// We are using Speed geared motors
// Set the encoder ticks per revolution
fw->ticks_per_rev = fw->MOTOR_TPR;
while(1)
{
// debug counter
fw->counter++;
// Calculate velocity
getEncoderAndTimeStamp(lFlyBottom,fw->e_current, fw->encoder_timestamp);
FwCalculateSpeed(fw);
// Set current speed for the tbh calculation code
fw->current = fw->v_current;
// Do the velocity TBH calculations
FwControlUpdateVelocityTbh( fw ) ;
// Scale drive into the range the motors need
fw->motor_drive = fw->drive * (FW_MAX_POWER/127);
// Final Limit of motor values - don't really need this
if( fw->motor_drive > 127 ) fw->motor_drive = 127;
if( fw->motor_drive < -127 ) fw->motor_drive = -127;
// and finally set the motor control value
//if(fw->current < fw->target - 20) {
// setRightFwSpeed( 70 );
//} else
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
setLeftFwSpeed(fw->motor_drive);
str = sprintf( str, "%4d %4d %5.2f", fw->target, fw->current, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
str = sprintf( str, "%4.2f %4.2f ", fw->drive, fw->drive_at_zero );
displayLCDString(1, 0, str );
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}
/*-----------------------------------------------------------------------------*/
task rightFwControlTask()
{
fw_controller *fw = rFly;
// We are using Speed geared motors
// Set the encoder ticks per revolution
fw->ticks_per_rev = fw->MOTOR_TPR;
while(1)
{
// debug counter
fw->counter++;
// Calculate velocity
getEncoderAndTimeStamp(flyWheelR2,fw->e_current, fw->encoder_timestamp);
FwCalculateSpeed(fw);
// Set current speed for the tbh calculation code
fw->current = fw->v_current;
// Do the velocity TBH calculations
FwControlUpdateVelocityTbh( fw ) ;
// Scale drive into the range the motors need
fw->motor_drive = (fw->drive * FW_MAX_POWER) + 0.5;
// Final Limit of motor values - don't really need this
if( fw->motor_drive > 127 ) fw->motor_drive = 127;
if( fw->motor_drive < -127 ) fw->motor_drive = -127;
// and finally set the motor control value
setRFly( fw->motor_drive );
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}
int pos_PID_StepController(pos_PID *pid)
{
//Calculate error
if (pid->usingIME)
{
//Calculate timestep
getEncoderAndTimeStamp(pid->imeMotor, pid->currentPos, pid->currentTime);
pid->dt = (pid->currentTime - pid->prevTime) / 1000.0;
pid->prevTime = pid->currentTime;
//Scrap dt if zero
if (pid->dt == 0)
{
return 0;
}
pid->error = pid->targetPos - pid->currentPos;
}
else if (pid->usingVar)
{
//Calculate timestep
pid->dt = (nSysTime - pid->prevTime) / 1000.0;
pid->prevTime = nSysTime;
//Scrap dt if zero
if (pid->dt == 0)
{
return 0;
}
pid->currentPos = *(pid->var);
pid->error = pid->targetPos - pid->currentPos;
}
else
{
//Calculate timestep
pid->dt = (nSysTime - pid->prevTime) / 1000.0;
pid->prevTime = nSysTime;
//Scrap dt if zero
if (pid->dt == 0)
{
return 0;
}
pid->currentPos = SensorValue[pid->sensor];
pid->error = pid->targetPos - pid->currentPos;
}
//If error is large enough, calculate integral and limit to avoid windup
if (abs(pid->error) > pid->errorThreshold && abs(pid->integral) < pid->integralLimit)
{
pid->integral = pid->integral + pid->error * pid->dt;
//Reset integral if reached target or overshot
if (pid->error == 0 || sgn(pid->error) != sgn(pid->prevError))
{
pid->integral = 0;
}
//Bound integral
else
{
pid->integral = pid->integral * pid->kI > 127 ? 127.0 / pid->kI : pid->integral;
pid->integral = pid->integral * pid->kI < -127 ? -127.0 / pid->kI : pid->integral;
}
}
//Calculate derivative
pid->derivative = (pid->error - pid->prevError) / pid->dt;
pid->prevError = pid->error;
//Calculate output
pid->outVal = (pid->error * pid->kP) + (pid->integral * pid->kI) + (pid->derivative * pid->kD) + pid->kBias;
return pid->outVal;
}
