/*-----------------------------------------------------------------------------*/
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 flw_tsk_FeedForwardCntrl(){
pidReset(_fly.flyPID);
pidInit(_fly.flyPID, 0.6, 0.05, 0, 0, 999999);
int integralLimit;
if(_fly.flyPID.kI == 0){
integralLimit = 0;
} else{
integralLimit = 13.0 / _fly.flyPID.kI;
}
float output = 0;
float initTime = nPgmTime;
while(true){
fw_ButtonControl();
//
//we do not want to zero our error sum when we cross
if(abs(_fly.flyPID.errorSum) > integralLimit){
_fly.flyPID.errorSum = integralLimit * _fly.flyPID.errorSum/(abs(_fly.flyPID.errorSum));
}
_fly.currSpeed = FwCalculateSpeed();
float outVal = pidExecute(_fly.flyPID, _fly.setPoint - _fly.currSpeed);
float dTime = nPgmTime - initTime;
initTime = nPgmTime;
playTone(_fly.currSpeed/2,2);
output = _fly.pred + outVal;
if(output < 0){
output = 0;
}
if(output > 127){
output = 127;
}
if((_fly.currSpeed <= 50 && _fly.setPoint == 0) && vexRT[Btn7L] == 1){
output = -90;
_updateFlyWheel(output);
wait1Msec(4000);
_updateFlyWheel(0);
wait1Msec(10000);
}
_updateFlyWheel(output);
delay(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 );
}
}
