Lab 7 : Servo Motors Crawler
by Hilfi Alkaff, Victor Tjhia, Albert Tjoeng
Description:
Creating a crawler with two legs using two servo motors. We use 2 potentiometer for input, 1 to increase/decrease the speed of both motors, and the other one to make the crawler turn left or right (i.e turning right: increase the speed of the left leg and reduce speed of right leg). Furthermore, we add 2 IR proximity sensors, facing left and right, so that the crawler will avoid object on its way. The crawler will stop moving when both sensors detect object in their proximity
Materials:
Proximity Sensor
Servo motor
Wooden stick
Plastic bowl
CODE:
/*
* Servo with Potentiometer control
* Theory and Practice of Tangible User Interfaces
* October 11 2007
*/
int servoPinR = 7;
int servoPinL = 4;// Control pin for servo motor
int potPinS = 0; // select the input pin for the potentiometer
int pulseWidthR = 0; // Amount to pulse the servo
int pulseWidthL = 0;
long lastPulseR = 0; // the time in millisecs of the last pulse
long lastPulseL = 0;
int refreshTime = 20; // the time in millisecs needed in between pulses
int speedVal; // variable used to store data from potentiometer
int yawVal;
int delta;
int cR;
int cL;
int irR;
int irL;
int minPulse = 500; // minimum pulse width
int maxPulse = 2500;
void setup() {
pinMode(servoPinR, OUTPUT); // Set servo pin as an output pin
pinMode(servoPinL, OUTPUT);
pulseWidthR = minPulse; // Set the motor position to the minimum
pulseWidthL = maxPulse;
Serial.begin(9600); // connect to the serial port
Serial.println("servo_serial_better ready");
}
void loop() {
speedVal = analogRead(0); // read the value from the sensor, between 0 - 1024
speedVal = map(speedVal,0,1023,20,35);
yawVal = analogRead(1);
yawVal = map(yawVal,0,1023,0,20);
delta = yawVal - 10;
irR = analogRead(4);
irL = analogRead(5);
if (irR > 400 && irL > 400) speedVal = 0;
else if (irR > 400) delta = delta - 15;
else if (irL > 400) delta = delta + 15;
if (pulseWidthR < 520) cR = speedVal - delta ;
else if (pulseWidthR > 2480) cR = -speedVal + delta;
if (pulseWidthL < 520) cL = speedVal + delta;
else if (pulseWidthL > 2480) cL = -speedVal - delta;
pulseWidthR = pulseWidthR + cR;
pulseWidthL = pulseWidthL + cL;
Serial.print(speedVal);
Serial.print("\t");
Serial.print(delta);
Serial.print("\t");
Serial.print(yawVal);
Serial.print("\t");
Serial.print(pulseWidthR);
Serial.print("\t");
Serial.print(irL);
Serial.print("\t");
Serial.print(irR);
Serial.print("\n");
updateServoR(); // update servo position
updateServoL();
}
// called every loop().
void updateServoR() {
// pulse the servo again if the refresh time (20 ms) has passed:
if (millis() - lastPulseR >= refreshTime) {
digitalWrite(servoPinR, HIGH); // Turn the motor on
delayMicroseconds(pulseWidthR); // Length of the pulse sets the motor position
digitalWrite(servoPinR, LOW); // Turn the motor off
lastPulseR = millis(); // save the time of the last pulse
}
}
void updateServoL() {
// pulse the servo again if the refresh time (20 ms) has passed:
if (millis() - lastPulseL >= refreshTime) {
digitalWrite(servoPinL, HIGH); // Turn the motor on
delayMicroseconds(pulseWidthL); // Length of the pulse sets the motor position
digitalWrite(servoPinL, LOW); // Turn the motor off
lastPulseL = millis(); // save the time of the last pulse
}
}