/*robo_arm Code to control a single degree of freedom robotic arm with gripper with automatic and manual control modes By Rich Pantaleo, October 2008 */ #include //Use the ServoTimer1 control library for arduino ServoTimer1 armservo; //create servo object to control the servo that moves the arm ServoTimer1 gripservo; //create servo object to control the servo that opens and closes the gripper int posIdle = 110; //Sevro angle for idle postion of arm in automatic mode int posGrip = 175; //Servo angle for gripping postion of arm in automatic mode int posPlace = 50; //Servo angle for placing postion of arm in automatic mode int gripOpen = 43; //Servo angle for open position of gripper int gripClosed = 85;//Servo angle for closed position of gripper int ballSwitch = 2; //Pin for ball detecion switch int ballVal = 0; //Variable for storing HIGH/LOW value of ball switch int button = 3; //Pin for push button input int buttonval = 0 //Variable for storing HIGH/LOW value of push button int potPin = 0; //Pin for potentiometer input int potVal = 0; //Variable for storing value of potPin int buttoncount = 0;//Variable for counting number of button presses int autoPin = 5; //Pin for swtich activating automatic mode int manPin = 6; //Pin for switch activiatin manual mode int autoVal = 0; //Variable for storing HIGH/LOW value auto switch int manVal = 0; //Variable for storing HIGH/LOW value manual switch boolean isThereABall = false; //Flag that stores if there is a ball on the ball switch boolean buttondown = false; //Flag that stores if the push button has been pressed or not boolean isGripClosed = false; //Flag that stores if the gripper is closed or open void setup() { armservo.attach(9); //Attaches the arm servo on pin 9 to the servo object gripservo.attach(10);//Attaches the grip servo on pin 10 to the servo object armservo.write(posIdle); //Sets the arm servo to the idle position gripservo.write(gripOpen);//Sets the grip servo to the open position pinMode(ballSwitch, INPUT);//Declares pin as input pinMode(button, INPUT); //Declares pin as input pinMode(autoPin, INPUT); //Declares pin as input pinMode(manPin, INPUT); //Declares pin as input } void loop() { autoVal = digitalRead(autoPin);//Read the value of the switch for automatic mode manVal = digitalRead(manPin); //Read the value of the switch for manual mode if (manVal == HIGH){ //If the switch is set to manual mode buttonval = digitalRead(button); //Read the value of the pushbutton if (buttonval == HIGH && buttondown == false){//If the button has been pressed buttondown = true; //The button is down buttoncount = buttoncount + 1; //Add 1 to the button counter } if (buttonval == LOW){//If the button is no longer being pressed buttondown = false; //The button is down } if (buttoncount == 1 && isGripClosed == false){//If the button has been pressed once and the gripper is open closeGrip(); //Close the gripper isGripClosed = true; //The gripper is now closed } if (buttoncount == 2){ //If the button has been pressed again gripservo.write(gripOpen);//Open the gripper buttoncount = 0; //Reset the counter to 0 isGripClosed = false; //The grip is open } potVal = analogRead(potPin); //Read the value (position) if the potentiometer potVal = map(potVal, 0, 1023, 0, 175);//Map the value of the potentiometer to the servo angle range of 0 to 175 armservo.write(potVal); //Move the servo to the angle set by the potentiometer } if (autoVal == HIGH){ //If the switch is set to automatic mode gripservo.write(gripOpen); //Open the gripper armservo.write(posIdle); //Move the arm to the idle position ballVal = digitalRead(ballSwitch); //See if a ball has been placed on the ball switch if (isThereABall == false && ballVal == HIGH){//If there is a ball getBall(); //Get the ball and put it in the box isThereABall = true; //There is a ball } else { isThereABall = false; //Otherwise, there isn't a ball } } } void getBall(){ //Subroutine for getting the ball and placing it in the box in automatic mode delay(1000); //Wait one second armservo.write(posGrip); //Move the arm to the postion to grab the ball delay(1000); //Wait one second closeGrip(); //Close the gripper delay(500); //Wait 0.5 seconds armservo.write(posPlace); //Move the arm to the postion to place the ball in the box delay(1000); //Wait 1 second gripservo.write(gripOpen);//Open the gripper delay(500); //Wait 0.5 seconds armservo.write(posIdle); //Move the arm to the idle position delay(1000); //Wait 1 second } void closeGrip(){ //Subroutine for closing the grip gripservo.write(75);//Quickly move the servo to the anlge 75 delay(50); //Wait 50 milliseconds gripservo.write(76);//Incrementally step the servo to the next angle with a delay between each step, to slow the closing rate of the servo delay(50); gripservo.write(77); delay(50); gripservo.write(78); delay(50); gripservo.write(79); delay(50); gripservo.write(80); delay(50); gripservo.write(81); delay(50); gripservo.write(82); delay(50); gripservo.write(83); delay(50); gripservo.write(84); delay(50); gripservo.write(gripClosed);//Move the servo to the final gripping position }