I decided to try and port the Stampy algorithm to Arduino code, could you guys check it out and see if I made any errors?
Thanks,
jkflorek
/****************************************************************************\
* *
* Copyright (c) 2009 Jeff Florek <[email protected] *
* Based off of code found at http://www.societyofrobots.com/ *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License version 2 as *
* published by the Free Software Foundation. *
* *
* Alternatively, this software may be distributed under the terms of BSD *
* license. *
* *
* $50 Robot with Arduino MCU and Sharp IR using Stampy Technology, *
* May 2nd, 2009 *
* *
* Simple case-based method for a robot to do edge detection. *
* *
\****************************************************************************/
#include <Servo.h>
Servo right_wheel;
Servo left_wheel;
Servo scanner;
int distance = 0; //holds value from IR scanner
int scan_threshold = 0; //threshold for detecting an object
int scan_angle = 90; //angle that the scanning servo is at
int led_pin = 13; //pin that status LED is connnected to
int scanner_pin = 0;
//scans for target
void scan()
{
distance = analogRead(scanner_pin);
if(distance > scan_threshold) //object detected
{
if(scan_angle > 0) //overflow protection
scan_angle -= 1; //turn scanner left
}
else //no object detected
{
if(scan_angle < 180) //overflow protection
scan_angle += 1; //turn right
}
scanner.write(scan_angle); //set servo angle to scan_angle
}
void auto_calibrate() //automatically find the target sensing threshold
{
scan_threshold = analogRead(scanner_pin);
}
void setup()
{
right_wheel.attach(3);
left_wheel.attach(5);
scanner.attach(6);
pinMode(led_pin, OUTPUT);
//Turn LED on to signal autocalibration,
//wait 2 seconds, autocalibrate, then turn LED off
digitalWrite(led_pin, HIGH);
delay(2000);
auto_calibrate();
digitalWrite(led_pin, LOW);
}
void loop()
{
scan();
//if object detected on left, turn left
if(scan_angle <= 50)
{
right_wheel.write(135);
left_wheel.write(135);
}
//if object detected on right, turn right
else if(scan_angle >= 130)
{
right_wheel.write(45);
left_wheel.write(45);
}
//object is centered, go strait
else
{
right_wheel.write(135);
left_wheel.write(45);
}
delay(15); //small delay to prevent crazy oscillations
}
