I have a 360 degree continuous motion servo attached to a rod. The interrupt is one of the channel pins on a wheel watcher watching the rotation of the rod. At the top of the rod is an infrared distancer and an ultrasonic one.
#include <Servo.h>
#define SCAN_RESOLUTION 1
#define ULTRASONIC_CONTROL_PIN 7
#define ULTRASONIC_READ_PIN A1
#define INFRARED_PIN A0
unsigned int ultraSonicDistance = 0;
double infraredValue = 0.0;
double infraredDistance = 0.0;
volatile int xAngle = 0; //Goes from 0-300 covering 1 full circle
//int yAngle = 0;
Servo xServo;
//Servo yServo;
int xServoDirection = SCAN_RESOLUTION;
int yServoDirection = SCAN_RESOLUTION;
boolean highResScan = false;
double surroundingsMap[300] = {0};
void setup()
{
Serial.begin(9600);
xServo.attach(9,492,2348); //values found experimentally to make write(90) say stop
//yServo.attach(10);
attachInterrupt(0,stopXServo,FALLING);
pinMode(ULTRASONIC_CONTROL_PIN,OUTPUT);
digitalWrite(ULTRASONIC_CONTROL_PIN,LOW);
}
void loop()
{
xServo.write(90 + (xServoDirection*2)); //90 is "don't move"
Serial.print(90 + (xServoDirection*2));
Serial.print(" ");
Serial.println(xAngle);
if(highResScan)
delay(100);
}
void readInfrared()
{
infraredValue = analogRead(INFRARED_PIN);
infraredValue *= .0049;
infraredDistance = 3*pow((infraredValue-3),4) + 15;
}
void readUltrasonic()
{
digitalWrite(ULTRASONIC_CONTROL_PIN,HIGH);
delayMicroseconds(20);
ultraSonicDistance = analogRead(ULTRASONIC_READ_PIN);
digitalWrite(ULTRASONIC_CONTROL_PIN,LOW);
}
void stopXServo()
{
xServo.write(90);
readInfrared();
ultraSonicDistance = infraredDistance;
if(highResScan)
readUltrasonic();
surroundingsMap[xAngle] = (infraredDistance+ultraSonicDistance)/2;
xAngle += xServoDirection;
if(xAngle == 300)
{
xAngle = 0;
//xServoDirection *= -1;
/*yServo.write(yServo.read()+yServoDirection);
if(yAngle == 90 || yAngle == 0)
yServoDirection *= -1;*/
}
}
The serial communication stops, as does the rotation of my servo/rod setup, around xAngle = 295 to 298.