Recent Changes - Search:

ENTER NEW DAILY DATA HERE:

PmWiki

pmwiki.org

edit SideBar

ElmoSensorColor-PositionTable

Leg Sensors:

Blue - Bent Below 90 Degrees (most bent) Green - Bent 90 Degrees Yellow - Bent Above 90 Degrees (least bent) Orange - Stand Straight Red - Leg Kicks Back (Brown in ground)

New Code (30 Jan 2009) to trigger movements using Elmo's built-in push-button. A new move is introduced in this code: shaking.

/*
 getting signal from master Xbee. moves step by step.
 AUTHOR: mike  & nick
 DATE: Jan 30th 2009
 PURPOSE:
 1. Perform one movement from a routine, each time Elmo's foot-button is pushed.

 TO DO:
 1. Find out how to move all the variables for pin declarations to an external file
 */

//DECLARE VARIABLES
//******************************************************

//Motor Feedback Pins:
int armSensorBrown = 6; //red wire -20 180+
int armSensorRed = 7; //white wire 0 90~180
int armSensorYellow = 8; //yellow wire 90 90
int armSensorGreen = 9; //grey wire 150 0
int legSensorBlue = 14; //blue wire 135
int legSensorGreen = 15; //green wire 90
int legSensorYellow = 16; //yellow wire 30
int legSensorOrange = 17; //orange wire 0
int legSensorRed = 18; //red wire -30

//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

//Switch Detection Pins:
int switchPin = 1;
//FOR DEBUGGING: int ledPin=13; //user feedback: lights while switch is depressed
//Connections to 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

//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;

//Motor Output Pins:
int armPwmPin = 10; // L293 pin 1
int armMtrPosPin = 2; // L293 pin 2
int armMtrNegPin = 3; // L293 pin 7
int legPwmPin = 11; // L293 pin 9
int legMtrPosPin = 4; // L293 pin 15
int legMtrNegPin = 5; // L293 pin 10
//Connections to drive motors:
// L293 pin 3 - Arm Motor + (green wire)
// L293 pin 6 - Arm Motor - (white wire)
// L293 pin 14 - Leg Motor + (yellow wire)
// L293 pin 11 - Leg Motor - (brown wire)

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



void setup() {
  //Motor Output Pins:
  pinMode(armPwmPin, OUTPUT); //Set pin 3 as an output
  pinMode(armMtrPosPin, OUTPUT); //Set pin 2 as an output
  pinMode(armMtrNegPin, OUTPUT); //Set pin 4 as an output
  digitalWrite(armPwmPin, LOW); //Disable motor on startup
  digitalWrite(armMtrPosPin, HIGH); //Prepare for clockwise motor rotation
  digitalWrite(armMtrNegPin, LOW); //Prepare for clockwise motor rotation

  pinMode(legPwmPin, OUTPUT); //Set pin 3 as an output
  pinMode(legMtrPosPin, OUTPUT); //Set pin 2 as an output
  pinMode(legMtrNegPin, OUTPUT); //Set pin 4 as an output
  digitalWrite(legPwmPin, LOW); //Disable motor on startup
  digitalWrite(legMtrPosPin, HIGH); //Prepare for clockwise motor rotation
  digitalWrite(legMtrNegPin, LOW); //Prepare for clockwise motor rotation

  //Motor Feedback Pins: for arm
  pinMode(armSensorRed, INPUT); //Set pin 7 as an input
  digitalWrite(armSensorRed, HIGH); //Enable internal pull-up resistor
  pinMode(armSensorBrown, INPUT);  //Set pin 8 as an input
  digitalWrite(armSensorBrown, HIGH);  //Enable internal pull-up resistor
  pinMode(armSensorYellow, INPUT);  //Set pin 9 as an input
  digitalWrite(armSensorYellow, HIGH);  //Enable internal pull-up resistor
  pinMode(armSensorGreen, INPUT);  //Set pin 10 as an input
  digitalWrite(armSensorGreen, HIGH);  //Enable internal pull-up resistor
  //Motor Feedback Pins: for legs
  pinMode(legSensorBlue, INPUT); //Set pin 7 as an input
  digitalWrite(legSensorBlue, HIGH); //Enable internal pull-up resistor
  pinMode(legSensorGreen, INPUT);  //Set pin 8 as an input
  digitalWrite(legSensorGreen, HIGH);  //Enable internal pull-up resistor
  pinMode(legSensorYellow, INPUT);  //Set pin 9 as an input
  digitalWrite(legSensorYellow, HIGH);  //Enable internal pull-up resistor
  pinMode(legSensorOrange, INPUT);  //Set pin 10 as an input
  digitalWrite(legSensorOrange, HIGH);  //Enable internal pull-up resistor
  pinMode(legSensorRed, INPUT);  //Set pin 10 as an input
  digitalWrite(legSensorRed, HIGH);  //Enable internal pull-up resistor


}
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);

}
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;
  }
}

