129 lines
3.6 KiB
C++
129 lines
3.6 KiB
C++
#include "FlashConfig.h"
|
|
#include "OSCRemote.h"
|
|
#include "Vbat.h"
|
|
#include "body.h"
|
|
#include "settings.h"
|
|
#include <ArduinoOTA.h>
|
|
#include <WiFi.h>
|
|
#include <memory>
|
|
|
|
Body body = Body::instance();
|
|
FlashConfig config;
|
|
std::unique_ptr<IRemote> remote;
|
|
Vbat vbat;
|
|
|
|
void servoTest() {
|
|
for (uint8_t legnum = 0; legnum < body.legs.size(); legnum++) {
|
|
body.legs[legnum].setPos(Leg::Hipp, 0);
|
|
body.legs[legnum].setPos(Leg::Knee, 0);
|
|
}
|
|
}
|
|
|
|
void connectWiFi() {
|
|
Serial.printf("Connecting to %s\n", config.data().wifiSSID.data());
|
|
WiFi.begin(config.data().wifiSSID.data(), config.data().wifiPass.data());
|
|
WiFi.setHostname(hostname);
|
|
while (WiFi.status() != WL_CONNECTED) {
|
|
delay(500);
|
|
Serial.print(".");
|
|
}
|
|
Serial.println("");
|
|
Serial.println("WiFi connected");
|
|
Serial.println("IP address: ");
|
|
Serial.println(WiFi.localIP());
|
|
}
|
|
|
|
void testRemoteCallback(const IRemote::Output& o) {
|
|
for (size_t i = 0; i < 6; ++i) {
|
|
body.legs[i].setPos(Leg::Hipp, double(o.attitude.roll) * 4);
|
|
body.legs[i].setPos(Leg::Knee, double(o.attitude.pitch) * 4);
|
|
// Serial.printf("CB: %f, %f\n", o.attitude.roll, o.attitude.pitch);
|
|
}
|
|
}
|
|
|
|
void sweepLeg() {
|
|
static uint8_t d = 0;
|
|
if (d < 2) {
|
|
for (long angle = 0; angle < 180 * 5; angle++) {
|
|
body.legs[1].setPos(Leg::Knee, double(angle / 5));
|
|
}
|
|
for (long angle = 180 * 5; angle > 0; angle--) {
|
|
body.legs[1].setPos(Leg::Knee, double(angle / 5));
|
|
}
|
|
}
|
|
d++;
|
|
}
|
|
|
|
void checkConfig() {
|
|
FlashConfig::Data compData;
|
|
strncpy(compData.wifiSSID.data(), wifiSSID, sizeof(compData.wifiSSID));
|
|
strncpy(compData.wifiPass.data(), wifiPass, sizeof(compData.wifiPass));
|
|
|
|
if (config.data() != compData) {
|
|
Serial.println("New configuration loaded");
|
|
config.save(compData);
|
|
config.printConfig();
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
|
|
body.init();
|
|
config.init();
|
|
delay(250);
|
|
|
|
servoTest();
|
|
config.load();
|
|
|
|
Serial.println("Current Flash Config:");
|
|
Serial.printf("- WiFi SSID: %s\n", config.data().wifiSSID.data());
|
|
Serial.printf("- WiFi pass: %s\n", config.data().wifiPass.data());
|
|
checkConfig();
|
|
connectWiFi();
|
|
remote.reset(new OSCRemote(vbat));
|
|
remote->registerCallback(testRemoteCallback);
|
|
|
|
sweepLeg();
|
|
|
|
// OTA
|
|
ArduinoOTA.setHostname("Hexapod");
|
|
ArduinoOTA.onStart([]() {
|
|
String type;
|
|
if (ArduinoOTA.getCommand() == U_FLASH)
|
|
type = "sketch";
|
|
else // U_SPIFFS
|
|
type = "filesystem";
|
|
|
|
// NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end()
|
|
Serial.println("Start updating " + type);
|
|
});
|
|
ArduinoOTA.onEnd([]() { Serial.println("\nEnd"); });
|
|
ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
|
|
Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
|
|
});
|
|
ArduinoOTA.onError([](ota_error_t error) {
|
|
Serial.printf("Error[%u]: ", error);
|
|
if (error == OTA_AUTH_ERROR)
|
|
Serial.println("Auth Failed");
|
|
else if (error == OTA_BEGIN_ERROR)
|
|
Serial.println("Begin Failed");
|
|
else if (error == OTA_CONNECT_ERROR)
|
|
Serial.println("Connect Failed");
|
|
else if (error == OTA_RECEIVE_ERROR)
|
|
Serial.println("Receive Failed");
|
|
else if (error == OTA_END_ERROR)
|
|
Serial.println("End Failed");
|
|
});
|
|
|
|
ArduinoOTA.begin();
|
|
}
|
|
|
|
void loop() {
|
|
// Serial.print(".");
|
|
// sleep(1);
|
|
remote->loop();
|
|
|
|
body.healthCheck();
|
|
ArduinoOTA.handle();
|
|
} |