Recent Changes - Search:

ENTER NEW DAILY DATA HERE:

PmWiki

pmwiki.org

edit SideBar

ElmoSlaveChoreographyCodeDepot

As I revise the Elmo Slave code with improved functions to make it easier to program the choreography, I'll post the latest version on this page.

/*
TO USE THIS CODE:
0. This code calls header file "declareElmoSlavePins.h".
   A. Copy the header file from the wiki link titled "Elmo Slave Header File," and paste into a plain text file.
   B. Save the text file as "declareElmoSlavePins" and close the text file.
   C. Rename the text file to have a ".h" extension (on the Mac, this is most easily done from the Terminal program
      using the "mv" command).
   D. Move this file into your Arduino folder
      (the Arduino folder is usually in "My Documents" on the PC, or in "Documents" on the Mac).
   E. Copy the code from this wiki page and paste it into a new Arduino sketch. Save the file with whatever name you want.
   F. Click on the drop-down menu "Sketch" and select "Add File...". Select the file "declareElmoSlavePins.h"
1. This code was written to test choreography on an ElmoSlave.
   It is assumed that you have an ElmoSlave with an Arduino board strapped to its head,
   so that you can easily upload code to try out new things. It is also assumed that your ElmoSlave
   is activated by pressing its foot-button.
   Wiring up Elmo's foot-button:
   black wire from foot: connect to ground (any ground available on pcb or arduino board)
   green wire from foot: connect to switchPin (whatever pin you pick on the Arduino to detect that the switch was pressed)
2. Modify the code to creat a new choreography, turn on the ElmoSlave, plug in the USB cable to connect
   your computer to the ElmoSlave, and upload your new code.
3. Press the foot-button and watch ElmoSlave perform the choreography.
4. Fine-tune the code by adjusting the delay times and motor speeds as needed.

AUTHORS: Mike Gazes, Nick Wong, Soyoung Park, Eric Beug
DATE: 13 February 2009
TO DO:
  1. Clean up functions. Create master functions for fundamental movements, then a library of
     specialized functions for specific movements.
See additional comments at end of file.
 */

//DECLARE VARIABLES AND PINS
//******************************************************

    //VARIABLES USED ONLY FOR CHOREOGRAPHY TESTING (FOOT-BUTTON ACTIVATED)
    // DELETE THIS SECTION FROM THE FINAL ELMO SLAVE CODE (WIRELESS ACTIVATED)
    // Beginning of variables for choreography testing:
    //**********************************************************
    //Switch Detection Pins:
    int switchPin = 1;

    //Switch Detection State Variables:
    int currentStep; //keep track of which movement we are up to
    int stepNumber=0; //keep track of which movement to do next
    int previousState=0;
    int nowState=0;
    int stateChange=0;
    // End of variables for choreography testing
    //*********************************************************
//Gyroscope Feedback Pins:
int gyroGreen = 13; // supine (this wire from the gyro is disconnected while we do tests)
int gyroYellow = 19; // upright
int gyroRed = 12; // prone
//brown is ground, and orange is not connected

# include "declareElmoSlavePins.h" // See this header file for all other pin declarations



//Motor Variables:
int forward=1;
int reverse=0;



