I have a sketch that converts a input string from a program using Azimuth/Elevation to create X/Y numbers.
The sketch works fine except when the input equals zero degrees for both Azimuth and Elevation.
So a input like 0,0.0,0.0 gives only a X and nothing else.
A input like 0,1.0,1.0 gives a correct output.
Maybe someone could have a look at the sketch.
// Detect a Videre command that looks like 1,aaa.a,eee.e
// if e = negative where waiting for a satilite
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Streaming.h>
#include <Servo.h>
#if defined(ARDUINO) && ARDUINO >= 100
#define printByte(args) write(args);
#else
#define printByte(args) print(args,BYTE);
#endif
String readString = "";
float x_axisTemp = 0;
float y_axisTemp = 0;
int videreIndex = 0; // counter if a Videre command is to be decoded
int decimalPoint = 0;
int dataEastWest = 0;
int teller = 0;
//long _XX = 0L;
//long _YY = 0L;
float newXaxis = 0L; // new azimuth for rotor move
float newYaxis = 0L; // new azimuth for rotor move
static const double DegreeToRadian = PI/180. ; // PI/180
static const double RadianToDegree = 1/DegreeToRadian; // 1 / ( PI / 180 )
boolean videreCommand = false; // Is set if the first character received is a 0 or 1
boolean videreEastWest = false; // Set if the pass is on the QTH-West
// Create display instance
// Arduino analog input 5 - I2C SCL
// Arduino analog input 4 - I2C SDA
LiquidCrystal_I2C lcdSerial(0x20,20,4); // set the LCD address to 0x20 for a 20 chars and 4 line display
Servo X_AXIS; // create servo object to control a servo
void setup() {
Serial.begin(19200);
Serial.println("Videre Command 0.0"); // so I can keep track of what is loaded
lcdSerial.init(); // initialize the lcd
lcdSerial.backlight();
lcdSerial.print(" Videre Command 0.0"); // so I can keep track of what is loaded
X_AXIS.attach(6); // attaches the servo on pin 9 to the servo object
X_AXIS.write(90); // And place it at the zero position
delay(5);
}
void loop() {
if (Serial.available()) {
readCommand(Serial.read());
}
else {
videreCommand = false;
}
}
void readCommand(char character) {
if (character == ',' || character == '\n' || character == '\r') {
if (readString.length() >0) {
switch(videreIndex) {
case 0:
{
if(readString == "0" && !videreCommand) {
Serial << "Pass on West so add 180 degrees (flip)" << endl;
videreCommand = true;
videreEastWest = true;
videreIndex = 1;
}
else if(readString == "1" && !videreCommand) {
Serial << "Pass on East no adjustment needed" << endl;
videreCommand = true;
videreEastWest = false;
videreIndex = 1;
}
break;
}
case 1:
{
x_axisTemp = convertToFloat(readString);
Serial << "Got X value = " << x_axisTemp << endl;
if(x_axisTemp > 360) {
videreCommand = false;
videreIndex = 0;
}
else {
videreIndex = 2;
}
readString = ""; //clears variable for new input
break;
}
case 2:
{
y_axisTemp = convertToFloat(readString);
Serial << "Got Y value = " << x_axisTemp << endl;
if(y_axisTemp <= 000.0) { // Waiting for satelite to come in range
y_axisTemp = 000.0;
if(videreEastWest) {
y_axisTemp = 180.0;
}
else {
y_axisTemp = 000.0;
}
}
if(y_axisTemp <= 180.0) {
lcdSerial.setCursor(0,1);
lcdSerial << "AZ/EL " << displayAxis(x_axisTemp);
lcdSerial.setCursor(13,1);
lcdSerial << displayAxis(y_axisTemp);
xyConvert(x_axisTemp, y_axisTemp);
lcdSerial.setCursor(0,2);
lcdSerial << " X/Y " << displayAxis(newXaxis);
lcdSerial.setCursor(13,2);
lcdSerial << displayAxis(newYaxis);
lcdSerial.setCursor(0,3);
lcdSerial << "Rotor " << " 0.00 0.00";
X_AXIS.write(newXaxis);
}
else {
Serial << "Wrong elevation, start over again " << endl;
}
videreIndex = 0;
videreCommand = false;
videreEastWest = false;
break;
}
default:
{
Serial << "Some Error" << endl;
break;
}
}
readString = ""; //clears variable for new input
}
}
else {
readString += character; //makes the string readString
}
}
float convertToFloat(String readString) {
char carray[readString.length() + 1]; //determine size of the array
readString.toCharArray(carray, sizeof(carray)); //put readStringinto an array
float f = atof(carray); //convert the array into a float
}
String displayAxis(double axis) {
static char Buffer[7];
String SString = "";
if(axis < 10.0) {
SString += " ";
}
if(axis < 100.0) {
SString += " ";
}
dtostrf(axis,4, 2, Buffer);
return SString += Buffer;
}
void xyConvert(float az, float el) {
double azFloat = az * DegreeToRadian; // One can also use DEG_TO_RAD
double elFloat = el * DegreeToRadian;
double _XX = 0L;
double _YY = 0L;
_XX = asin(sin(azFloat)*cos(elFloat)) * RadianToDegree; // One can also use RAD_TO_DEG
if(el < 0.1) {
_YY = 000.1;
}
if(el == 180) {
_YY = -090.0;
}
else {
_YY = atan(cos(azFloat)/tan(elFloat)) * RadianToDegree;
}
newXaxis = 90 - _XX;
newYaxis = 90 - _YY;
}
float RadiansToDegrees(float rads)
{
float RTD = rads * 180/PI;
return RTD;
}