void ResetArmEncoders() { //asdfadsf
SetArm(-75);
while(GetDigitalInput(armResetSwitch)){
}
PresetIntegratedMotorEncoder(armR, 0);
PresetIntegratedMotorEncoder(armL, 0);
}
void ArmResetStateMachine() {
// switch(armResetState) {
// case STOP_PID:
StopIntegratedMotorEncoderPID(armR);
StopIntegratedMotorEncoderPID(armL);
// armResetState = LOWER_ARM;
// break;
// case LOWER_ARM:
while(GetDigitalInput(armResetSwitch)
&& GetJoystickDigital (1, 7, 2)){
SetArm(-75);
}
WaitInMsec(150);
// armResetState = START_PID;
// break;
// case START_PID:
PresetIntegratedMotorEncoder(armR, 0);
PresetIntegratedMotorEncoder(armL, 0);
StartIntegratedMotorEncoderPID(armR, 0);
StartIntegratedMotorEncoderPID(armL, 0);
SetArmState(GROUND);
// armResetState = DONE;
// break;
// case DONE:
// break;
// }
}
void WaitForTouch() {
int pressed = !GetDigitalInput(button);
while(pressed == 1 && IsAutoActive()) {
SetDrive(0,0);
}
PrintToScreen ("Pressed!\n");
}
int MotorCompliance(void)
{
int StopSwitch = 0;
int ForLoop = 1;
int ArmEmergencyRelease = 0;
unsigned int LetsGo = 0;
/* Bring the motor down at first powerup so the robot is compliant. */
while(ForLoop) // This is the same while loop as in the main program,
{ // but is using a variable set that starts set to 1
/* Get digital input from the switch mounted to the tower.
* GetDigitalInput is weird - 1 is unpressed, 0 is pressed */
StopSwitch = GetDigitalInput(7);
if(StopSwitch == 1)
{
/* Button on the joypad to press in case the above DI doesn't trigger.
* Same button as the arm unlock button. */
ArmEmergencyRelease = GetJoystickDigital(1,5,2);
if(ArmEmergencyRelease == 1)
{
SetServo(6,0);
ForLoop = 0;
}
/* Set various motors and servos to a compliant position */
SetMotor(2,-90);
SetServo(4,64);
SetServo(3,127);
SetServo(6,127);
SetServo(7,127);
}
else
{
LetsGo = GetJoystickAnalog(1,3);
if(LetsGo) //Move Raisy in any position where it won't be set to 0
{
SetServo(6,0); //Set Tilty to the straight position
ForLoop = 0; //Exit the while loop
}
else
{
SetMotor(2,-40); // Keep Raisy down to the base until the joystick is moved
}
}
}
/* Return 1 when this function is called. We need this set to 1 so this init function only runs
* once. MotorBringDown gets set to 1 and thus fails the if check when we do this. */
return 1;
}
/*
* Get the value from a digital input channel.
* Retrieve the value of a single digital input channel from the FPGA.
*
* @param channel The particular channel this digital input is using
*/
UINT32 GetDigitalInput(UINT32 channel)
{
return GetDigitalInput(SensorBase::GetDefaultDigitalModule(), channel);
}
