/*-----------------------------------------------------------------------------*/
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 );
}
}
void shooterPowerDown () {
stopTask(feederWait);
stopTask(shooter);
stopTask(shooterDJ);
motor[feeder]=0;
int motorSpeed = motor[LUflywheel];
while(motorSpeed>0){
motorSpeed-=2;
FwMotorSet(motorSpeed);
wait1Msec(50);
}
}
task shooter () {
bool canRunAgain = true;
while (true)
{
if(vexRT(Btn7U))
speed++;
if(vexRT(Btn7D))
speed--;
FwMotorSet(speed);
//sprintf( str, "%4d %4d", target_velocity, motor_velocity, nImmediateBatteryLevel/1000.0 );
wait1Msec(200);
}
}
task shooterDJ () {
startTask(feederWait);
int timesFed = 0;
bool canRunAgain = true;
while (true)
{
if(vexRT(Btn7U))
speed++;
if(vexRT(Btn7D))
speed--;
if(SensorValue[ballHigh]&&canRunAgain) {
timesFed++;
speed+=15;//7;
}
if(timesFed>0 & canRunAgain)
canRunAgain = false;
FwMotorSet(speed);
//sprintf( str, "%4d %4d", target_velocity, motor_velocity, nImmediateBatteryLevel/1000.0 );
wait1Msec(200);
}
}
/*-----------------------------------------------------------------------------*/
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 );
}
}