void setup() {

//DECLARE INPUTS/OUTPUTS

//MOTORS
//*******************************************************
  //Arm motor output pins:
    pinMode(armSpeed, OUTPUT);
    pinMode(armMotorPlus, OUTPUT);
    pinMode(armMotorMinus, OUTPUT);

  //State of arm motor on startup:
    digitalWrite(armSpeed, LOW); //Disable motor
    //Set for forward rotation:
      digitalWrite(armMotorPlus, HIGH);
      digitalWrite(armMotorMinus, LOW);

  //Leg motor output pins:
    pinMode(legSpeed, OUTPUT);
    pinMode(legMotorPlus, OUTPUT);
    pinMode(legMotorMinus, OUTPUT);

  //State of leg motor on startup:
    digitalWrite(legSpeed, LOW); //Disable motor
    //Set for forward rotation:
      digitalWrite(legMotorPlus, HIGH);
      digitalWrite(legMotorMinus, LOW);
//*******************************************************

//SENSORS
//*******************************************************
  //Motor Sensor Feedback Pins for arm position and leg position (see header file "declareElmoSlavePins.h")
    //Set all sensor pins as inputs
      pinMode(armReachBack, INPUT);
      pinMode(armUp, INPUT);
      pinMode(armStraightAhead, INPUT);
      pinMode(armDown, INPUT);
      pinMode(bodyBentMax, INPUT);
      pinMode(bodyBent90, INPUT);
      pinMode(bodyBentMin, INPUT);
      pinMode(bodyStraight, INPUT);
      pinMode(bodyKickedBack, INPUT);
    //Enable internal pull-up resistors on all sensor pins
      digitalWrite(armReachBack, HIGH);
      digitalWrite(armUp, HIGH);
      digitalWrite(armStraightAhead, HIGH);
      digitalWrite(armDown, HIGH);
      digitalWrite(bodyBentMax, HIGH);
      digitalWrite(bodyBent90, HIGH);
      digitalWrite(bodyBentMin, HIGH);
      digitalWrite(bodyStraight, HIGH);
      digitalWrite(bodyKickedBack, HIGH);
//*******************************************************

}
void loop() {
  //Wait for the button to be pressed. Then execute
  // the current stepNumber, and increment the stepNumber.
  currentStep = stepNumber;
  while (currentStep == stepNumber) {
    stepNumber=waitForButton(stepNumber);
  }

  doNextStep(stepNumber);
//     blink(ledPin,stepNumber,500);

}
//FUNCTION DEFINITIONS
//******************************************************************************

//CHOREOGRAPHY ROUTINE
//*********************************************
void doNextStep(int whichStep) {
  switch(whichStep) {
//BREAK UP THE ROUTINE INTO AS MANY STEPS AS YOU WANT. EACH SEPARATE STEP BEGINS WITH A case STATEMENT.
//THE FINAL CASE STATEMENT MUST RESET STEP NUMBER TO 0.
  case 1:
    //initialize arm position
      moveArm(armDown, forward, 100);
    //shake body uncontrollably
      shakeLeg(bodyStraight, 13, 200);
      break;
  case 2:
      shakeArm(armDown, 13, 200);
      break;
  case 3:
  //wiggle arm like laughing
    moveArm(armStraightAhead, reverse, 125);
    moveArm(armDown, forward, 100);
    moveArm(armStraightAhead, reverse, 125);
    moveArm(armDown, forward, 100);
    moveArm(armUp, reverse, 150);
    delay(300);
    moveArm(armDown, forward, 100);
    moveArm(armStraightAhead, reverse, 125);
    moveArm(armDown, forward, 100);
    moveArm(armStraightAhead, reverse, 125);
    moveArm(armDown, forward, 100);
    moveArm(armStraightAhead, reverse, 125);
    moveArm(armDown, forward, 80);
    delay(500);
    break;
  case 4:
    fallBack();
    break;
  case 5:
  //shake body uncontrollably
    shakeLeg(bodyStraight, 5, 200);
    delay(50);
    break;
  case 6:
  //then kick legs uncontrollably
    moveArm(armDown,forward,150);
    shakeLeg(bodyStraight, 13, 255);
    delay(100);
    break;
  case 7:
    standUp();
    stepNumber=0;
    break;

  }
}
//*************************
// FUNDAMENTAL MOTOR SKILLS:
void moveArm(int armPosition, int whichWay, int howFast) {
//Description:
// Rotate arm motor in direction "whichWay" at speed "howFast" until
// the arm reaches "armPosition."
// Choices for armPosition: armReachBack, armUp, armStraightAhead, armDown.
// Choices for whichWay: forward, reverse.
// Choices for howFast: any number from 80 (very slow and weak) to 255 (max speed and power)

  //Set for rotation in the direction of "whichWay":
    int motorPlus = whichWay;
    int motorMinus = ~whichWay & 1;
    digitalWrite(armMotorPlus, motorPlus); 
    digitalWrite(armMotorMinus, motorMinus);

  //Begin rotating at speed "howFast":
    analogWrite(armSpeed, howFast);

  //Stop rotation when arm reaches "armPosition":
    int sensor = 1;
    while(sensor==HIGH) {
      sensor = digitalRead(armPosition);
      }
    analogWrite(armSpeed,0);
}
void bendBody(int bodyPosition, int whichWay, int howFast) {
//Description:
// Rotate leg motor in direction "whichWay" at speed "howFast" until
// the body reaches "bodyPosition."
// Choices for bodyPosition: bodyBentMax, bodyBent90, bodyBentMin, bodyStraight, bodyKickedBack.
// Choices for whichWay: forward, reverse.
// Choices for howFast: any number from 80 (very slow and weak) to 255 (max speed and power)

  //Set for rotation in the direction of "whichWay":
    int motorPlus = whichWay;
    int motorMinus = ~whichWay & 1;
    digitalWrite(legMotorPlus, motorPlus); 
    digitalWrite(legMotorMinus, motorMinus);

  //Begin rotating at speed "howFast":    
    analogWrite(legSpeed,howFast); // Turn motor on at speed "howFast"

  //Stop rotation when body reaches "bodyPosition":
    int sensor = 1;
    while(sensor==1) {
      sensor = digitalRead(bodyPosition);
      }
    analogWrite(legSpeed,0);
}
//*************************
// BASIC MOVES:

