/*-----------------------------------------------------------------------------*/
void FwCalculateSpeed()
{
int delta_ms;
int delta_enc;
// Get current encoder value
encoder_counts = FwMotorEncoderGet();
// This is just used so we don't need to know how often we are called
// how many mS since we were last here
delta_ms = nSysTime - nSysTime_last;
nSysTime_last = nSysTime;
// Change in encoder count
delta_enc = (encoder_counts - encoder_counts_last);
// save last position
encoder_counts_last = encoder_counts;
// Calculate velocity in rpm
motor_velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;
}
/*-----------------------------------------------------------------------------*/
void FwCalculateSpeed( fw_controller *fw )
{
int delta_ms;
int delta_enc;
// Get current encoder value
fw->e_current = FwMotorEncoderGet();
// This is just used so we don't need to know how often we are called
// how many mS since we were last here
delta_ms = nSysTime - fw->v_time;
fw->v_time = nSysTime;
// Change in encoder count
delta_enc = (fw->e_current - fw->e_last);
// save last position
fw->e_last = fw->e_current;
// Calculate velocity in rpm
fw->v_current = (1000.0 / delta_ms) * delta_enc * 60.0 / fw->ticks_per_rev;
}
