60 lines
1.6 KiB
C++
60 lines
1.6 KiB
C++
#include "headerFile.h"
|
|
|
|
// SensorManager::Rotation offset;
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
Serial.println("startup");
|
|
|
|
connectivity.connectWiFi(ssid, pass);
|
|
sensorManager.sensorSetup();
|
|
|
|
//ws server address, port and URL
|
|
// webSocket.begin("145.28.160.108", 8001, "");
|
|
// try every 500 again if connection has failed
|
|
// webSocket.setReconnectInterval(500);
|
|
}
|
|
|
|
void loop() {
|
|
SensorManager::eulerAngles eulerRotation = sensorManager.getEulerAngles();
|
|
|
|
// Subtract offset
|
|
// rotation.i -= offset.i;
|
|
// rotation.j -= offset.j;
|
|
// rotation.k -= offset.k;
|
|
// rotation.w -= offset.w;
|
|
|
|
// Convert quaternion to Euler angles in radians
|
|
|
|
|
|
// Convert to degrees
|
|
// float rollDegrees = roll * 180.0f / PI;
|
|
// float pitchDegrees = pitch * 180.0f / PI;
|
|
// float yawDegrees = yaw * 180.0f / PI;
|
|
|
|
Serial.print(eulerRotation.roll);
|
|
Serial.print(" ");
|
|
Serial.print(eulerRotation.pitch);
|
|
Serial.print(" ");
|
|
Serial.print(eulerRotation.yaw);
|
|
sendData(eulerRotation.roll, eulerRotation.pitch, eulerRotation.yaw);
|
|
|
|
Serial.println();
|
|
// webSocket.loop();
|
|
}
|
|
// if (Serial.available()) {
|
|
// String command = Serial.readStringUntil('\n');
|
|
// command.trim(); // remove any trailing whitespace
|
|
// if (command == "setZeroPoint") {
|
|
// setZeroPoint();
|
|
// }
|
|
// }
|
|
// }
|
|
// void setZeroPoint() {
|
|
// offset = sensorManager.readLoop();
|
|
// }
|
|
|
|
void sendData(float roll, float pitch, float yaw){
|
|
String message = "{\"Sensor\": 1, \"roll\":\"" + String(roll) + "\",\"pitch\":\"" + String(pitch) + "\",\"yaw\":\"" + String(yaw) + "\"}";
|
|
webSocket.sendTXT(message);
|
|
} |