void fallBack() {
//Description:
// Elmo falls down onto his butt, then lies down on his back.

  //initialize arm position:
    moveArm(armReachBack, reverse, 225);
    delay(300);
    moveArm(armUp, forward, 100);
  //fall back by bending forward medium speed, then standing up really fast
    bendBody(bodyBent90, forward, 175);
    bendBody(bodyStraight, reverse, 255);
    delay(100);
  //bend while falling to land on butt instead of on back
    bendBody(bodyBent90, forward, 150);
    delay(500); //Elmo is now sitting down
    bendBody(bodyStraight, reverse, 120);
    delay(800); // Elmo is now flat on his back
}

void standUp() {
//Description:
// Elmo bends forward really fast to get his feet flat on the floor, then stands straight.
    moveArm(armReachBack, reverse, 225);
    delay(100);
    moveArm(armUp, forward, 100);
    bendBody(bodyKickedBack, reverse, 255); // Prep for standing up
    delay(300); 
    bendBody(bodyBentMax, forward, 255); // shoot forward in bent position!
    delay(800);
    bendBody(bodyStraight, reverse, 200); // stand straight
}

void shakeLeg(int bodyPosition, int numShakes, int howFast) {
//Description:
// Body rotates forward, then reverse, numShakes times.
// The decision of when to switch from forward to reverse is based on delay times,
// not on position sensor readings. The "shakeLeg" and "shakeArm" functions
// use very small delay times to allow for small movements.
// The final movement is to rotate in reverse. This movement is stopped when
// the body reaches bodyPosition.
// NOTE: Elmo's behavior during "shakeLeg" depends on the position of the arm.
//       1. If the arm is in the armDown position when this function is called,
//          Elmo "shakes" by scissor-kicking his legs (body remains straight).
//       2. If the arm is in any other position when this function is called,
//          Elmo "shakes" by moving rotating his body (his legs are rotating in unison).

  //Initialize body position
  bendBody(bodyStraight,reverse,150);

  //Set for forward rotation on 1st shake, reverse rotation on 2nd shake, forward rotation on 3rd shake, etc.:
  int positivePin = 0;
  int negativePin = 1;
  numShakes = 2*numShakes; //ensures an even number of shakes, so that final direction of rotation is reverse
  int i;
  for (i=0;i<numShakes;i++) {
    positivePin = ~positivePin & 1;
    negativePin = ~negativePin & 1;
    if (i < 2) {
      delay(160);
    }
    else {
      if (positivePin == 1) {
        delay(100);
        }
      else {
        delay(85);
      }
    }
    digitalWrite(legMotorPlus, positivePin); 
    digitalWrite(legMotorMinus, negativePin);
    analogWrite(legSpeed,howFast); // Turn motor on at speed "howFast"    
  }
  //Shaking is finished, but Elmo's body is still rotating in reverse.
  // Continue the rotation until Elmo returns to bodyPosition.

  int sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(bodyPosition);
  }
  //When Elmo has returned to bodyPosition, stop moving:
  analogWrite(legSpeed,0);
}

