You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
70 lines
1.8 KiB
C++
70 lines
1.8 KiB
C++
#include "servo.h"
|
|
|
|
|
|
void ServoController::initServos() {
|
|
HrzSrv.attach(12, 500, 2500);
|
|
delay(50); // Espera 50ms para o motor se mover
|
|
VrtSrv.attach(13, 500, 2500);
|
|
}
|
|
|
|
void ServoController::initMag() {
|
|
while (!mag.begin()) {
|
|
WebMonitor.println("ERRO: Não foi possível iniciar o magnetometro.");
|
|
WebMonitor.println("Tentando novamente em 500ms");
|
|
delay(500);
|
|
}
|
|
|
|
if (mag.isQMC()) {
|
|
WebMonitor.println("Iniciando magnetometro QMC5883L");
|
|
|
|
mag.setRange(QMC5883_RANGE_8GA);
|
|
WebMonitor.print("Range: ");
|
|
WebMonitor.println(mag.getRange());
|
|
|
|
mag.setMeasurementMode(QMC5883_CONTINOUS);
|
|
WebMonitor.print("Modo de leitura: ");
|
|
WebMonitor.println(mag.getMeasurementMode());
|
|
|
|
mag.setDataRate(QMC5883_DATARATE_200HZ);
|
|
WebMonitor.print("Velocidade: ");
|
|
WebMonitor.println(mag.getDataRate());
|
|
|
|
mag.setSamples(QMC5883_SAMPLES_8);
|
|
WebMonitor.print("Sample: ");
|
|
WebMonitor.println(mag.getSamples());
|
|
|
|
mag.setDeclinationAngle(DECLINATION);
|
|
WebMonitor.print("Declinação: ");
|
|
WebMonitor.println(DECLINATION);
|
|
} else {
|
|
WebMonitor.println("ERRO: Magnetometro encontrado não é suportado.");
|
|
WebMonitor.println("Suspendendo operação.");
|
|
while (true);
|
|
}
|
|
}
|
|
|
|
void ServoController::updateHeading() {
|
|
mag.getHeadingDegrees();
|
|
sVector_t raw = mag.readRaw();
|
|
float tmp = raw.HeadingDegress;
|
|
|
|
headingBuffer[bufferPos++] = tmp;
|
|
if (bufferPos > HEADING_BUFFER_LENGTH) {
|
|
bufferPos = 0;
|
|
}
|
|
|
|
headingDegrees = 0;
|
|
for (byte i = 0; i < HEADING_BUFFER_LENGTH; i++) {
|
|
headingDegrees += headingBuffer[i];
|
|
}
|
|
headingDegrees /= HEADING_BUFFER_LENGTH;
|
|
}
|
|
|
|
void ServoController::moveToAzAlt() {
|
|
|
|
}
|
|
|
|
void ServoController::moveToRADec() {
|
|
|
|
}
|