home project text outofthebox work wiki code sample credits

CODE FOR CONTROL OF ARM AND LEG

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