void shakeArm(int armPosition, int numShakes, int howFast) {
//Description:
// Arm rotates forward, then reverse, numShakes times.
// The final movement is to rotate in reverse. This movement is stopped when
// the arm reaches armPosition.
  int sensor; //stores value of armPosition pin
  int positivePin; //sets polarity of motor
  int negativePin;

//FIRST, INITIALIZE STARTING POSITION:
  moveArm(armDown,forward,150);
  moveArm(armStraightAhead,reverse,150);

  //Set for forward rotation on 1st shake, reverse rotation on 2nd shake, forward rotation on 3rd shake, etc.:
  positivePin = 0;
  negativePin = 1;
  numShakes = 2*numShakes;  //ensures an even number of shakes, so that final direction of rotation is reverse
  int i;
  for (i=0;i<numShakes;i++) {
    delay(100);
    positivePin = ~positivePin & 1;
    negativePin = ~negativePin & 1;
    digitalWrite(armMotorPlus, positivePin); 
    digitalWrite(armMotorMinus, negativePin);
    analogWrite(armSpeed,howFast); // Turn motor on at speed "howFast"    
  }
  //When Elmo's arm has returned to armPosition, stop moving:
  sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(armPosition);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(armSpeed,0);
}

//FOR DETECTING BUTTON PRESSES AND INCREMENTING STEP NUMBER
//**********************************************
int waitForButton(int whichStep) {
    state();
  if (stateChange == 1 && nowState == 1){
//FOR DEBUGGING:    digitalWrite(ledPin,LOW);
    previousState=nowState;
    state();
  }
  else if (stateChange == 1 && nowState == 0){
//FOR DEBUGGING:    digitalWrite(ledPin,HIGH);
    previousState=nowState;
    state();
    whichStep = whichStep+1;
  }
      return whichStep;
}

void state(){
  nowState=digitalRead(switchPin);
  if (nowState != previousState){
    stateChange=1;
  }
  else {
    stateChange=0;
  }
}
//*********************************************

This code reads in the stepnumber from the master:

/*
TO USE THIS CODE:
0. This code calls header file "declareElmoSlavePins.h".
   A. Copy the header file from the wiki link titled "Elmo Slave Header File," and paste into a plain text file.
   B. Save the text file as "declareElmoSlavePins" and close the text file.
   C. Rename the text file to have a ".h" extension (on the Mac, this is most easily done from the Terminal program
      using the "mv" command).
   D. Move this file into your Arduino folder
      (the Arduino folder is usually in "My Documents" on the PC, or in "Documents" on the Mac).
   E. Copy the code from this wiki page and paste it into a new Arduino sketch. Save the file with whatever name you want.
   F. Click on the drop-down menu "Sketch" and select "Add File...". Select the file "declareElmoSlavePins.h"
1. This code was written to test choreography on an ElmoSlave.
   It is assumed that you have an ElmoSlave with an Arduino board strapped to its head,
   so that you can easily upload code to try out new things. It is also assumed that your ElmoSlave
   is activated by pressing its foot-button.
   Wiring up Elmo's foot-button:
   black wire from foot: connect to ground (any ground available on pcb or arduino board)
   green wire from foot: connect to switchPin (whatever pin you pick on the Arduino to detect that the switch was pressed)
2. Modify the code to creat a new choreography, turn on the ElmoSlave, plug in the USB cable to connect
   your computer to the ElmoSlave, and upload your new code.
3. Press the foot-button and watch ElmoSlave perform the choreography.
4. Fine-tune the code by adjusting the delay times and motor speeds as needed.

AUTHORS: Mike Gazes, Nick Wong, Soyoung Park, Eric Beug
DATE: 13 February 2009
TO DO:
  1. Clean up functions. Create master functions for fundamental movements, then a library of
     specialized functions for specific movements.
See additional comments at end of file.
 */

