/*************** GRAB FUNCTION ********************/
void grab_function() // FUNCTION to Open/Close Grabber Arm
{
Serial1.end();
servo_enter_function_time = millis();
grabberServo.attach(grab_servo);
if( grabber_open == true)
{
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Grabber Open");
grabber_open = false;
grabberServo.write(grabber_close_angle);
}
else //( grabber_open == false)
{
lcd.clear();
lcd.setCursor(0,0);
Serial.print("Grabber Closed");
grabber_open = true;
grabberServo.write(grabber_open_angle);
}
while(millis() < servo_enter_function_time + 1250) // Wait for 1250 ms
{
}
grabberServo.detach();
Serial1.begin(9600);
/**************** END GRAB FUNCTION *****************/
/*************** ARM FUNCTION ********************/
void arm_function() //Function that closes the grabber arm
{
Serial1.end(); // Stop communicating with the XBee
servo_enter_function_time = millis();
liftServo.attach(lift_servo);
if( arm_down == true) // IF the arm is down
{
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Arm Down");
arm_down = false; // Now the arm is up
liftServo.write(arm_lift_angle); // Raise the arm
}
else //(arm_down == false) // IF the arm is not down
{
lcd.clear();
lcd.setCursor(0,0);
lcd.print("Arm Up");
arm_down = true; // Now the arm is down
liftServo.write(arm_lower_angle); // Lower the arm
}
while(millis() < servo_enter_function_time + 1250) // Wait for 1250 ms
{
}
liftServo.detach();
Serial1.begin(9600); // Restart communicating with the XBee
}
/**************** END ARM FUNCTION *****************/[code]
[/code]