void
Elevator::PeriodicService()
{
// For now, add a test for NULL for our motors. Until they are connected to to the bot,
// we can't do anything
if ((motorA == NULL) || (motorB == NULL)) return;
CheckJoystick();
if(isHoming)
{
if(motorA->GetReverseLimitOK())
{
motorA->Set(-maxSpeed/2); //both move down at half speed, can change to accomodate for others
motorB->Set(-maxSpeed/2);
return;
}
isHoming = false;
motorA->Set(0);
motorB->Set(0);
motorA->ResetEncoder();
targetPosition = 0;
}
float positionNow = GetCurrentPositionFeet();
float err = (targetPosition - positionNow);
float output = err*Kp;
if(IsAtTargetPosition()) //brakes if at the target position
SetBrake(true);
else
SetBrake(false);
if(output > maxSpeed) //makes sure the elevator doesn't exceed a set max speed
output = maxSpeed;
if(output < -maxSpeed)
output = -maxSpeed;
motorA->Set(output);
motorB->Set(output);
}
__interrupt void Timer_A (void)
{
ms_ticks++;
//for (index = 0; index < NUM_TIMERS; index++)
//{
if (timers[0].Enabled == 1)
{
if (timers[0].NextTime == ms_ticks)
{
timers[0].Flag = 1;
timers[0].Enabled = 0;
}
}
if (timers[1].Enabled == 1)
{
if (timers[1].NextTime == ms_ticks)
{
timers[1].Flag = 1;
timers[1].Enabled = 0;
}
}
if (timers[2].Enabled == 1)
{
if (timers[2].NextTime == ms_ticks)
{
timers[2].Flag = 1;
timers[2].Enabled = 0;
}
}
if (timers[3].Enabled == 1)
{
if (timers[3].NextTime == ms_ticks)
{
timers[3].Flag = 1;
timers[3].Enabled = 0;
}
}
if (timers[4].Enabled == 1)
{
if (timers[4].NextTime == ms_ticks)
{
timers[4].Flag = 1;
timers[4].Enabled = 0;
}
}
if (timers[5].Enabled == 1)
{
if (timers[5].NextTime == ms_ticks)
{
timers[5].Flag = 1;
timers[5].Enabled = 0;
}
}
if (timers[6].Enabled == 1)
{
if (timers[6].NextTime == ms_ticks)
{
timers[6].Flag = 1;
timers[6].Enabled = 0;
}
}
if (sound.duration.Enabled == 1) //Sound timer is on
{
if (sound.duration.NextTime == ms_ticks) //Sound has ended
{
sound.duration.Flag = 1;
sound.duration.Enabled = 0;
sound.frequency.Enabled = 0;
P2OUT &= ~SPEAKER_PORT;
if (soundIndex < soundLength) //Playing a continuous sound
{
soundIndex++;
StartSound(frequencies[soundIndex], durations[soundIndex]);
}
}
else
{
if (sound.frequency.Enabled == 1)
{
if (sound.frequency.NextTime == ms_ticks) //Time to toggle sound
{
P2OUT ^= SPEAKER_PORT;
sound.frequency.NextTime = sound.frequency.Time + ms_ticks;
}
}
}
}
CheckJoystick();
}
void
Arm::PeriodicService()
{
if (motorA == NULL) return;
CheckJoystick();
CheckManualControl();
switch(controlMode)
{
// printf("Arm ControlMode = %d\n", controlMode);
case HomingMode:
SetMotors(-maxSpeed/2); //both move down at half speed, can change to accomodate for others
if(IsDownLimitSwitchActive())
{
angleOfArm = 90;
motorA->Set(0);
//zeroAngleVolts = magEncoder->GetVoltage();
controlMode = ManualMode;
}
break;
case ReleasePositionMode:
{
if(armTimer->Get() >= timeToGoToReleaseAngle)
{
SetMotors(0);
angleOfArm = ReleaseAngle;
controlMode = ManualMode;
}
break;
}
case ManualMode:
break;
case ToUpLimitSwitchMode:
{
if(IsUpLimitSwitchActive())
{
SetMotors(0);
angleOfArm = 0;
controlMode = ManualMode;
}
break;
}
case ToDownLimitSwitchMode:
{
if(IsDownLimitSwitchActive())
{
SetMotors(0);
angleOfArm = 90;
controlMode = ManualMode;
}
break;
}
default:
break;
}
}