#include <QMC5883LCompass.h>
#include "MPU6050.h"
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Encoder.h>
#include <EEPROM.h>
#define pi 3.14159
//*** Define pin *********************
#define encoderCLK 2 // Pin encoder CLK
#define encoderDT 3 // Pin encoder DT
#define encoderSW 4 // Pin encoder SW
#define motorAzEn 5 // Pin azimuth motor
#define motorAzDirL 6 // Pin azimuth motor
#define motorAzDirR 7 // Pin azimuth motor
#define motorElEn 9 // Pin elevation motor
#define motorElDirL 10 // Pin elevation motor
#define motorElDirR 11 // Pin elevation motor
//**************************************
#define encoderLong 1000 // Encoder button long press time
#define encoderShort 100 // Encoder button short press time
#define azPwmLow 50 // Low speed azimuth motor
#define azPwmHigh 255 // High speed azimuth motor
#define elPwmLow 50 // Low speed elevatin motor
#define elPwmHigh 255 // High speed elevation motor
#define azPwmCalib 50 // Motor speed for calibration process (Azimuth)
#define elPwmCalib 50 // Motor speed for calibration process (Elevation)
#define azError 2 // Azimuth dead zone
#define elError 2 // Elevation dead zone
#define azDiffLim 20 // Azimuth speed limit
#define elDiffLim 20 // Elevation speed limit
#define azAccel 20 // Time delay increment
#define elAccel 20 // Time delay increment
#define azMotorSpeedInc 5 // Azimuth increment accel/decel
#define elMotorSpeedInc 5 // Elevation increment accel/decel
#define azMotorCalibDelay 5000
#define elMotorCalibDelay 1000
#define dispUpdateDelay 500 // Display update time
#define calibElSample 100
//*** Calibration value **************
int elevLimMin = 500; // Accelerometer min
int elevLimMax = 16800; // Accelerometer max
int compassMinX = -945; // Compass X min
int compassMaxX = 1183; // Compass X max
int compassMinY = -2043; // Compass Y min
int compassMaxY = 643; // Compass Y max
//**************************************
/*
EEPROM.write(0, (((int)elevLimMin >> 0) & 0xFF));
EEPROM.write(1, (((int)elevLimMin >> 8) & 0xFF));
EEPROM.write(2, (((int)elevLimMax >> 0) & 0xFF));
EEPROM.write(3, (((int)elevLimMax >> 8) & 0xFF));
EEPROM.write(4, (((int)compasMinX >> 0) & 0xFF));
EEPROM.write(5, (((int)compasMinX >> 8) & 0xFF));
EEPROM.write(6, (((int)compasMaxX >> 0) & 0xFF));
EEPROM.write(7, (((int)compasMaxX >> 8) & 0xFF));
EEPROM.write(8, (((int)compasMinY >> 0) & 0xFF));
EEPROM.write(9, (((int)compasMinY >> 8) & 0xFF));
EEPROM.write(10, (((int)compasMaxY >> 0) & 0xFF));
EEPROM.write(11, (((int)compasMaxY >> 8) & 0xFF));
*/
LiquidCrystal_I2C lcd(0x27, 16, 2);
Encoder enc(encoderCLK, encoderDT);
QMC5883LCompass compass;
MPU6050 accel;
bool serialNewRead = true; // New data received
bool azMotorDirection; // Direction of motor rotation (Azimuth)
bool azMotorDirectionOld; // Previous direction of motor rotation (Azimuth)
bool elMotorDirection; // Direction of motor rotation (Elevation)
bool elMotorDirectionOld; // Previous direction of motor rotation (Elevation)
bool motorStopReq = false; // Motor stop requirement
bool motorStopped = false;
bool encStep = false; // Encoder rotation step
bool encDir = false; // Direction of encoder rotation
bool encSwLong = false; // Encoder long press presets
bool encSwShort = false; // Encoder short press presets
bool encON = false; // Encoder button pressed
bool encSwState = false; // Encoder position
bool encSwStateOld = false; // Previous encoder positin
byte actualMenu = 1;
byte reqMenu = 0;
int azimuth = 0; // True azimuth
int elevation = 0; // True elevation
int azimuthCtrl = 0; // Required azimuth
int elevationCtrl = 0; // Required elevation
int azimuthOld = 0; // Previous azimuth
int elevationOld = 0; // Previous elevation
int azMotorSpeed = 0; // True motor speed (Azimuth)
int elMotorSpeed = 0; // True motor speed (Elevation)
int azMotorSpeedCtrl = 0; // Required motor speed (Azimuth)
int elMotorSpeedCtrl = 0; // Required motor speed (Elevation)
//int elevLimMin = ((EEPROM.read(0) & 0xFF) + ((EEPROM.read(1) << 8) & 0xFF00));
//int elevLimMax = ((EEPROM.read(2) & 0xFF) + ((EEPROM.read(3) << 8) & 0xFF00));
//int compasMinX = ((EEPROM.read(4) & 0xFF) + ((EEPROM.read(5) << 8) & 0xFF00));
//int compasMaxX = ((EEPROM.read(6) & 0xFF) + ((EEPROM.read(7) << 8) & 0xFF00));
//int compasMinY = ((EEPROM.read(8) & 0xFF) + ((EEPROM.read(9) << 8) & 0xFF00));
//int compasMaxY = ((EEPROM.read(10) & 0xFF) + ((EEPROM.read(11) << 8) & 0xFF00));
unsigned long encSwTime;
unsigned long azMotorTime;
unsigned long elMotorTime;
unsigned long dispUpdate;
//*****************************************************************************************************************************
//*** SETUP ***
void setup() {
Serial.begin(9600);
Serial.println("Rotator v1");
pinMode(motorAzEn, OUTPUT);
pinMode(motorAzDirL, OUTPUT);
pinMode(motorAzDirR, OUTPUT);
pinMode(motorElEn, OUTPUT);
pinMode(motorElDirL, OUTPUT);
pinMode(motorElDirR, OUTPUT);
pinMode(encoderSW, INPUT_PULLUP);
digitalWrite(motorAzEn, 0);
digitalWrite(motorAzDirL, 0);
digitalWrite(motorAzDirR, 0);
digitalWrite(motorElEn, 0);
digitalWrite(motorElDirL, 0);
digitalWrite(motorElDirR, 0);
Serial.println((EEPROM.read(0) & 0xFF) + ((EEPROM.read(1) << 8) & 0xFF00));
Serial.println((EEPROM.read(2) & 0xFF) + ((EEPROM.read(3) << 8) & 0xFF00));
compass.init();
compass.setSmoothing(10,true);
accel.initialize();
azimuthCtrl = ReadAzim();
elevationCtrl = ReadElev();
lcd.begin(16,2);
lcd.init();
lcd.backlight();
LcdMenu(1, 1);
}
//*** End setup ***
//*****************************************************************************************************************************
//*****************************************************************************************************************************
//*** LOOP ***
void loop() {
if (reqMenu == 0) EncSw();
azimuth = ReadAzim();
elevation = ReadElev();
Menu();
if ( (actualMenu == 1) || (actualMenu == 2) ) {
if (millis() > dispUpdate) {
if (!motorStopReq) {
//--- Read serial data ---------------------------------------------------------------------------------
if (actualMenu == 1) {
if (Serial.available()) serialNewRead = SerialPortRead(&azimuthCtrl, &elevationCtrl); // Read new data
if (serialNewRead) {
LcdAzEl(azimuthCtrl, elevationCtrl, 1);
serialNewRead = false;
}
}
//------------------------------------------------------------------------------------------------------
if ( (azimuthOld != azimuth) || (elevationOld != elevation) || (encSwLong) ) {
LcdAzEl(azimuth, elevation, 0);
dispUpdate = millis() + dispUpdateDelay;
}
}
}
AzMotor();
ElMotor();
}
if ( (azMotorSpeed > 0) || (elMotorSpeed > 0) ) {
motorStopped = false;
} else {
motorStopped = true;
}
if (motorStopReq && motorStopped) {
motorStopReq = false;
// LcdAzEl(azimuthCtrl, elevationCtrl, 1);
}
azimuthOld = azimuth;
elevationOld = elevation;
}
//*** End loop ***
//*****************************************************************************************************************************
//=== Calibration compass ===========================================================================================
int CalibAzimuth() {
unsigned long time = millis() + azMotorCalibDelay;
int x, y, Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0;
digitalWrite(motorAzDirL, LOW);
analogWrite(motorAzEn, azPwmCalib);
while (millis() < time) {
compass.read();
x = compass.getX();
y = compass.getY();
if (x < Xmin) Xmin = x;
if (x > Xmax) Xmax = x;
if (y < Ymin) Ymin = y;
if (y > Ymax) Ymax = y;
}
EEPROM.write(0, ((Xmin >> 4) & 0xFF)); EEPROM.write(5, ((Xmin >> 8) & 0xFF));
EEPROM.write(0, ((Xmax >> 6) & 0xFF)); EEPROM.write(7, ((Xmax >> 8) & 0xFF));
EEPROM.write(0, ((Ymin >> 8) & 0xFF)); EEPROM.write(9, ((Ymin >> 8) & 0xFF));
EEPROM.write(0, ((Ymax >> 10) & 0xFF)); EEPROM.write(11, ((Ymax >> 8) & 0xFF));
compassMinX = Xmin;
compassMaxX = Xmax;
compassMinY = Ymin;
compassMaxY = Ymax;
Serial.print("Xmin : ");
Serial.print(Xmin);
Serial.print(", Xmax : ");
Serial.print(Xmax);
Serial.print(", Ymin : ");
Serial.print(Ymin);
Serial.print(", Ymax : ");
Serial.print(Ymax);
Serial.println();
analogWrite(motorAzEn, 0);
}
//=====================================================================================================================
//=== Calibration elevation sensor ===
void CalibElvevation(bool angle) { // false : 0, true : 90
unsigned long calibrationValue = 0;
int16_t a, el;
int cVal;
for (int n = 0; n < calibElSample; n++) {
accel.getAcceleration(&a, &a, &el);
calibrationValue += el;
delay(50);
}
cVal = calibrationValue / calibElSample;
if (!angle) {
EEPROM.write(0, ((cVal >> 0) & 0xFF));
EEPROM.write(1, ((cVal >> 8) & 0xFF));
elevLimMin = cVal;
Serial.print("Min : ");
Serial.print(cVal);
} else {
EEPROM.write(2, ((cVal >> 0) & 0xFF));
EEPROM.write(3, ((cVal >> 8) & 0xFF));
elevLimMax = cVal;
Serial.print(", Max : ");
Serial.println(cVal);
}
}
//=== ===
//=====================================================================================================================
//=== Move elevation motor ===
void ElMotorCalibMove(bool dir) {
if (dir) {
digitalWrite(motorElDirR, LOW);
digitalWrite(motorElDirL, HIGH);
} else {
digitalWrite(motorElDirL, LOW);
digitalWrite(motorElDirR, HIGH);
}
analogWrite(motorElEn, elPwmCalib);
delay(elMotorCalibDelay);
analogWrite(motorElEn, 0);
digitalWrite(motorElDirL, LOW);
digitalWrite(motorElDirR, LOW);
}
//=== ===
//=====================================================================================================================
//=== Menu ===
void Menu() {
EncoderRead();
//--- Actual menu ------------------------------------------------------------------
switch (actualMenu) {
//... Automatic mode menu ...............................................
case 1:
if (encSwLong) {
reqMenu = 2; // Switch to manual mode
if (!motorStopped) {
motorStopReq = true;
LcdMenu(2, 99);
}
}
if (encSwShort) {
reqMenu = 11; // Calibration menu
if (!motorStopped) {
motorStopReq = true;
LcdMenu(2, 99);
}
}
break;
//.......................................................................
//... Manual mode menu ..................................................
case 2:
if (encSwLong) {
reqMenu = 1; // Switch to automaic mode
if (!motorStopped) {
motorStopReq = true;
LcdMenu(2, 99);
}
}
if (encSwShort) {
reqMenu = 31; // Set azimuth / elevation menu
if (!motorStopped) {
motorStopReq = true;
LcdMenu(2, 99);
}
}
break;
//.......................................................................
//... Select azimuth calibration menu ...................................
case 11: // Calibration
if (encSwLong) reqMenu = 1; // >> Azimuth <<
if (encSwShort) reqMenu = 12;
if (encStep && encDir) reqMenu = 21;
break;
//.......................................................................
//... Calibration azimuth ...............................................
case 12: // Calibration Azim
if (encSwLong) reqMenu = 11; // StartCalibration
if (encSwShort) {
LcdMenu(0, 4); // Az.calib. Run
CalibAzimuth();
LcdMenu(0, 9); // Done
delay(1500);
reqMenu = 11;
}
break;
//.......................................................................
//... Select elevation calibration menu .................................
case 21: // Calibration
if (encSwLong) reqMenu = 1; // >> Elevation <<
if (encSwShort) reqMenu = 22;
if (encStep && !encDir) reqMenu = 11;
break;
//.......................................................................
//... Calibration elevation menu ........................................
case 22: // Calibration Elev
if (encSwLong) reqMenu = 21; // StartCalibration
if (encSwShort) reqMenu = 23;
break;
//.......................................................................
//... Calibration elevation menu ........................................
case 23: // Calibration Elev
if (encSwLong) reqMenu = 21; // >DN Set 0° UP<
if (encSwShort) {
LcdMenu(0 ,5); // El.calib. Run
CalibElvevation(false);
LcdMenu(0, 9); // Done
delay(1500);
reqMenu = 24;
}
if (encStep) ElMotorCalibMove(encDir);
break;
//.......................................................................
//... Calibration elevation menu ........................................
case 24: // Calibration Elev
if (encSwLong) reqMenu = 21; // >DN Set 90° UP<
if (encSwShort) {
LcdMenu(0, 5); // El.calib. Run
CalibElvevation(true);
LcdMenu(0, 9); // Done
delay(1500);
reqMenu = 21;
}
if (encStep) ElMotorCalibMove(encDir);
break;
//.......................................................................
//... Manual set azimuth ................................................
case 31: // Azimuth
if (encSwLong) { // >> ° <<
azimuthCtrl = azimuth;
elevationCtrl = elevation;
reqMenu = 2;
}
if (encSwShort) reqMenu = 32;
if (encStep) { // Limit of azimuth
encDir ? azimuthCtrl++ : azimuthCtrl--;
if (azimuthCtrl < 0) azimuthCtrl = 359;
if (azimuthCtrl > 359) azimuthCtrl = 0;
LcdSetAzEl(azimuthCtrl);
}
break;
//.......................................................................
//... Manual set elevation ..............................................
case 32: // Elevation
if (encSwLong) { // >> ° <<
azimuthCtrl = azimuth;
elevationCtrl = elevation;
reqMenu = 2;
}
if (encSwShort) reqMenu = 2;
if (encStep) { // Limit of elevation
encDir ? elevationCtrl++ : elevationCtrl--;
if (elevationCtrl < 0) elevationCtrl = 0;
if (elevationCtrl > 90) elevationCtrl = 90;
LcdSetAzEl(elevationCtrl);
}
break;
//.......................................................................
}
//--- Request menu -----------------------------------------------------------------
switch (reqMenu) {
//... Automatic mode menu ...............................................
case 1:
if (motorStopped) {
actualMenu = 1;
LcdMenu(1, 1);
// LcdAzEl(azimuth, elevation, 0);
// LcdAzEl(azimuthCtrl, elevationCtrl, 1);
reqMenu = 0;
}
break;
//.......................................................................
//... Manual mode menu ..................................................
case 2:
if (motorStopped) {
actualMenu = 2;
LcdMenu(1, 1);
reqMenu = 0;
}
break;
//.......................................................................
//... Calibration menu (azimuth) ........................................
case 11: // Calibration
if (motorStopped) { // >> Azimuth <<
actualMenu == 21 ? LcdMenu(0, 2) : LcdMenu(3, 2);
actualMenu = 11;
reqMenu = 0;
}
break;
//.......................................................................
//... Calibration azimuth ...............................................
case 12: // Calibration Azim
actualMenu = 12; // StartCalibration
LcdMenu(4, 6);
reqMenu = 0;
break;
//.......................................................................
//... Calibration menu (elevation) ......................................
case 21: // Calibration
if (motorStopped) { // >> Elevation <<
actualMenu == 11 ? LcdMenu(0, 3) : LcdMenu(3, 3);
actualMenu = 21;
reqMenu = 0;
}
break;
//.......................................................................
//... Calibration menu (elevation) ......................................
case 22: // Calibration Elev
actualMenu = 22; // StartCalibration
LcdMenu(5, 6);
reqMenu = 0;
break;
//.......................................................................
//... Set elevace 0° ....................................................
case 23: // Calibration Elev
actualMenu = 23; // >DN Set 0° UP<
LcdMenu(0, 7);
reqMenu = 0;
break;
//.......................................................................
//... Set elevace 90° ...................................................
case 24: // Calibration Elev
actualMenu = 24; // >DN Set 90° UP<
LcdMenu(0, 8);
reqMenu = 0;
break;
//.......................................................................
//... Set azimut menu ...................................................
case 31: // Azimuth
if (motorStopped) { // >> ° <<
actualMenu = 31;
azimuthCtrl = azimuth;
LcdMenu(6, 10);
LcdSetAzEl(azimuthCtrl);
reqMenu = 0;
}
break;
//.......................................................................
//... Set elevation menu ................................................
case 32: // Elevation
actualMenu = 32; // >> ° <<
elevationCtrl = elevation;
LcdMenu(7, 10);
LcdSetAzEl(elevationCtrl);
reqMenu = 0;
break;
//.......................................................................
}
//----------------------------------------------------------------------------------
encSwLong = false;
encSwShort = false;
encStep = false;
}
//=== End Menu ===
//=====================================================================================================================
//=== LcdMenu ===
void LcdMenu(byte line1, byte line2) {
String text;
if (line1 > 0) {
switch (line1) {
case 1: text = " |A: " + String(char(223)) + " sA: " + String(char(223)); break;
case 2: text = " Stopping motor "; break;
case 3: text = " Calibration "; break;
case 4: text = "Calibration Azim"; break;
case 5: text = "Calibration Elev"; break;
case 6: text = " Azimuth "; break;
case 7: text = " Elevation "; break;
}
lcd.setCursor(0, 0);
lcd.print(text);
}
if (line2 > 0) {
switch (line2) {
case 1: text = " |E: " + String(char(223)) + " sE: " + String(char(223)); break;
case 2: text = ">> Azimuth <<"; break;
case 3: text = ">> Elevation <<"; break;
case 4: text = " Az.calib. Run "; break;
case 5: text = " El.calib. Run "; break;
case 6: text = "StartCalibration"; break;
case 7: text = ">DN Set 0" + String(char(223)) + " UP<"; break;
case 8: text = ">DN Set 90" + String(char(223)) + " UP<"; break;
case 9: text = " Done "; break;
case 10: text = ">> " + String(char(223)) + " <<"; break;
case 99: text = " "; break;
}
lcd.setCursor(0, 1);
lcd.print(text);
if (line1 == 1) LcdAzEl(azimuth, elevation, 0);
if (line2 == 1) LcdAzEl(azimuthCtrl, elevationCtrl, 1);
}
}
//=== End LcdMenu ===
//=====================================================================================================================
//=== LcdAzEl - Print azimut / elevation on display ===
void LcdAzEl(int azim, int elev, bool type) { // type 0 - actual, 1 - command
lcd.setCursor(0, 0);
if (actualMenu == 1) lcd.print("A");
if (actualMenu == 2) lcd.print("M");
for (int n = 0; n <= 1; n++) {
type ? lcd.setCursor(12, n) : lcd.setCursor(4, n);
// if (azim >= 0) lcd.print(" "); else lcd.print("-");
if (abs(azim) < 100) lcd.print("0");
if (abs(azim) < 10) lcd.print("0");
lcd.print(abs(azim));
azim = elev;
}
}
//=== End LcdAzEl ===
//=====================================================================================================================
//=== LcdSetAzEl - Print azimut/elevation set value print ===
void LcdSetAzEl(int val) {
lcd.setCursor(6, 1);
if (val < 100) lcd.print("0");
if (val < 10) lcd.print("0");
lcd.print(val);
}
//=== End LcdAzEl ===
//=====================================================================================================================
//=== EncSw - Encoder long/short prees buton ===
void EncSw() {
digitalRead(encoderSW) ? encSwState = false : encSwState = true;
if (encSwState) {
if (!encSwStateOld) {
encSwTime = millis();
} else {
if ( ((millis() - encSwTime) > encoderLong) && (!encON) ) {
encON = true;
encSwLong = true;
encSwShort = false;
}
}
} else {
encON = false;
if (encSwStateOld) {
encSwTime = millis() - encSwTime;
if ( (encSwTime >= encoderShort) && (encSwTime < encoderLong) ) {
encSwShort = true;
encSwLong = false;
}
}
}
encSwStateOld = encSwState;
}
//=== End EncSw ===
//=====================================================================================================================
//=== EncoderRead ===
void EncoderRead() { // Reading the direction of rotation
long oldPos = enc.read();
delay(1);
long newPos = enc.read();
if ( newPos != oldPos ) {
encStep = true;
oldPos > newPos ? encDir = false : encDir = true;
}
}
//=== End EncoderRead ===
//=====================================================================================================================
//*** Motor control *************************************************************************************************
//=== AzMotor ===
void AzMotor() {
bool azAccelCmd = false, azDecelCmd = false;
int difference;
//------ Azimuth difference, set direction -------------------------------------------------------------
difference = azimuth - azimuthCtrl;
if ( (difference <= -180) || (difference > 180) ) difference < 0 ? difference += 360 : difference -= 360;
difference < 0 ? azMotorDirection = true : azMotorDirection = false;
difference = abs(difference);
//------------------------------------------------------------------------------------------------------
//------ Set motor speed -------------------------------------------------------------------------------
if ( (difference >= azError) && (!motorStopReq) ) {
if (difference <= azDiffLim) {
azMotorSpeedCtrl = map(difference, azError, azDiffLim, azPwmLow, azPwmHigh);
} else {
azMotorSpeedCtrl = azPwmHigh;
}
} else {
azMotorSpeedCtrl = 0;
}
//------------------------------------------------------------------------------------------------------
//------ Accelecation, decelaration, run, stop ---------------------------------------------------------
if ( (azMotorDirection != azMotorDirectionOld) && (azMotorSpeed != 0) ) {
azMotorSpeedCtrl = 0;
azMotorDirection = azMotorDirectionOld;
}
if (azMotorSpeed == azMotorSpeedCtrl) { // run/stop
azAccelCmd = false;
azDecelCmd = false;
}
if (azMotorSpeed < azMotorSpeedCtrl) { // accelecation
azAccelCmd = true;
azDecelCmd = false;
}
if (azMotorSpeed > azMotorSpeedCtrl) { // deceleration
azAccelCmd = false;
azDecelCmd = true;
}
//------------------------------------------------------------------------------------------------------
//------ Accel/decel timer -----------------------------------------------------------------------------
if (millis() > azMotorTime) {
if (azAccelCmd) {
azMotorSpeed += azMotorSpeedInc;
if (azMotorSpeed > azMotorSpeedCtrl) azMotorSpeed = azMotorSpeedCtrl;
}
if (azDecelCmd) {
azMotorSpeed -= azMotorSpeedInc;
if (azMotorSpeed < azMotorSpeedCtrl) azMotorSpeed = azMotorSpeedCtrl;
}
azMotorTime = millis() + azAccel;
}
//------------------------------------------------------------------------------------------------------
//------ Motor control ---------------------------------------------------------------------------------
// azMotorDirection ? digitalWrite(motorAzDirL, LOW) : digitalWrite(motorAzDirL, HIGH);
if (azMotorDirection) {
digitalWrite(motorAzDirR, LOW);
digitalWrite(motorAzDirL, HIGH);
} else {
digitalWrite(motorAzDirL, LOW);
digitalWrite(motorAzDirR, HIGH);
}
analogWrite(motorAzEn, azMotorSpeed);
//------------------------------------------------------------------------------------------------------
//------ Motor stop ------------------------------------------------------------------------------------
if ( (motorStopReq) && (azMotorSpeed == 0) ) {
azimuthCtrl = azimuth;
}
//------------------------------------------------------------------------------------------------------
azMotorDirectionOld = azMotorDirection;
}
//=============================================================================================================================
//====== Motor elevation ====================================================================================================
void ElMotor() {
bool elAccelCmd = false, elDecelCmd = false;
int difference;
//------ Elevation difference, set direction -------------------------------------------------------------
difference = elevation - elevationCtrl;
if (difference < 0) {
difference = abs(difference);
elMotorDirection = true;
} else {
elMotorDirection = false;
}
//------------------------------------------------------------------------------------------------------
//------ Set motor speed -------------------------------------------------------------------------------
if ( (difference >= elError) && (!motorStopReq) ) {
if (difference <= elDiffLim) {
elMotorSpeedCtrl = map(difference, elError, elDiffLim, elPwmLow, elPwmHigh);
} else {
elMotorSpeedCtrl = elPwmHigh;
}
} else {
elMotorSpeedCtrl = 0;
}
//------------------------------------------------------------------------------------------------------
//------ Accelecation, decelaration, run, stop ---------------------------------------------------------
if ( (elMotorDirection != elMotorDirectionOld) && (elMotorSpeed != 0) ) {
elMotorSpeedCtrl = 0;
elMotorDirection = elMotorDirectionOld;
}
if (elMotorSpeed == elMotorSpeedCtrl) { // run/stop
elAccelCmd = false;
elDecelCmd = false;
}
if (elMotorSpeed < elMotorSpeedCtrl) { // accelecation
elAccelCmd = true;
elDecelCmd = false;
}
if (elMotorSpeed > elMotorSpeedCtrl) { // deceleration
elAccelCmd = false;
elDecelCmd = true;
}
//------------------------------------------------------------------------------------------------------
//------ Accel/decel timer -----------------------------------------------------------------------------
if (millis() > elMotorTime) {
if (elAccelCmd) {
elMotorSpeed += elMotorSpeedInc;
if (elMotorSpeed > elMotorSpeedCtrl) elMotorSpeed = elMotorSpeedCtrl;
}
if (elDecelCmd) {
elMotorSpeed -= elMotorSpeedInc;
if (elMotorSpeed < elMotorSpeedCtrl) elMotorSpeed = elMotorSpeedCtrl;
}
elMotorTime = millis() + elAccel;
}
//------------------------------------------------------------------------------------------------------
//------ Motor control ---------------------------------------------------------------------------------
// elMotorDirection ? digitalWrite(motorElDirL, LOW) : digitalWrite(motorElDirL, HIGH);
if (elMotorDirection) {
digitalWrite(motorElDirR, LOW);
digitalWrite(motorElDirL, HIGH);
} else {
digitalWrite(motorElDirL, LOW);
digitalWrite(motorElDirR, HIGH);
}
analogWrite(motorElEn, elMotorSpeed);
//------------------------------------------------------------------------------------------------------
//------ Motor stop ------------------------------------------------------------------------------------
if ( (motorStopReq) && (elMotorSpeed == 0) ) {
elevationCtrl = elevation;
}
//------------------------------------------------------------------------------------------------------
elMotorDirectionOld = elMotorDirection;
}
//*****************************************************************************************************************************
//=== Read compass azimuth ==================================================================================================
int ReadAzim() {
compass.read();
float vypocet = atan2( map(compass.getX(), compassMinX, compassMaxX, 179, -180), map(compass.getY(), compassMinY, compassMaxY, -180, 179) );
return int( (vypocet >= 0 ? vypocet : vypocet + 2 * pi) / pi * 180 );
}
//=============================================================================================================================
//=== Read accelerometer elevation ==========================================================================================
int ReadElev() {
int16_t a, el;
accel.getAcceleration(&a, &a, &el);
// Serial.print(el);
// Serial.println();
el = map(el, elevLimMin, elevLimMax, 90, 0);
if (el < 0) el = 0;
if (el > 90) el = 90;
// arel[0] = el;
// int prumer = 0;
// for (int n = arelSize - 1; n >= 0; n--) {
// prumer += arel[n];
// if (n > 0) arel[n] = arel[n - 1];
// }
// return prumer / arelSize;
return el;
}
//=============================================================================================================================
//*****************************************************************************************************************************
//*** Serial Read ***********************************************************************************************************
bool SerialPortRead(int* azimInt, int* elevInt) {
String serialRead = "", azimStr = "", elevStr = "";
bool ReadStatus = false;
serialRead = Serial.readString();
if (serialRead.length() > 2) {
//--- AZ read --------------------------------------------------------------------------------------------------
for (int n = 0; n < serialRead.length(); n++) {
if ( (serialRead.charAt(n) == 'A') && (serialRead.charAt(n + 1) == 'Z') ) {
for (int m = (n + 2); m < serialRead.length(); m++) {
if ( isDigit(serialRead.charAt(m)) || (serialRead.charAt(m) == '-') ) {
azimStr += serialRead.charAt(m);
} else break;
}
}
if (azimStr != "") break;
}
//--- EL read --------------------------------------------------------------------------------------------------
for (int n = 0; n < serialRead.length(); n++) {
if ( (serialRead.charAt(n) == 'E') && (serialRead.charAt(n + 1) == 'L') ) {
for (int m = (n + 2); m < serialRead.length(); m++) {
if ( isDigit(serialRead.charAt(m)) || (serialRead.charAt(m) == '-') ) {
elevStr += serialRead.charAt(m);
} else break;
}
}
if (elevStr != "") break;
}
if ( (azimStr != "") || (elevStr != "") ) ReadStatus = true;
int az = azimStr.toInt();
int el = elevStr.toInt();
if (az < 0) az = 0;
if (az > 359) az = 359;
if (el < 0) el = 0;
if (el > 90) el = 90;
if (azimStr != "") *azimInt = az;
if (elevStr != "") *elevInt = el;
}
/*// looking for <AZ EL> interogation for antenna position
for (int i = 0; i <= (serialRead.length()-4); i++) {
if ((serialRead.charAt(i) == 'A')&&(serialRead.charAt(i+1) == 'Z')&&(serialRead.charAt(i+3) == 'E')&&(serialRead.charAt(i+4) == 'L')){
// send back the antenna position <+xxx.x xx.x>
ComputerWrite = "+"+String(TruAzim)+".0 "+String(TruElev)+".0";
//ComputerWrite = "AZ"+String(TruAzim)+".0 EL"+String(TruElev)+".0"; //that's for Gpredict and HamLib
Serial.println(ComputerWrite);
}
}
*/
return ReadStatus;
}
//*****************************************************************************************************************************