| /**************************************************************************** * * Copyright (c) 2007 www.societyofrobots.com (http://www.societyofrobots.com) * (please link back if you use this code!) * * 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 Sharp IR using Stampy Technology v1, May 19th, 2007 * Simple case-based method for a robot to do edge detection. * * ****************************************************************************/ //SoR Include #include "SoR_Utils.h" //includes all the technical stuff //global variables int sharp_IR_reading=0; int scan_thresh=0;//threshold value of scanning sensor int scan_angle=30;//position of scanner, in units of servo command int max_scan_angle=56;//maximum position scanner can rotate to (57) //this function causes scanning servo to center on edge of object void scan(void) { //lower (-) goes right //higher (+) goes left //30 far right, 50 straight, 56 far left (until it jams) /*psuedocode object is detected scan right object detected object not detected scan left until object detected*/ sharp_IR_reading=a2dConvert8bit(3); if (sharp_IR_reading > scan_thresh)//object detected { if (scan_angle>41) //overflow protection scan_angle-=2;//scan right } else //object not detected { if (scan_angle<=max_scan_angle) //maximum servo angle scan_angle+=2; //scan left //else //if scanned all the way, this forces it to start over // scan_angle=30; } //servo scan code servo_scan(scan_angle); } //automatically calculates threshold value before run void autocalibrate(void) { scan_thresh=a2dConvert8bit(3);//sensor reading } int main(void) { //LED_on(); initialize(); delay_cycles(42000);//two second wait delay /*********ADD YOUR CODE BELOW THIS LINE **********/ //find thresholds autocalibrate(); //LED_off(); while(1) { scan(); //object on left if(scan_angle > 57) robot_turn_left(); //object on right else if(scan_angle < 41) robot_turn_right(); //object is centered else robot_go_straight(); delay_cycles(400);//a small delay to prevent crazy oscillations } /*********ADD YOUR CODE ABOVE THIS LINE **********/ return 0; } /*********************COMMAND LIST************************* delay_cycles(cycles); Delays - you can make your robot wait for a certain amount of time with this function. Put the number of computational cycles to delay in the (). 23 cycles is about .992 milliseconds to calculate: 23/.992*(time in milliseconds to delay) = cycles Check servo datasheet where it says: 'Direction: Clockwise/Pulse Traveling 1500 to 1900usec' servo_left(speed); and servo_right(speed); Commands your servos to rotate at a certain speed. Vary speed (which represents a delay in cycles) from 20 to 50. Left is for port D0 and right is for port D1. robot_turn_left(); and robot_turn_right(); and robot_go_straight(); Dont want to deal with speed? Just call this function and it will 'just work.' LED_on(); and LED_off(); Turns on and off your LED. The LED is on port D4. By bringing port D4 low, you are turning on the LED. variable=a2dConvert8bit(pin); Reads analog pin. For example, set 'pin' to 5 to read PC5. 'variable' will store the value. ***********************************************************/ |
This servo can operate 180° when given a pulse signal ranging from 600usec to 2400usec. Since most R/C controllers cannot generate this wide of signal range, you will need to use our servo stretcher for 180° operation.
| /**************************************************************************** * * Copyright (c) 2007 www.societyofrobots.com (http://www.societyofrobots.com) * (please link back if you use this code!) * * 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 Sharp IR using Stampy Technology v1, May 19th, 2007 * Simple case-based method for a robot to do edge detection. * * ****************************************************************************/ //SoR Include #include "SoR_Utils.h" //includes all the technical stuff //global variables int sharp_IR_reading=0; int scan_thresh=0;//threshold value of scanning sensor int scan_angle=30;//position of scanner, in units of servo command int max_scan_angle=56;//maximum position scanner can rotate to (57) //this function causes scanning servo to center on edge of object void scan(void) { //lower (-) goes right //higher (+) goes left //30 far right, 50 straight, 56 far left (until it jams) /*psuedocode object is detected scan right object detected object not detected scan left until object detected*/ sharp_IR_reading=a2dConvert8bit(3); if (sharp_IR_reading > scan_thresh)//object detected { if (scan_angle>41) //overflow protection scan_angle-=2;//scan right } else //object not detected { if (scan_angle<=max_scan_angle) //maximum servo angle scan_angle+=2; //scan left //else //if scanned all the way, this forces it to start over // scan_angle=30; } //servo scan code servo_scan(scan_angle); } //automatically calculates threshold value before run void autocalibrate(void) { scan_thresh=a2dConvert8bit(3);//sensor reading } int main(void) { //LED_on(); initialize(); delay_cycles(42000);//two second wait delay /*********ADD YOUR CODE BELOW THIS LINE **********/ //find thresholds autocalibrate(); //LED_off(); while(1) { scan(); //object on left if(scan_angle > 57) robot_turn_left(); //object on right else if(scan_angle < 41) robot_turn_right(); //object is centered else robot_go_straight(); delay_cycles(400);//a small delay to prevent crazy oscillations } /*********ADD YOUR CODE ABOVE THIS LINE **********/ return 0; } /*********************COMMAND LIST************************* delay_cycles(cycles); Delays - you can make your robot wait for a certain amount of time with this function. Put the number of computational cycles to delay in the (). 23 cycles is about .992 milliseconds to calculate: 23/.992*(time in milliseconds to delay) = cycles Check servo datasheet where it says: 'Direction: Clockwise/Pulse Traveling 1500 to 1900usec' servo_left(speed); and servo_right(speed); Commands your servos to rotate at a certain speed. Vary speed (which represents a delay in cycles) from 20 to 50. Left is for port D0 and right is for port D1. robot_turn_left(); and robot_turn_right(); and robot_go_straight(); Dont want to deal with speed? Just call this function and it will 'just work.' LED_on(); and LED_off(); Turns on and off your LED. The LED is on port D4. By bringing port D4 low, you are turning on the LED. variable=a2dConvert8bit(pin); Reads analog pin. For example, set 'pin' to 5 to read PC5. 'variable' will store the value. ***********************************************************/ |
| /**************************************************************************** * * Copyright (c) 2007 www.societyofrobots.com (http://www.societyofrobots.com) * (please link back if you use this code!) * * 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. * * SoR Utilities v1, March 10th, 2007 * ****************************************************************************/ //AVR includes #include <avr/io.h> // include I/O definitions (port names, pin names, etc) #include <avr/interrupt.h> // include interrupt support //AVRlib includes #include "global.h" // include global settings //#include "buffer.h" // include buffer function library //#include "uart.h" // include uart function library //#include "rprintf.h" // include printf function library //#include "timer.h" // include timer function library (timing, PWM, etc) #include "a2d.h" // include A/D converter function library //define port functions; example: PORT_ON( PORTD, 6); #define PORT_ON( port_letter, number ) port_letter |= (1<<number) #define PORT_OFF( port_letter, number ) port_letter &= ~(1<<number) #define PORT_ALL_ON( port_letter, number ) port_letter |= (number) #define PORT_ALL_OFF( port_letter, number ) port_letter &= ~(number) #define FLIP_PORT( port_letter, number ) port_letter ^= (1<<number) #define PORT_IS_ON( port_letter, number ) ( port_letter & (1<<number) ) #define PORT_IS_OFF( port_letter, number ) !( port_letter & (1<<number) ) //************CONFIGURE PORTS************ //configure ports for input or output - specific to ATmega8 void configure_ports(void) { DDRC = 0x00; //configure all C ports for input PORTC = 0x00; //make sure pull-up resistors are turned off DDRD = 0xFF; //configure all D ports for output } //*************************************** //************DELAY FUNCTIONS************ //wait for X amount of cycles (23 cycles is about .992 milliseconds) //to calculate: 23/.992*(time in milliseconds) = number of cycles //or (number of cycles)*.992/23 = time in milliseconds void delay_cycles(unsigned long int cycles) { while(cycles > 0) cycles--; } //*************************************** //*********SIMPLIFIED FUNCTIONS********** //functions to make coding easier for a beginner //but could cause port mixup confusion for intermediate users void LED_on(void) { PORT_OFF(PORTD, 4);//turn LED on } void LED_off(void) { PORT_ON(PORTD, 4);//turn LED off } void servo_left(signed long int speed) { PORT_ON(PORTD, 0); delay_cycles(speed); PORT_OFF(PORTD, 0);//keep off delay_cycles(200); } void servo_right(signed long int speed) { PORT_ON(PORTD, 1); delay_cycles(speed); PORT_OFF(PORTD, 1);//keep off delay_cycles(200); } void servo_scan(signed long int speed) { PORT_ON(PORTD, 2); delay_cycles(speed); PORT_OFF(PORTD, 2);//keep off delay_cycles(200); } void robot_turn_left(void) { servo_left(25); servo_right(25); } void robot_turn_right(void) { servo_left(44); servo_right(44); } void robot_go_straight(void) { servo_left(25); servo_right(44); } void hold_position(void)//37 { servo_left(39);//39 servo_right(35);//35 } //*************************************** //*************INITIALIZATIONS*********** void initialize(void) { //other stuff Im experimenting with for SoR //uartInit(); // initialize the UART (serial port) //uartSetBaudRate(9600);// set the baud rate of the UART for our debug/reporting output //rprintfInit(uartSendByte);// initialize rprintf system //timerInit(); // initialize the timer system configure_ports(); // configure which ports are analog, digital, etc. a2dInit(); // initialize analog to digital converter (ADC) a2dSetPrescaler(ADC_PRESCALE_DIV32); // configure ADC scaling a2dSetReference(ADC_REFERENCE_AVCC); // configure ADC reference voltage //rprintf("Initialization Complete\r\n"); } //**************************************** |
int scan_angle=10;//position of scanner, in units of servo command
int max_scan_angle=56;//maximum position scanner can rotate to (57) if (sharp_IR_reading > scan_thresh)//object detected
{
if (scan_angle>41) //overflow protection << HERE
scan_angle-=2;//scan right
} while(1)
{
scan();
//object on left
if(scan_angle > 57) << SET THIS TO YOUR max_scan_angle VALUE
robot_turn_left();
//object on right
else if(scan_angle < 41) << AND THIS TO YOUR NEW LOWER LIMIT
robot_turn_right();
//object is centered
else
robot_go_straight();
delay_cycles(400);//a small delay to prevent crazy oscillations
}int max_scan_angle=56;//maximum position scanner can rotate to