Recent Changes - Search:

ENTER NEW DAILY DATA HERE:

PmWiki

pmwiki.org

edit SideBar

10-16-arm-legCodeUpdate


/*
move together!!! Great!
AUTHOR: mike  & soyoung
 DATE: Oct 13 2008
 PURPOSE:
 1. Flash an LED three times when this code is loaded onto the arduino.
 2. Perform one movement from a routine, each time a button is pushed.
 */

//Verificatin Pin:
int ledPin = 13;
int switchPin = 12;        
int val = 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 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

//Variables:
int forward=1;
int reverse=0;
int stepNumber=0; //keep track of which movement to do next

void setup() {
  Serial.begin(9600);
  //Verificatin Pin:
  pinMode(ledPin, OUTPUT); //Set pin 13 as an output
  pinMode(switchPin, INPUT);
  //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

  blink(ledPin,3,100);    //Call the blink routine, three quick blinks to show the code is loaded
  Serial.print("Step: ");
  Serial.println(stepNumber);

}

void loop() {

  //Wait for the button to be pressed. Then execute
  // the current stepNumber, and increment the stepNumber.
  stepNumber=waitForButton(stepNumber);
  Serial.print("...doing stepNumber ");
  Serial.println(stepNumber);
  doNextStep(stepNumber);

//    
//    moveArm(armSensorGreen, forward, 100);

  }

int waitForButton(int whichStep) {

  val = digitalRead(switchPin);
  while(val == LOW){
    val = digitalRead(switchPin);
    delay(250); //debounces switch
  }
  whichStep = whichStep+1;
  Serial.print("Step: ");
  Serial.println(whichStep);
  return whichStep;
}
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 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);
}

Edit - History - Print - Recent Changes - Search
Page last modified on October 16, 2008, at 05:26 PM