//DECLARE VARIABLES AND PINS
//******************************************************

    //VARIABLES USED ONLY FOR CHOREOGRAPHY TESTING (FOOT-BUTTON ACTIVATED)
    // DELETE THIS SECTION FROM THE FINAL ELMO SLAVE CODE (WIRELESS ACTIVATED)
    // Beginning of variables for choreography testing:
    //**********************************************************
    //Switch Detection Pins:
    int switchPin = 1;

    //Switch Detection State Variables:
    int currentStep; //keep track of which movement we are up to
    int stepNumber=0; //keep track of which movement to do next
    int previousState=0;
    int nowState=0;
    int stateChange=0;
    // End of variables for choreography testing
    //*********************************************************
//Gyroscope Feedback Pins:
int gyroGreen = 13; // supine (this wire from the gyro is disconnected while we do tests)
int gyroYellow = 19; // upright
int gyroRed = 12; // prone
//brown is ground, and orange is not connected

# include "declareElmoSlavePins.h" // See this header file for all other pin declarations



//Motor Variables:
int forward=1;
int reverse=0;

//Wireless Communication Variables:
int incomingByte = 0;

void setup() {
Serial.begin(9600);
//DECLARE INPUTS/OUTPUTS

//MOTORS
//*******************************************************
  //Arm motor output pins:
    pinMode(armSpeed, OUTPUT);
    pinMode(armMotorPlus, OUTPUT);
    pinMode(armMotorMinus, OUTPUT);

  //State of arm motor on startup:
    digitalWrite(armSpeed, LOW); //Disable motor
    //Set for forward rotation:
      digitalWrite(armMotorPlus, HIGH);
      digitalWrite(armMotorMinus, LOW);

  //Leg motor output pins:
    pinMode(legSpeed, OUTPUT);
    pinMode(legMotorPlus, OUTPUT);
    pinMode(legMotorMinus, OUTPUT);

  //State of leg motor on startup:
    digitalWrite(legSpeed, LOW); //Disable motor
    //Set for forward rotation:
      digitalWrite(legMotorPlus, HIGH);
      digitalWrite(legMotorMinus, LOW);
//*******************************************************

//SENSORS
//*******************************************************
  //Motor Sensor Feedback Pins for arm position and leg position (see header file "declareElmoSlavePins.h")
    //Set all sensor pins as inputs
      pinMode(armReachBack, INPUT);
      pinMode(armUp, INPUT);
      pinMode(armStraightAhead, INPUT);
      pinMode(armDown, INPUT);
      pinMode(bodyBentMax, INPUT);
      pinMode(bodyBent90, INPUT);
      pinMode(bodyBentMin, INPUT);
      pinMode(bodyStraight, INPUT);
      pinMode(bodyKickedBack, INPUT);
    //Enable internal pull-up resistors on all sensor pins
      digitalWrite(armReachBack, HIGH);
      digitalWrite(armUp, HIGH);
      digitalWrite(armStraightAhead, HIGH);
      digitalWrite(armDown, HIGH);
      digitalWrite(bodyBentMax, HIGH);
      digitalWrite(bodyBent90, HIGH);
      digitalWrite(bodyBentMin, HIGH);
      digitalWrite(bodyStraight, HIGH);
      digitalWrite(bodyKickedBack, HIGH);
//*******************************************************

}
void loop() {
  if (Serial.available() > 0) {
      incomingByte = Serial.read();
      stepNumber = incomingByte;
      doNextStep(stepNumber);
  }
}
//FUNCTION DEFINITIONS
//******************************************************************************

