Good evening,
I have a botboardino with a 1shield and separate L298N module and I am attempting to control 10 servos and 2 DC motors with a modified code that I from the Arduino project forums.
All of the servos are working but I cannot get the motors to run.
I have used the motor code separately with an Arduino Uno and it seems to work O.k. The only change is to the motor speed control pins, I am using A0 & A5 instead of 3 & 5 as they are already in use.
Please could someone check the code for me to help point me in the right direction.
> //
> // ********** Digital Pins D0 thru D13 **********
>
> // Using 0 and 1 disables upload!
>
> //#define DO_NOT_USE_0 0
> //#define DO_NOT_USE_1 1
>
> #define SHOULDER_PIN 2
> #define ELBOW_PIN 3
> #define WRIST_PIN 4
>
> #define SPEAKER_PIN 5
>
> #define BASE_PIN 6
> #define BACKPAN_PIN 7
> #define ONBOARD_PB_B 8
> #define BACKTILT_PIN 9
>
> #define WRISTROTATE_PIN 10
> #define GRIPPER_PIN 11
>
> #define BACKELBOW_PIN 12
> #define BACKGRIPPER_PIN 13
>
>
>
>
>
>
>
>
>
>
>
> #define CUSTOM_SETTINGS
> #define INCLUDE_TOGGLE_BUTTON_SHIELD
> #define INCLUDE_GAMEPAD_SHIELD
> #define INCLUDE_LED_SHIELD
> #define INCLUDE_ACCELEROMETER_SENSOR_SHIELD
> #define INCLUDE_SLIDER_SHIELD
>
> float x, y, z;
> #include <OneSheeld.h>
> /* Include Motor library. */
> #include <AFMotor.h>
> #include <Servo.h>
> #include <pitch.h>
>
> /* Pin configuration of the Seeedstudio's motor shield. */
> int motorAPin1 = A1;
> int motorAPin2 = A2;
> int motorBPin1 = A3;
> int motorBPin2 = A4;
> int motorASpeedPin = A0;
> int motorBSpeedPin = A5;
>
> // *********************************************************************************************************************************************************************
> // Global Variables.
> // Professional programmers cringe at the thought of globals, but they seem to be generally acceptable in the "wild west" Arduino world.
> // Prudent use of them simplifies program design since variable passing via parameters between functions are reduced.
> // *********************************************************************************************************************************************************************
>
> // An homage to every program I've written since my WATFIV days.
> // Every program needs an "int i", right?
> int i;
>
> // Servos that make up the AL5D
> Servo Base, Shoulder, Elbow, Wrist, WristRotate, Gripper, BackElbow, BackGripper, BackPan, BackTilt;
>
> // Store the current servo angles
> int currentBaseAngle, currentShoulderAngle, currentElbowAngle,
> currentWristAngle, currentWristRotateAngle, currentGripperAngle,
> currentBackElbowAngle, currentBackGripperAngle, currentBackPanAngle, currentBackTiltAngle;
>
> // Control the speed of the arm movements. Increase angle, decrease delay to make arm react faster.
> int servoStepAngle = 2 , servoStepDelay = 25;
> // Determine whether the arm is under user control, or autonomous demo mode.
> bool demoMode = false;
> //
>
>
> // **********************************************************************************************************************************************************************
> // Function Prototypes <-------------------------------------------------------------------------------------------------------------------------------------------------
> // **********************************************************************************************************************************************************************
>
> void gamepadControl ( void );
> void gamepadControl_BORG ( void );
> void gamepadControl_RULD ( void );
> void shaveAndAHaircut ( void );
>
> void rotateBase ( const int& endAt );
> void rotateShoulder ( const int& endAt );
> void rotateElbow ( const int& endAt );
> void rotateWrist ( const int& endAt );
> void rotateWristRotate ( const int& endAt );
> void rotateGripper ( const int& endAt );
> void rotateBackElbow ( const int& endAt );
> void rotateBackGripper ( const int& endAt );
> void rotateBackPan ( const int& endAt );
> void rotateBackTilt ( const int& endAt );
> void rotateServo ( const int& endAt , Servo& whichServo , int& currentAngle );
>
> void deployArms ( void );
> void grabVerticalItem ( void );
> void dropItem ( void );
> void autonomousDemo ( void );
>
> #ifdef SecondArm_Installed
> void grabItemFromBackArm ( void );
> #endif
> void extendArm ( const int& = 34 , const bool& = 0 ); // If no arguments provided, default Shoulder height is 34, default "withItem" is no
> void retractArm ( const int& = 150 ); // If no argument provided, default Shoulder height is 150,
>
>
>
> // *********************************************************************************************************************************************************************
> // Main program <-------------------------------------------------------------------------------------------------------------------------------------------------------
> // *********************************************************************************************************************************************************************
>
> void setup() {
>
> // Connect to your Smartphone/Tablet for remote control
> OneSheeld.begin( );
> // Init the BotBoarduino onboard push button B
>
> pinMode ( ONBOARD_PB_B , INPUT );
>
>
> deployArms( );
>
> /* Motor shield initialization. */
> pinMode(motorAPin1, OUTPUT); // IN1 of motor A
> pinMode(motorAPin2, OUTPUT); // IN2 of motor A
> pinMode(motorBPin1, OUTPUT); // IN3 of motor B
> pinMode(motorBPin2, OUTPUT); // IN4 of motor B
> pinMode(motorASpeedPin, OUTPUT); // Speed of motor A
> pinMode(motorBSpeedPin, OUTPUT); // Speed of Motor B
>
> }
>
> void loop() {
>
>
> if ( digitalRead ( ONBOARD_PB_B ) == LOW ) demoMode = false;
>
> if ( demoMode ) autonomousDemo( );
> else gamepadControl( );
>
>
> }
>
>
>
> // *********************************************************************************************************************************************************************
> // Functions <----------------------------------------------------------------------------------------------------------------------------------------------------------
> // *********************************************************************************************************************************************************************
>
> // Incoming parameters : constant reference to an int denoting servo angle to move to.
> // Global variables : Uses - "Servo", current"Servo"Angle
> // Changes -
> // Responsibilities : Passes servo variables on to generic servo control function.
> // Returns : Nothing.
>
> void rotateBase ( const int& endAt ) { rotateServo( endAt , Base , currentBaseAngle ); }
> void rotateShoulder ( const int& endAt ) { rotateServo( endAt , Shoulder , currentShoulderAngle ); }
> void rotateElbow ( const int& endAt ) { rotateServo( endAt , Elbow , currentElbowAngle ); }
> void rotateWrist ( const int& endAt ) { rotateServo( endAt , Wrist , currentWristAngle ); }
> void rotateWristRotate ( const int& endAt ) { rotateServo( endAt , WristRotate , currentWristRotateAngle ); }
> void rotateGripper ( const int& endAt ) { rotateServo( endAt , Gripper , currentGripperAngle ); }
> void rotateBackElbow ( const int& endAt ) { rotateServo( endAt , BackElbow , currentBackElbowAngle ); }
> void rotateBackGripper ( const int& endAt ) { rotateServo( endAt , BackGripper , currentBackGripperAngle ); }
> void rotateBackPan ( const int& endAt ) { rotateServo( endAt , BackPan, currentBackPanAngle ); }
> void rotateBackTilt ( const int& endAt ) { rotateServo( endAt , BackTilt, currentBackTiltAngle ); }
> void rotateServo ( const int& endAt , Servo& whichServo , int& currentAngle ) {
>
> // Incoming parameters : constant reference to an int denoting servo angle to move to,
> // reference to a Servo instance,
> // reference to the current angle of the Servo
>
> // Global variables : Uses - servoStepAngle, servoStepSpeed
> // Changes - "current...Angle" of Servo passed by reference
>
> // Responsibilities : Moves desired Servo to requested angle. Changes "current...Angle" of that Servo.
> // Why this function? Moving a Servo from one angle to another can be a rather fast and brutal action,
> // not very conducive to finely controlling the action(s) of the arm.
> // This function slows the movements down to a smooth and managable speed.
> // Want faster or slower? Just manipulate the values of globals servoStepAngle and servoStepSpeed.
>
> // Returns : Nothing.
>
> if ( currentAngle == endAt ) return;
>
> if ( currentAngle < 0 ) {
> tone(5, 999, 250); // Stop that!
> whichServo.write(currentAngle = 2); delay(servoStepDelay);
> return;
> }
>
> if ( currentAngle > 180 ) {
> tone(5, 999, 250); // Stop that!
> whichServo.write(currentAngle = 178); delay(servoStepDelay);
> return;
> }
>
>
> if ( endAt > currentAngle ) {
> for ( int i = currentAngle; i <= endAt; i += servoStepAngle ) {
>
> whichServo.write(currentAngle = i); delay(servoStepDelay);
>
> }
> }
> else {
> for ( int i = currentAngle; i >= endAt; i -= servoStepAngle ) {
>
> whichServo.write(currentAngle = i); delay(servoStepDelay);
>
> }
> }
>
> }
>
>
> void deployArms ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses -
> // Changes - currentBaseAngle, currentShoulderAngle, currentElbowAngle,
> // currentWristAngle, currentWristRotateAngle, currentGripperAngle,
> // currentBackElbowAngle, currentBackGripperAngle
>
> // Responsibilities : Set arm(s) and global current...Angle's to initial working position.
> // You can fine tune this to other behaviour if you wish.
>
> // Returns : Nothing.
>
>
> BackGripper.attach (BACKGRIPPER_PIN); BackGripper.write (currentBackGripperAngle = 90);
> delay(1500); BackElbow.attach (BACKELBOW_PIN); BackElbow.write (currentBackElbowAngle = 90);
> delay(1500); BackPan.attach (BACKPAN_PIN); BackPan.write (currentBackPanAngle = 90);
> delay(1500); BackTilt.attach (BACKTILT_PIN); BackTilt.write (currentBackTiltAngle = 90);
>
> delay( 500); Shoulder.attach (SHOULDER_PIN); Shoulder.write (currentShoulderAngle = 90);
> delay( 500); Elbow.attach (ELBOW_PIN); Elbow.write (currentElbowAngle = 136);
>
> delay( 500); Wrist.attach (WRIST_PIN); Wrist.write (currentWristAngle = 70);
> delay( 500); WristRotate.attach (WRISTROTATE_PIN); WristRotate.write (currentWristRotateAngle = 160);
> delay( 500); Gripper.attach (GRIPPER_PIN); Gripper.write (currentGripperAngle = 90);
>
> delay( 500); Base.attach (BASE_PIN); Base.write (currentBaseAngle = 90);
>
> delay(1000); retractArm( );
>
> }
>
>
> void gamepadControl ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses -
> // Changes -
>
> // Responsibilities : Control arm servos using 1Sheeld Gamepad Buttons...
> //
> // Orange, Blue, Red, Green buttons are used in conjunction with Up, Left, Right, Down buttons.
> // Up, Left, Right, Down buttons can be used independently.
>
> // Returns : Nothing.
>
> if ( GamePad.isBluePressed( ) || GamePad.isOrangePressed( ) ||
> GamePad.isRedPressed( ) || GamePad.isGreenPressed( ) ) {
>
> gamepadControl_BORG( );
>
> }
> else if ( GamePad.isUpPressed( ) || GamePad.isDownPressed( ) ||
> GamePad.isLeftPressed( ) || GamePad.isRightPressed( ) ) {
>
> gamepadControl_RULD( );
>
> }
>
> }
>
> void gamepadControl_BORG ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses - currentBaseAngle, currentShoulderAngle, currentElbowAngle,
> // currentWristAngle, currentWristRotateAngle, currentGripperAngle,
> // servoStepAngle
> // Changes -
>
> // Responsibilities : Control primary arm servos using the following 1Sheeld Gamepad Button Combinations...
> //
> // Green ... Up/Down - Back Elbow, Left/Right - Back Gripper
> // Red ... Up/Down - Gripper Open/Close, Left/Right - Gripper Rotate
> // Blue ... Up/Down - Elbow, Wrist
> // Orange ... Up/Down - Shoulder, Left/Right - |Base
>
> // Returns : Nothing.
> if ( GamePad.isOrangePressed( ) ) {
>
> if ( GamePad.isUpPressed( ) ) {
> // tone(5, 111, 250);
> rotateShoulder( currentShoulderAngle - servoStepAngle );
> }
>
> else if ( GamePad.isDownPressed( ) ) {
> // tone(5, 222, 250);
> rotateShoulder( currentShoulderAngle + servoStepAngle );
> }
>
> else if ( GamePad.isLeftPressed( ) ) {
> // tone(5, 333, 250);
> rotateBase( currentBaseAngle + servoStepAngle );
> }
>
> else if ( GamePad.isRightPressed( ) ) {
> // tone(5, 444, 250);
> rotateBase( currentBaseAngle - servoStepAngle );
> }
> }
>
> else if ( GamePad.isBluePressed( ) ) {
>
> if ( GamePad.isUpPressed( ) ) {
> rotateElbow( currentElbowAngle + servoStepAngle );
> }
>
> else if ( GamePad.isDownPressed( ) ) {
> rotateElbow( currentElbowAngle - servoStepAngle );
> }
>
> else if ( GamePad.isLeftPressed( ) ) {
> // tone(5, 333, 250);
> rotateWristRotate( currentWristRotateAngle - servoStepAngle );
> }
>
> else if ( GamePad.isRightPressed( ) ) {
> // tone(5, 444, 250);
> rotateWristRotate( currentWristRotateAngle + servoStepAngle );
> }
>
> }
> else if ( GamePad.isRedPressed( ) ) {
>
> if ( GamePad.isUpPressed( ) ) {
> // tone(5, 111, 250);
> rotateGripper( currentGripperAngle + servoStepAngle );
> }
>
> else if ( GamePad.isDownPressed( ) ) {
> // tone(5, 222, 250);
> rotateGripper( currentGripperAngle - servoStepAngle );
> }
>
> else if ( GamePad.isLeftPressed( ) ) {
> // tone(5, 333, 250);
> rotateWrist( currentWristAngle - servoStepAngle );
> }
>
> else if ( GamePad.isRightPressed( ) ) {
> // tone(5, 444, 250);
> rotateWrist( currentWristAngle + servoStepAngle );
> }
>
> }
> else if ( GamePad.isGreenPressed( ) ) {
>
> if ( GamePad.isUpPressed( ) ) {
> // tone(5, 111, 250); // Activate tones for debugging purposes if you wish
> rotateBackElbow ( currentBackElbowAngle - servoStepAngle );
>
> }
>
> else if ( GamePad.isDownPressed( ) ) {
> // tone(5, 222, 250);
> rotateBackElbow ( currentBackElbowAngle + servoStepAngle );
>
> }
>
> else if ( GamePad.isLeftPressed( ) ) {
> // tone(5, 333, 250);
> rotateBackGripper ( currentBackGripperAngle + servoStepAngle );
>
> }
>
> else if ( GamePad.isRightPressed( ) ) {
> // tone(5, 444, 250);
> rotateBackGripper ( currentBackGripperAngle - servoStepAngle );
>
> }
>
> }
>
>
>
> }
>
>
>
> void gamepadControl_RULD ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses - currentBackElbowAngle, currentBackGripperAngle,
> // servoStepAngle
>
>
>
> // Returns : Nothing.
>
> /* Always check the status of gamepad buttons. */
> if ( GamePad.isUpPressed( ) ) {
> rotateBackTilt( currentBackTiltAngle + servoStepAngle );
> }
>
> else if ( GamePad.isDownPressed( ) ) {
> rotateBackTilt( currentBackTiltAngle - servoStepAngle );
> }
>
> else if ( GamePad.isLeftPressed( ) ) {
> // tone(5, 333, 250);
> rotateBackPan( currentBackPanAngle - servoStepAngle );
> }
>
> else if ( GamePad.isRightPressed( ) ) {
> // tone(5, 444, 250);
> rotateBackPan( currentBackPanAngle + servoStepAngle );
> }
>
>
>
>
>
> x=AccelerometerSensor.getX();
> y=AccelerometerSensor.getY();
> z=AccelerometerSensor.getZ();
>
> /* If tilt forward move forward */
> if ((x < -4) && (x > -8))
> {
> analogWrite(motorASpeedPin, 122);
> analogWrite(motorBSpeedPin, 122);
> digitalWrite(motorAPin1, LOW);
> digitalWrite(motorAPin2, HIGH);
> digitalWrite(motorBPin1, LOW);
> digitalWrite(motorBPin2, HIGH);
>
>
> }
>
> else if (x < -8)
> {
> analogWrite(motorASpeedPin, 255);
> analogWrite(motorBSpeedPin, 255);
> digitalWrite(motorAPin1, LOW);
> digitalWrite(motorAPin2, HIGH);
> digitalWrite(motorBPin1, LOW);
> digitalWrite(motorBPin2, HIGH);
>
>
> }
>
> /* If tilted backward, move the car backwards. */
>
>
> else if ((x > 4) && (x < 8))
> {
> analogWrite(motorASpeedPin, 122);
> analogWrite(motorBSpeedPin, 122);
> digitalWrite(motorAPin1, HIGH);
> digitalWrite(motorAPin2, LOW);
> digitalWrite(motorBPin1, HIGH);
> digitalWrite(motorBPin2, LOW);
>
> }
> else if (x > 8)
> {
> analogWrite(motorASpeedPin, 255);
> analogWrite(motorBSpeedPin, 255);
> digitalWrite(motorAPin1, HIGH);
> digitalWrite(motorAPin2, LOW);
> digitalWrite(motorBPin1, HIGH);
> digitalWrite(motorBPin2, LOW);
>
>
>
> }
> /* If tilted right, turn the car to the right. */
> else if ((y < -4) && (y > -8))
> {
> analogWrite(motorASpeedPin, 122);
> analogWrite(motorBSpeedPin, 122);
> digitalWrite(motorAPin1, LOW);
> digitalWrite(motorAPin2, HIGH);
> digitalWrite(motorBPin1, HIGH);
> digitalWrite(motorBPin2, LOW);
>
>
> }
>
> else if (y < -8)
> {
> analogWrite(motorASpeedPin, 255);
> analogWrite(motorBSpeedPin, 255);
> digitalWrite(motorAPin1, LOW);
> digitalWrite(motorAPin2, HIGH);
> digitalWrite(motorBPin1, HIGH);
> digitalWrite(motorBPin2, LOW);
>
>
> }
> /* If tilted left, turn the car to the left. */
> else if ((y > 4) && (y < 8))
> {
> analogWrite(motorASpeedPin, 122);
> analogWrite(motorBSpeedPin, 122);
> digitalWrite(motorAPin1, HIGH);
> digitalWrite(motorAPin2, LOW);
> digitalWrite(motorBPin1, LOW);
> digitalWrite(motorBPin2, HIGH);
>
>
> }
> else if (y > 8)
> {
> analogWrite(motorASpeedPin, 255);
> analogWrite(motorBSpeedPin, 255);
> digitalWrite(motorAPin1, HIGH);
> digitalWrite(motorAPin2, LOW);
> digitalWrite(motorBPin1, LOW);
> digitalWrite(motorBPin2, HIGH);
>
>
> }
> /* If nothing is
> /* If nothing is pressed stop all motors. */
> else
> {
> //analogWrite(motorASpeedPin, 0);
> //analogWrite(motorBSpeedPin, 0);
> digitalWrite(motorAPin1, LOW);
> digitalWrite(motorAPin2, LOW);
> digitalWrite(motorBPin1, LOW);
> digitalWrite(motorBPin2, LOW);
> }
>
> }
> void shaveAndAHaircut ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses -
> // Changes -
>
> // Responsibilities : Plays the tune.
>
> // Returns : Nothing.
>
> // Individual notes
> int melody_ShaveHaircut[] = {
> NOTE_C4, NOTE_G3, NOTE_G3, NOTE_A3, NOTE_G3, 0, NOTE_B3, NOTE_C4
> };
> // Note durations: 4 = quarter note, 8 = eighth note, etc.
> int noteDurations_ShaveHaircut[] = {
> 4, 8, 8, 4, 4, 4, 4, 4
> };
> int noteDuration;
>
> for (int thisNote = 0; thisNote < 8; thisNote++) {
>
> // To calculate the note duration, take one second divided by the note type.
> // e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
> noteDuration = 1000 / noteDurations_ShaveHaircut[thisNote];
>
> tone(SPEAKER_PIN, melody_ShaveHaircut[thisNote], noteDuration);
>
> // To distinguish the notes, set a minimum time between them.
> // The note's duration + 30% seems to work well
> delay(noteDuration * 1.30);
>
> // Stop the tone playing
> noTone(SPEAKER_PIN);
>
> }
>
> }
>
>
>
>
>
> void retractArm ( const int& shoulderStop ) {
>
> // Incoming parameters : constant reference to an int denoting ending angle of Shoulder servo.
>
> // Global variables : Uses - servoStepAngle, servoStepDelay
> // Changes - currentShoulderAngle, currentElbowAngle, currentWristAngle
>
> // Responsibilities : Retracts the arm in a predetermined manner.
> // A function of convenience, takes the place of the user having to perform
> // a number of actions. Modify to suit your needs.
>
> // Returns : Nothing.
>
> int braking = 0;
>
> for ( int i = currentShoulderAngle; i <= shoulderStop; i += servoStepAngle ) {
>
> Shoulder.write(currentShoulderAngle = i);
>
> if ( (i+86) <= 180 ) Elbow.write(currentElbowAngle = i+86);
>
> // Keep the wrist as horizontal as possible
> if ( i <= 90 ) Wrist.write(currentWristAngle = 140);
> else if ( i >= 150 ) Wrist.write(currentWristAngle = 75);
> else Wrist.write(currentWristAngle = ( 140 - ( i - 90 )) );
>
> // As you rise towards 90 degrees, weight is working for you, keeping things slow.
> // Past 90 degrees, weight works against you as it increases the speed of the arm,
> // resulting in an momentum that builds the farther you go.
> // The following formula calculates an extra time in servo delay, resulting in a
> // braking action.
> braking = ( i > 90 ) ? ( ( i - 90 ) / 10 ) * 15 : 0;
> delay(servoStepDelay+braking);
>
> }
>
>
> delay(1000);
>
> }
>
> void extendArm ( const int& shoulderStop , const bool& withItem ) {
>
> // Incoming parameters : constant reference to an int denoting ending angle of Shoulder servo.
> // constant reference to a bool denoting if the arm is holding an item ( 0 - no , 1 - yes ).
>
> // Global variables : Uses - servoStepAngle, servoStepDelay
> // Changes - currentShoulderAngle, currentElbowAngle, currentWristAngle
>
> // Responsibilities : Extend the arm in a predetermined manner.
> // A function of convenience, takes the place of the user having to perform
> // a number of actions. Modify to suit your needs.
>
> // Returns : Nothing.
>
>
> for ( int i = currentShoulderAngle; i >= shoulderStop; i -= servoStepAngle ) {
>
> if ( (i+86) <= 180 ) Elbow.write(currentElbowAngle = i+86);
>
> Shoulder.write(currentShoulderAngle = i);
>
> if ( withItem ) {
> // Keep the wrist as horizontal as possible
> if ( i <= 90 ) Wrist.write(currentWristAngle = 140);
> else if ( i >= 150 ) Wrist.write(currentWristAngle = 75);
> else Wrist.write(currentWristAngle = ( 140 - ( i - 90 )) );
> }
> else {
> // Move the wrist to angle up onto an item that is to be grasped
> Wrist.write(currentWristAngle = 160-i);
> }
>
> delay(servoStepDelay);
> }
>
>
>
> delay(1000);
>
> }
>
>
>
> void grabVerticalItem ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses -
> // Changes -
>
> // Responsibilities : Grabs a vertical item (something "standing up" rather than "laying down") with the Gripper
> // A function of convenience, takes the place of the user having to perform
> // a number of actions. Modify to suit your needs.
>
> // Returns : Nothing.
>
> delay( 100); rotateWristRotate ( 70); // Rotate to grab a vertical item ( ie | )
> delay( 100); rotateGripper ( 00); // Open
> delay(2000); rotateGripper (180); // Close
> //delay(2000); rotateWristRotate (160); // Rotate item to horizontal ( ie --- )
> //delay( 100); rotateWrist (180); // Raise
> //delay(1000);
>
> }
>
> void dropItem ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses -
> // Changes -
>
> // Responsibilities : Releases an item from the Gripper.
> // A function of convenience, takes the place of the user having to perform
> // a number of actions. Modify to suit your needs.
>
> // Returns : Nothing.
>
>
> delay( 500); rotateWrist ( 40); // Lower
> delay( 500); rotateGripper ( 00); // Open
> delay( 500); rotateShoulder ( 44); // Raise arm a little
> delay( 500); rotateGripper (180); // Close
> delay( 500); rotateWrist (180); // Raise
> delay( 500);
>
> }
>
>
>
>
>
> void autonomousDemo ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses -
> // Changes - currentBaseAngle
>
> // Responsibilities : Run the arm through a series of actions for demonstration purposes.
>
> // Returns : Nothing.
>
>
> // Announce start of demo
> shaveAndAHaircut ( );
>
>
> // Grab an item from the front
> rotateBase ( 90); // Move arm to front
> rotateWristRotate ( 70); // Rotate to grab a vertical item ( ie | )
> rotateGripper ( 00); // Open before moving arm into position
> extendArm ( 14); // Modify to suit your height requirement
> rotateWrist (130); // Tweak wrist angle if required
> rotateGripper (180); // Close on item
> retractArm (110); // Move back to this Shoulder angle
>
> delay(1000);
>
> // Place it to the right
> rotateBase (180); // Move arm full right
> rotateWristRotate ( 70); // Rotate to place vertical item ( ie | )
> extendArm (14,1); // Modify to suit your height requirement
> //rotateWrist (???); // Tweak wrist angle if required
> rotateGripper ( 00); // Open to release item
> retractArm ( ); // Park arm
>
>
> #ifdef SecondArm_Installed
> grabItemFromBackArm ( ); // Interact with back arm (optional)
> retractArm ( ); // Return to standard starting position
> #endif
>
>
> // Grab an item from the right
> rotateBase (180); // Move arm full right
> rotateWristRotate ( 70); // Rotate to grab a vertical item ( ie | )
> rotateGripper ( 00); // Open before moving arm into position
> extendArm ( 14); // Modify to suit your height requirement
> //rotateWrist (???); // Tweak wrist angle if required
> rotateGripper (180); // Close on item
> retractArm (110); // Move back to this Shoulder angle
>
> delay(1000);
>
> // Place it in front
> rotateBase ( 90); // Move arm to front
> rotateWristRotate ( 70); // Rotate to place vertical item ( ie | )
> extendArm (14,1); // Modify to suit your height requirement
> rotateWrist (130); // Tweak wrist angle if required
> rotateGripper ( 00); // Open to release item
> retractArm ( ); // Park arm
>
>
> }
>
>
>
>
> #ifdef SecondArm_Installed
> void grabItemFromBackArm ( void ) {
>
> // Incoming parameters : Nothing.
>
> // Global variables : Uses - Wrist, WristRotate, Gripper,
> // BackElbow, BackGripper
> // Changes - currentWristAngle, currentWristRotateAngle, currentGripperAngle,
> // currentBackElbowAngle, currentBackGripperAngle
>
> // Responsibilities : The primary arm will reach back, grab an item from the secondary arm,
> // raise it in the air, and then return it to the secondary arm.
> // Admittedly not very useful, just a demo of arm capabilities.
> // A function of convenience, takes the place of the user having to perform
> // a number of actions. Modify to suit your needs.
> //
> // Why all the attach's and detach's? With all the servos operating, I was
> // getting feedback through some of the circuits, causing servo flutter and
> // undesirable effects. Shutting down unnecessary servos solved the problem.
> // You may or may not have the same issues.
>
> // Returns : Nothing.
>
> // Announce beginning of sequence.
> for ( i = 0; i < 4; i++ ) { tone(5, 444, 250); delay(500); tone(5, 888, 250); delay(500); }
>
> // Grab something if BotBoarduino BotBoarduino onboard push button C is pressed
> if ( digitalRead ( ONBOARD_PB_C ) == LOW ) {
>
> BackGripper.attach (BACKGRIPPER_PIN);
> delay( 500); rotateBackGripper (180); // Open to accept item
> while ( digitalRead ( ONBOARD_PB_C ) == LOW ) delay(500); // Wait until button released
> rotateBackGripper ( 00); // Close to grab item
>
> }
>
>
> // Raise back arm.
> BackElbow.attach (BACKELBOW_PIN);
> rotateBackGripper ( 00); // Make sure gripper is closed
> rotateBackElbow ( 16);
> BackElbow.detach ( );
>
> // Orient wrist and gripper for the task.
> rotateWrist (120);
> rotateWristRotate ( 70);
> rotateGripper ( 00); // Open in preparation
>
> // Move front arm backwards to contact item.
> rotateBase ( 90);
> rotateShoulder ( 90);
> rotateElbow ( 00);
>
> // Exchange item from back to front.
> // My gripper servos are configured differently...
> // Gripper closes at 180, but BackGripper opens at 180.
> // Adjust your angles to suit your setup.
> delay( 500); BackGripper.attach (BACKGRIPPER_PIN);
> delay( 500); rotateGripper (180); // Close on item
> delay( 500); rotateBackGripper (180); // Open to release item
> delay( 500); BackGripper.detach ( );
>
>
> // Raise in the air, then return item.
> delay( 500); rotateElbow ( 90);
> delay( 500); rotateShoulder ( 88);
> delay(2000); rotateElbow ( 00);
>
>
> // Exchange item from front to back.
> // My gripper servos are configured differently...
> // Gripper opens at 00, but BackGripper closes at 00.
> // Adjust your angles to suit your setup.
> BackGripper.attach (BACKGRIPPER_PIN);
> delay( 500); rotateBackGripper ( 00); // Close to grip item
> delay( 500); rotateGripper ( 00); // Open to release item
>
>
> // At this point, there a lot of power being drawn because so many servos are operating.
> // Shut down unnecessary servos to lessen the load on your circuits.
> // Leave the Shoulder attached because it's holding up the arm.
> delay( 500); Wrist.detach ( );
> WristRotate.detach ( );
> Gripper.detach ( );
>
> // Lower back arm
> BackElbow.attach (BACKELBOW_PIN);
> rotateBackElbow ( 88); // Lower back arm
> BackElbow.detach ( );
>
>
> // Raise front arm.
> delay( 500); rotateElbow (180);
> delay( 500); rotateShoulder (150);
>
>
> // Reattach to front arm servos.
> Wrist.attach (WRIST_PIN);
> WristRotate.attach (WRISTROTATE_PIN);
> Gripper.attach (GRIPPER_PIN);
>
> delay(500);
>
> }
> #endif
