Recent Changes - Search:

ENTER NEW DAILY DATA HERE:

PmWiki

pmwiki.org

edit SideBar

SlaveChoreography-ElmosCompleteRoutine1

This code performs the first Tickle Me Elmo routine:

/*
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);
//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);
//Bend over and Fall back
    moveArm(armReachBack, reverse, 225);
    delay(300);
    moveArm(armUp, forward, 100);
    bendBody(bodyBent90, forward, 175);
    bendBody(bodyStraight, reverse, 255);
    delay(100);
    bendBody(bodyBent90, forward, 150);
    delay(500); //Elmo is sitting down
    bendBody(bodyStraight, reverse, 120);
    delay(800); // Elmo is flat on his back
    shakeLeg(bodyStraight, 5, 200); //shake body uncontrollably
    delay(50);
    moveArm(armDown,forward,150);
    shakeLeg(bodyStraight, 13, 255); //shake body uncontrollably
    delay(100);
    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
    stepNumber=0;
    break;
  }
}
//*************************
// NEW MOVEMENTS:

void blink(int whatPin, int howManyTimes, int milliSecs) {
  int i=0;
  for(i=0;i<howManyTimes;i++) {
    digitalWrite(whatPin, HIGH);
    delay(milliSecs/2);
    digitalWrite(whatPin, LOW);
    delay(milliSecs/2);
  }
}
void moveArm(int armPosition, int whichWay, int howFast) {
  //MOVE MOTOR IN DIRECTION OF "whichWay" (EITHER FORWARD OR REVERSE)
  int positivePin = whichWay;
  int negativePin = ~whichWay & 1;
  digitalWrite(armMotorPlus, positivePin); 
  digitalWrite(armMotorMinus, negativePin);
  analogWrite(armSpeed,howFast); // Turn motor on at speed "howFast"
  //WATCH FOR WHEN THE MOTOR REACHES THE NEXT TARGET SENSOR
  int sensor = 1;
  while(sensor==HIGH) {
    sensor = digitalRead(armPosition);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(armSpeed,0);
}
void bendBody(int bodyPosition, int whichWay, int howFast) {
  //MOVE MOTOR IN DIRECTION OF "whichWay" (EITHER FORWARD OR REVERSE)
  int positivePin = whichWay;
  int negativePin = ~whichWay & 1;
  digitalWrite(legMotorPlus, positivePin); 
  digitalWrite(legMotorMinus, negativePin);
  analogWrite(legSpeed,howFast); // Turn motor on at speed "howFast"
  //WATCH FOR WHEN THE MOTOR REACHES THE NEXT TARGET SENSOR
  int sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(bodyPosition);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(legSpeed,0);
}

void shakeLeg(int bodyPosition, int numShakes, int howFast) {
  int sensor;
  int positivePin;
  int negativePin;

  bendBody(bodyStraight,reverse,150);

  positivePin = 0; //set leg to rotate forward
  negativePin = 1;
  numShakes = 2*numShakes;
  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"    
  }
  //END SHAKING BY RETURNING LEG TO legSensorColor
  sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(bodyPosition);
  }
  //AND WHEN ELMO'S LEG REACHES TARGET SENSOR, STOP:
  analogWrite(legSpeed,0);
}

void shakeArm(int armPosition, int numShakes, int howFast) {
  //ARM ROTATES FORWARD, THEN REVERSE, 2*numShakes TIMES, THEN FINISHES BY CONTINUING IN REVERSE TO POSITION armSensorColor
  int sensor; //stores value of armSensorColor pin
  int positivePin; //sets polarity of motor
  int negativePin;

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

  positivePin = 0; //set arm to rotate forward
  negativePin = 1;
  numShakes = 2*numShakes; //ensures an even number of movements, so that motor is definitely in reverse at end of shaking
  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"    
  }
  //END SHAKING BY RETURNING ARM TO armSensorColor
  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;
  }
}
//*********************************************
Edit - History - Print - Recent Changes - Search
Page last modified on February 15, 2009, at 12:25 PM