go_away

Author Topic: Arduino Code for Stampy algorithm  (Read 2498 times)

0 Members and 1 Guest are viewing this topic.

Offline jkflorekTopic starter

  • Beginner
  • *
  • Posts: 3
  • Helpful? 0
Arduino Code for Stampy algorithm
« on: March 02, 2009, 06:12:54 PM »
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

Code: [Select]
/****************************************************************************\
*                                                                            *
*    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
}

Offline jkflorekTopic starter

  • Beginner
  • *
  • Posts: 3
  • Helpful? 0
Re: Arduino Code for Stampy algorithm
« Reply #1 on: March 04, 2009, 06:31:33 PM »
no comments? :(

Offline Admin

  • Administrator
  • Supreme Robot
  • *****
  • Posts: 11,663
  • Helpful? 169
    • Society of Robots
Re: Arduino Code for Stampy algorithm
« Reply #2 on: March 19, 2009, 03:25:30 AM »
I think you should test it on a robot to see if it works before asking if it works or not :P

Plus, its already been ported to the ATmega8! ;D
http://www.societyofrobots.com/robot_50_robot_sharpIR.shtml

 


Get Your Ad Here

data_list