/*
***********************************
//OLD ROUTINES:
void doNextStep(int whichStep) {
  switch(whichStep) {
  case 1:
    moveArm(armSensorRed, reverse, 150);
    break;
  case 2:
    moveArm(armSensorGreen, forward, 150);
    break;
  case 3:
    moveArm(armSensorRed, reverse, 150);
    break;
  case 4:
    moveArm(armSensorGreen, forward, 150);
    break;
  case 5:
    moveArm(armSensorYellow, reverse, 150);      
    break;
  case 6:
    moveLeg(legSensorGreen,forward, 200);
    break;
  case 7:
    stepNumber = 0;
    moveLeg(legSensorOrange, reverse, 200);
    break;
  }
}
*/

void doNextStep(int whichStep) {
  switch(whichStep) {
  case 1:
    shakeArm(armSensorYellow,13,150);
    break;
  case 2:
    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 armSensorColor, int whichWay, int howFast) {
  //MOVE MOTOR IN DIRECTION OF "whichWay" (EITHER FORWARD OR REVERSE)
  int positivePin = whichWay;
  int negativePin = ~whichWay & 1;
  digitalWrite(armMtrPosPin, positivePin); 
  digitalWrite(armMtrNegPin, negativePin);
  analogWrite(armPwmPin,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(armSensorColor);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(armPwmPin,0);
}
void moveLeg(int legSensorColor, int whichWay, int howFast) {
  //MOVE MOTOR IN DIRECTION OF "whichWay" (EITHER FORWARD OR REVERSE)
  int positivePin = whichWay;
  int negativePin = ~whichWay & 1;
  digitalWrite(legMtrPosPin, positivePin); 
  digitalWrite(legMtrNegPin, negativePin);
  analogWrite(legPwmPin,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(legSensorColor);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(legPwmPin,0);
}

/*
void shakeLeg(int numShakes, int howFast) {
  //MOVE MOTOR FORWARD A LITTLE, THEN BACKWARD A LITTLE
  //REPEAT numShakes TIMES
  int positivePin = 0;
  int negativePin = 1;
  numShakes = 2*numShakes;
  int i;
  for (i=0;i<numShakes;i++) {
    positivePin = ~positivePin & 1;
    negativePin = ~negativePin & 1;
    digitalWrite(legMtrPosPin, positivePin);
    digitalWrite(legMtrNegPin, negativePin);
    analogWrite(legPwmPin,howFast);
    delay(100);
  }
    analogWrite(legPwmPin,0);
}
*/
void shakeArm(int armSensorColor, 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(armSensorGreen,forward,150);
  moveArm(armSensorYellow,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(armMtrPosPin, positivePin); 
    digitalWrite(armMtrNegPin, negativePin);
    analogWrite(armPwmPin,howFast); // Turn motor on at speed "howFast"    
  }
  //END SHAKING BY RETURNING ARM TO armSensorColor
  sensor = 1;
  while(sensor==1) {
    sensor = digitalRead(armSensorColor);
  }
  //AND WHEN ELMO'S ARM REACHES TARGET SENSOR, STOP:
  analogWrite(armPwmPin,0);
}
Edit - History - Print - Recent Changes - Search
Page last modified on January 30, 2009, at 06:32 PM