//CHOREOGRAPHY ROUTINE
//*********************************************
void doNextStep(int whichStep) {
  switch(whichStep) {
//BREAK UP THE ROUTINE INTO AS MANY STEPS AS YOU WANT. EACH SEPARATE STEP BEGINS WITH A case STATEMENT.
//THE FINAL CASE STATEMENT MUST RESET STEP NUMBER TO 0.

  case 1:
    moveArm(armReachBack,reverse,255);
    break;
  case 2:
    moveArm(armDown,forward,255);
    break;
  case 3:
    moveArm(armReachBack,reverse,255);
    break;
  case 4:
    moveArm(armDown,forward,255);
    break;
  case 5:
    moveArm(armReachBack,reverse,255);
    break;
  case 6:
    moveArm(armDown,forward,255);
    break;
  case 7:
    moveArm(armReachBack,reverse,255);
    moveArm(armDown,forward,255);
    break;
  }
}
//*************************
// FUNDAMENTAL MOTOR SKILLS:
void moveArm(int armPosition, int whichWay, int howFast) {
//Description:
// Rotate arm motor in direction "whichWay" at speed "howFast" until
// the arm reaches "armPosition."
// Choices for armPosition: armReachBack, armUp, armStraightAhead, armDown.
// Choices for whichWay: forward, reverse.
// Choices for howFast: any number from 80 (very slow and weak) to 255 (max speed and power)

  //Set for rotation in the direction of "whichWay":
    int motorPlus = whichWay;
    int motorMinus = ~whichWay & 1;
    digitalWrite(armMotorPlus, motorPlus); 
    digitalWrite(armMotorMinus, motorMinus);

  //Begin rotating at speed "howFast":
    analogWrite(armSpeed, howFast);

  //Stop rotation when arm reaches "armPosition":
    int sensor = 1;
    while(sensor==HIGH) {
      sensor = digitalRead(armPosition);
      }
    analogWrite(armSpeed,0);
}
void bendBody(int bodyPosition, int whichWay, int howFast) {
//Description:
// Rotate leg motor in direction "whichWay" at speed "howFast" until
// the body reaches "bodyPosition."
// Choices for bodyPosition: bodyBentMax, bodyBent90, bodyBentMin, bodyStraight, bodyKickedBack.
// Choices for whichWay: forward, reverse.
// Choices for howFast: any number from 80 (very slow and weak) to 255 (max speed and power)

  //Set for rotation in the direction of "whichWay":
    int motorPlus = whichWay;
    int motorMinus = ~whichWay & 1;
    digitalWrite(legMotorPlus, motorPlus); 
    digitalWrite(legMotorMinus, motorMinus);

  //Begin rotating at speed "howFast":    
    analogWrite(legSpeed,howFast); // Turn motor on at speed "howFast"

  //Stop rotation when body reaches "bodyPosition":
    int sensor = 1;
    while(sensor==1) {
      sensor = digitalRead(bodyPosition);
      }
    analogWrite(legSpeed,0);
}
//*************************
// BASIC MOVES:

void fallBack() {
//Description:
// Elmo falls down onto his butt, then lies down on his back.

  //initialize arm position:
    moveArm(armReachBack, reverse, 225);
    delay(300);
    moveArm(armUp, forward, 100);
  //fall back by bending forward medium speed, then standing up really fast
    bendBody(bodyBent90, forward, 175);
    bendBody(bodyStraight, reverse, 255);
    delay(100);
  //bend while falling to land on butt instead of on back
    bendBody(bodyBent90, forward, 150);
    delay(500); //Elmo is now sitting down
    bendBody(bodyStraight, reverse, 120);
    delay(800); // Elmo is now flat on his back
}

