Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
173 lines (157 sloc) 5.79 KB
#include <Servo.h>
Servo elservo;
Servo roservo;
String str;
int x;
int xmap;
int xmap2;
int inByte = 800; // incoming serial byte
int angle = 90;
int position = 90;
int elfeedbackPin = 1;
int rofeedbackPin = 0;
int val = 0;
int rocalVal[] = {80, 445}; // initial cal values
int elcalVal[] = {80, 445}; // initial cal values
#define NUMREADINGS 10 // Number of readings to take for smoothing
int readings[NUMREADINGS]; // the readings from the analog input
int index = 0; // the index of the current reading
int total = 0; // the running total
int average = 0; // the average
void setup()
{
Serial.begin(9600);
Serial.setTimeout(200);
elservo.attach(9);
roservo.attach(8);
//roservo.writeMicroseconds(1000);
}
void loop() {
position = analogRead(elfeedbackPin);
position = smooth(position);
if (Serial.available() > 0) {
str = Serial.readStringUntil(';');
x = Serial.parseInt();
Serial.println(str);
if (str == "e"){
Serial.println(x);
//x = Serial.parseInt();
// elevation servo
// map the range of angles to the servo
//xmap = map(x, 0, 180, 700, 2300);
//Serial.println(xmap);
elservo.writeMicroseconds(x);
Serial.println("Adjusting Elevation");
delay(1000);
Serial.print("Elevation adjusted to: ");
Serial.println(analogRead(elfeedbackPin));
}
else if (str == "r"){
Serial.println(x);
//x = Serial.parseInt();
// rotation servo
//xmap = map(x, 0, 180, 500, 2400);
roservo.writeMicroseconds(x);
Serial.println("Adjusting Rotation");
delay(1000);
Serial.print("Rotation adjusted to: ");
Serial.println(analogRead(rofeedbackPin));
}
else if (str == "cale"){
// calibrate
//Serial.println(x);
//x = Serial.parseInt();
// rotation servo
Serial.println("Calibrating elevation");
elservo.writeMicroseconds(500);
delay(5000); // wait 5 seconds for servo to reach the position
elcalVal[0] = analogRead(elfeedbackPin);
//servo.write(180);
elservo.writeMicroseconds(3000);
delay(5000);
elcalVal[1] = analogRead(elfeedbackPin);
Serial.print("Elevation cal values: ");
Serial.print(elcalVal[0]);
Serial.print(",");
Serial.println(elcalVal[1]);
}
else if (str == "calr"){
// calibrate roation
//Serial.println(x);
//x = Serial.parseInt();
// rotation servo
Serial.println("Calibrating rotation");
roservo.writeMicroseconds(500);
delay(5000); // wait 5 seconds for servo to reach the position
rocalVal[0] = analogRead(rofeedbackPin);
//servo.write(180);
roservo.writeMicroseconds(3000);
delay(5000);
rocalVal[1] = analogRead(rofeedbackPin);
Serial.print("Roation cal values: ");
Serial.print(rocalVal[0]);
Serial.print(",");
Serial.println(rocalVal[1]);
}
else if (str == "ec"){
// move elevation using cal values
Serial.println("Adjusting Elevation using cal values to: ");
Serial.println(x);
//x = Serial.parseInt();
// elevation servo
// map the range of angles to the servo
xmap = map(x, 0, 180, elcalVal[0], elcalVal[1]);
//Serial.println(xmap);
xmap2 = map(xmap, elcalVal[0], elcalVal[1], 500, 2400);
elservo.writeMicroseconds(xmap2);
//Serial.println(xmap2);
delay(1000);
Serial.print("Elevation adjusted to: ");
//Serial.println(analogRead(elfeedbackPin));
Serial.println(map(analogRead(elfeedbackPin), elcalVal[0], elcalVal[1], 0, 180));
}
else if (str == "er"){
// move rotation using cal values
Serial.print("Adjusting Rotation using cal values to: ");
Serial.println(x);
//x = Serial.parseInt();
// elevation servo
// map the range of angles to the servo
xmap = map(x, 0, 180, rocalVal[0], rocalVal[1]);
//Serial.println(xmap);
xmap2 = map(xmap, rocalVal[0], rocalVal[1], 500, 2400);
roservo.writeMicroseconds(xmap2);
//Serial.println(xmap2);
delay(1000);
Serial.print("Rotation adjusted to: ");
Serial.println(map(analogRead(rofeedbackPin), elcalVal[0], elcalVal[1], 0, 180));
}
else if (str == "fullr"){
int d;
int dr = 1200;
// rotate one degree at a time
Serial.println("Rotating");
roservo.writeMicroseconds(500);
delay(2000);
for ( d = 500; d < 2300; d++ ) {
roservo.writeMicroseconds(d);
delay(10);
if( d % 100 == 0){
elservo.writeMicroseconds(dr);
dr = dr + 100;
}
}
Serial.println("Done");
}
}
}
int smooth(int data) {
total -= readings[index]; // subtract the last reading
readings[index] = analogRead(elfeedbackPin); // read from the sensor
total += readings[index]; // add the reading to the total
index = (index + 1); // advance to the next index
if (index >= NUMREADINGS) // if we're at the end of the array...
index = 0; // ...wrap around to the beginning
val = total / NUMREADINGS; // calculate the average
return val;
}