So for a robotics program of mine I'm attempting to build a 180 degree sonar scanner and so I developed a scanner library for this purpose. However, when I implement this library on a servo it causes the servo to one side of its boundaries and cease to work. When I use the Sweep example though, it works fine. If someone could help me and tell me what I'm doing wrong that'd be awesome. The relevant code is below. Let me know if you you guys need more. Thank you!
The main sketch
#include <stdlib.h>
#include "MotorController.h"
#include "DiffDrive.h"
#include "Scanner.h"
void * operator new(size_t size)
{
return malloc(size);
}
void operator delete(void * ptr)
{
free(ptr);
}
//DiffDrive drive(5, 6, 7, 8);
Scanner scanner;
void setup(){
delay(1000);
}
void loop(){
for(int pos = 0; pos < 180; pos += 1) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
scanner.servo->write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
for(int pos = 180; pos>=1; pos-=1) // goes from 180 degrees to 0 degrees
{
scanner.servo->write(pos); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
The Scanner library.
/*
Keith Elliott
Created: 10/21/11
DESC: A library for controlling a rotating scanner with a servo and distance sensors
*/
#include "Scanner.h"
Scanner::Scanner(){
//sonar = new USSensor();
//sonar->init();
servo = new Servo();
servo->attach(10);
//sweep = new Sweep();
}
Sweep Scanner::doSweep(int inc){
servo->write(SERVO_MIN);
delay(200);
if (inc > 0){
for (int i = SERVO_MIN; i <= SERVO_MAX; i+=inc){
servo->write(i);
delay(200);
sweep->addNode(sonar->getDist(), i-CENT_OFF);
}
}
else if (inc < 0){
for (int i = SERVO_MAX; i >= SERVO_MIN; i+=inc){
servo->write(i);
delay(200);
sweep->addNode(sonar->getDist(), i-CENT_OFF);
}
}
}
int Scanner::scan(int threshold, unsigned int timeout){
unsigned long start = millis();
int i;
while (1){
for (i = CENT_OFF; i <= SERVO_MAX; i+=30){
if (sonar->getDist() < threshold)
return i-CENT_OFF;
}
for (; i >= SERVO_MIN; i-=30){
if (sonar->getDist() < threshold)
return i-CENT_OFF;
}
for (; i <= CENT_OFF; i+=30){
if (sonar->getDist() < threshold)
return i-CENT_OFF;
}
if (millis() - start >= timeout*1000)
return 0;
}
}