void standUp() {
//Description:
// Elmo bends forward really fast to get his feet flat on the floor, then stands straight.
    moveArm(armReachBack, reverse, 225);
    delay(100);
    moveArm(armUp, forward, 100);
    bendBody(bodyKickedBack, reverse, 255); // Prep for standing up
    delay(300); 
    bendBody(bodyBentMax, forward, 255); // shoot forward in bent position!
    delay(800);
    bendBody(bodyStraight, reverse, 200); // stand straight
}

void shakeLeg(int bodyPosition, int numShakes, int howFast) {
//Description:
// Body rotates forward, then reverse, numShakes times.
// The decision of when to switch from forward to reverse is based on delay times,
// not on position sensor readings. The "shakeLeg" and "shakeArm" functions
// use very small delay times to allow for small movements.
// The final movement is to rotate in reverse. This movement is stopped when
// the body reaches bodyPosition.
// NOTE: Elmo's behavior during "shakeLeg" depends on the position of the arm.
//       1. If the arm is in the armDown position when this function is called,
//          Elmo "shakes" by scissor-kicking his legs (body remains straight).
//       2. If the arm is in any other position when this function is called,
//          Elmo "shakes" by moving rotating his body (his legs are rotating in unison).

  //Initialize body position
  bendBody(bodyStraight,reverse,150);

  //Set for forward rotation on 1st shake, reverse rotation on 2nd shake, forward rotation on 3rd shake, etc.:
  int positivePin = 0;
  int negativePin = 1;
  numShakes = 2*numShakes; //ensures an even number of shakes, so that final direction of rotation is reverse
  int i;
  for (i=0;i<numShakes;i++) {
    positivePin = ~positivePin & 1;
    negativePin = ~negativePin & 1;
    if (i < 2) {
      delay(160);
    }
    else {
      if (positivePin == 1) {
        delay(100);
        }
      else {
        delay(85);
      }
    }
    digitalWrite(legMotorPlus, positivePin); 
    digitalWrite(legMotorMinus, negativePin);
    analogWrite(legSpeed,howFast); // Turn motor on at speed "howFast"    
  }
  //Shaking is finished, but Elmo's body is still rotating in reverse.
  // Continue the rotation until Elmo returns to bodyPosition.

  int sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(bodyPosition);
  }
  //When Elmo has returned to bodyPosition, stop moving:
  analogWrite(legSpeed,0);
}

void shakeArm(int armPosition, int numShakes, int howFast) {
//Description:
// Arm rotates forward, then reverse, numShakes times.
// The final movement is to rotate in reverse. This movement is stopped when
// the arm reaches armPosition.
  int sensor; //stores value of armPosition pin
  int positivePin; //sets polarity of motor
  int negativePin;

//FIRST, INITIALIZE STARTING POSITION:
  moveArm(armDown,forward,150);
  moveArm(armStraightAhead,reverse,150);

  //Set for forward rotation on 1st shake, reverse rotation on 2nd shake, forward rotation on 3rd shake, etc.:
  positivePin = 0;
  negativePin = 1;
  numShakes = 2*numShakes;  //ensures an even number of shakes, so that final direction of rotation is reverse
  int i;
  for (i=0;i<numShakes;i++) {
    delay(100);
    positivePin = ~positivePin & 1;
    negativePin = ~negativePin & 1;
    digitalWrite(armMotorPlus, positivePin); 
    digitalWrite(armMotorMinus, negativePin);
    analogWrite(armSpeed,howFast); // Turn motor on at speed "howFast"    
  }
  //When Elmo's arm has returned to armPosition, stop moving:
  sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(armPosition);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(armSpeed,0);
}
Edit - History - Print - Recent Changes - Search
Page last modified on February 17, 2009, at 02:16 PM