/*-----------------------------------------------------------------------------*/
task FwControlTask()
{
// Set the gain
gain = 0.00025;
// We are using Speed geared motors
// Set the encoder ticks per revolution
ticks_per_rev = MOTOR_TPR_393T;
while(1)
{
// Calculate velocity
FwCalculateSpeed();
// Do the velocity TBH calculations
FwControlUpdateVelocityTbh() ;
// Scale drive into the range the motors need
motor_drive = (driver * FW_MAX_POWER) + 0.5;
// Final Limit of motor values - don't really need this
if( motor_drive > 127 ) motor_drive = 127;
if( motor_drive < -127 ) motor_drive = -127;
// and finally set the motor control value
FwMotorSet( motor_drive );
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}
/*-----------------------------------------------------------------------------*/
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
FwControlTask()
{
fw_controller *fw = &flywheel;
// Set the gain
fw->gain = 0.00025;
// We are using Speed geared motors
// Set the encoder ticks per revolution
fw->ticks_per_rev = MOTOR_TPR_393S;
while(1)
{
// debug counter
fw->counter++;
// Calculate velocity
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.55; // Orig: 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
FwMotorSet( fw->motor_drive );
// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}
/*-----------------------------------------------------------------------------*/
void FwControlTask(fw_parameters FW, int e_count){
// Set the gain
FW.gain = 0.00025;
// We are using Speed geared motors
// Set the encoder ticks per revolution
FW.ticks_per_rev = MOTOR_TPR_393S;
while(1){
// debug counter
FW.counter++;
// Calculate velocity
FwCalculateSpeed(FW, e_count);
// 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
FwMotorSet(FW.motor_drive );
// 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 );
}
}
