From c9c87ae70460de47f301a5b87f1f8c919dc546a3 Mon Sep 17 00:00:00 2001 From: Philip Johansson Date: Sun, 5 Apr 2020 17:42:08 +0200 Subject: [PATCH] Clang-format on everything --- include/IRemote.h | 64 ++++++------- include/OSCRemote.h | 30 +++--- include/Vbat.h | 9 +- include/body.h | 69 +++++++------- include/config.h | 27 +++--- src/OSCRemote.cpp | 227 ++++++++++++++++++++------------------------ src/Vbat.cpp | 13 +-- src/body.cpp | 149 +++++++++++++++-------------- src/config.cpp | 57 +++++------ src/main.cpp | 145 +++++++++++++--------------- 10 files changed, 370 insertions(+), 420 deletions(-) diff --git a/include/IRemote.h b/include/IRemote.h index c494aa1..5f376f0 100644 --- a/include/IRemote.h +++ b/include/IRemote.h @@ -2,43 +2,41 @@ #include -class IRemote -{ +class IRemote { public: + //! @brief Attitude is the "pose" of the Hexapod + //! without movement + //! + //! @arg roll/pitch Tilt it around. Deg [-45, 45] + //! @arg yaw Twist it. Deg [-45, 45] + //! @arg elevator Lower or raise it. m [0, 0.1] + typedef struct { + float roll; + float pitch; + float yaw; + float elevator; + } Attitude; - //! @brief Attitude is the "pose" of the Hexapod - //! without movement - //! - //! @arg roll/pitch Tilt it around. Deg [-45, 45] - //! @arg yaw Twist it. Deg [-45, 45] - //! @arg elevator Lower or raise it. m [0, 0.1] - typedef struct { - float roll; - float pitch; - float yaw; - float elevator; - } Attitude; + //! @brief Movement consists of the target velocity for + //! + //! @arg vx Vel back/forward. m [-?, ?] + //! @arg vy Vel left/right. m [-?, ?] + //! @arg w Rotational speed. deg/s [-20, 20] + typedef struct { + float vx; + float vy; + float w; + } Movement; - //! @brief Movement consists of the target velocity for - //! - //! @arg vx Vel back/forward. m [-?, ?] - //! @arg vy Vel left/right. m [-?, ?] - //! @arg w Rotational speed. deg/s [-20, 20] - typedef struct { - float vx; - float vy; - float w; - } Movement; + typedef struct { + Attitude attitude; + Movement movement; + bool isNew; + } Output; - typedef struct { - Attitude attitude; - Movement movement; - bool isNew; - } Output; + virtual const Output &output() = 0; - virtual const Output& output() = 0; + virtual void loop() = 0; - virtual void loop() = 0; - - virtual void registerCallback(void (*callback)(const Output&)) = 0; + virtual void registerCallback(void (*callback)(const Output &)) = 0; }; \ No newline at end of file diff --git a/include/OSCRemote.h b/include/OSCRemote.h index 89b20dc..9871116 100644 --- a/include/OSCRemote.h +++ b/include/OSCRemote.h @@ -3,32 +3,32 @@ #include "Vbat.h" -#include -#include #include #include +#include +#include + #include -class OSCRemote : public IRemote -{ +class OSCRemote : public IRemote { public: - //! Needs to be called in "Setup" - OSCRemote(const Vbat&); + //! Needs to be called in "Setup" + OSCRemote(const Vbat &); - void loop(); + void loop(); - const IRemote::Output& output() override; + const IRemote::Output &output() override; - void registerCallback(void (*callback)(const IRemote::Output&)) override; + void registerCallback(void (*callback)(const IRemote::Output &)) override; private: - //! Place this after WiFi.begin() in main - void init(); - - IRemote::Output _output; + //! Place this after WiFi.begin() in main + void init(); - std::list _callbacks; + IRemote::Output _output; - const Vbat& _vbat; + std::list _callbacks; + + const Vbat &_vbat; }; \ No newline at end of file diff --git a/include/Vbat.h b/include/Vbat.h index 020a6aa..66dfe30 100644 --- a/include/Vbat.h +++ b/include/Vbat.h @@ -1,6 +1,5 @@ #pragma once - //! @todo: //! * Actual reading of battery //! * Some simple calibration method e.g via terminal @@ -9,10 +8,10 @@ //! @brief Keep track of the battery status class Vbat { public: - Vbat(); + Vbat(); - float getCurrentVoltage() const; + float getCurrentVoltage() const; - //! @return Charge in % - float getCurrentCharge() const; + //! @return Charge in % + float getCurrentCharge() const; }; \ No newline at end of file diff --git a/include/body.h b/include/body.h index fcbe962..e734923 100644 --- a/include/body.h +++ b/include/body.h @@ -1,58 +1,53 @@ #pragma once -#include #include +#include -class Leg -{ + +class Leg { public: + enum Joint { + Hipp, + Knee, + }; - enum Joint - { - Hipp, - Knee, - }; + Leg(uint8_t hippIndex, uint8_t kneeIndex); - Leg(uint8_t hippIndex, uint8_t kneeIndex); + //! @brief positive trim in deg to center this joint + void setTrim(Joint, uint8_t angle); - //! @brief positive trim in deg to center this joint - void setTrim(Joint, uint8_t angle); - - //! @brief position in deg - void setPos(Joint, double angle); + //! @brief position in deg + void setPos(Joint, double angle); private: - struct JointInfo { - uint8_t index; - uint8_t trim; - double center; - double pos; - }; + struct JointInfo { + uint8_t index; + uint8_t trim; + double center; + double pos; + }; - std::map _joints; + std::map _joints; }; typedef std::vector Legs; -class Body -{ +class Body { public: - static Body& instance() - { - static Body instance; - return instance; - } + static Body &instance() { + static Body instance; + return instance; + } - //! @brief Nescessary to configure the driver - void init(); + //! @brief Nescessary to configure the driver + void init(); - //! @brief Check if driver has frozen or lost contact/sync - //! Resets the driver if nescesarry - void healthCheck(); - - Legs legs; + //! @brief Check if driver has frozen or lost contact/sync + //! Resets the driver if nescesarry + void healthCheck(); + + Legs legs; private: - Body(); - + Body(); }; \ No newline at end of file diff --git a/include/config.h b/include/config.h index 89ab5fc..b394442 100644 --- a/include/config.h +++ b/include/config.h @@ -5,26 +5,25 @@ class Config { public: + typedef struct { + char wifiSSID[16]; + char wifiPass[16]; + } Data; - typedef struct { - char wifiSSID[16]; - char wifiPass[16]; - } Data; + Config(){}; - Config() {}; + void init(); - void init(); + void load(); - void load(); + void save(); + void save(Data &); - void save(); - void save(Data&); + void setSSID(String); + void setWifiPass(String); - void setSSID(String); - void setWifiPass(String); - - const Data& data(); + const Data &data(); private: - Data _data; + Data _data; }; \ No newline at end of file diff --git a/src/OSCRemote.cpp b/src/OSCRemote.cpp index da50be0..61af86e 100644 --- a/src/OSCRemote.cpp +++ b/src/OSCRemote.cpp @@ -3,179 +3,156 @@ //! --- Start of onfiguration --- //#define DEBUG // Enables CM prints if commented -const unsigned int outPort = 9999; +const unsigned int outPort = 9999; const unsigned int inPort = 8888; //! --- End of configuration --- -IRemote::Output* pOutput; +IRemote::Output *pOutput; namespace { - WiFiUDP* pUdp; - IPAddress remoteIP; +WiFiUDP *pUdp; +IPAddress remoteIP; - // Scaling - //! @todo This is just temporary placement - const float rollFactor = 45; - const float pitchFactor = 45; - const float yawFactor = 45; - const float elevatorFactor = 0.1; - const float vxFactor = 0.1; - const float vyFactor = 0.1; - const float wFactor = 10; +// Scaling +//! @todo This is just temporary placement +const float rollFactor = 45; +const float pitchFactor = 45; +const float yawFactor = 45; +const float elevatorFactor = 0.1; +const float vxFactor = 0.1; +const float vyFactor = 0.1; +const float wFactor = 10; - void remoteMsgParser(OSCMessage &msg, int offset) - { +void remoteMsgParser(OSCMessage &msg, int offset) { char topic[20]; msg.getAddress(topic); Serial.println(topic); - if (!strcmp(topic, "/att/xy")) - { - pOutput->attitude.pitch = msg.getFloat(0)*pitchFactor; - pOutput->attitude.roll = msg.getFloat(1)*rollFactor; + if (!strcmp(topic, "/att/xy")) { + pOutput->attitude.pitch = msg.getFloat(0) * pitchFactor; + pOutput->attitude.roll = msg.getFloat(1) * rollFactor; } - else if (!strcmp(topic, "/att/z")) - { - pOutput->attitude.yaw = msg.getFloat(0)*yawFactor; - } - else if (!strcmp(topic, "/att/e")) - { - pOutput->attitude.elevator = msg.getFloat(0)*elevatorFactor; + else if (!strcmp(topic, "/att/z")) { + pOutput->attitude.yaw = msg.getFloat(0) * yawFactor; } - else if (!strcmp(topic, "/mov/xy")) - { - pOutput->movement.vy = msg.getFloat(0)*vxFactor; - pOutput->movement.vx = msg.getFloat(1)*vyFactor; + else if (!strcmp(topic, "/att/e")) { + pOutput->attitude.elevator = msg.getFloat(0) * elevatorFactor; } - else if (!strcmp(topic, "/mov/z")) - { - pOutput->movement.w = msg.getFloat(0)*wFactor; + else if (!strcmp(topic, "/mov/xy")) { + pOutput->movement.vy = msg.getFloat(0) * vxFactor; + pOutput->movement.vx = msg.getFloat(1) * vyFactor; } - else - { - Serial.println("OSC Unkown topic"); + else if (!strcmp(topic, "/mov/z")) { + pOutput->movement.w = msg.getFloat(0) * wFactor; } - } + else { + Serial.println("OSC Unkown topic"); + } +} - void remoteMsgParser(OSCMessage &msg) - { +void remoteMsgParser(OSCMessage &msg) { remoteMsgParser(msg, 0); - } +} - String getOSCErrorName(OSCErrorCode error) - { - switch(error) - { - case OSC_OK: +String getOSCErrorName(OSCErrorCode error) { + switch (error) { + case OSC_OK: return String("OSC_OK"); - case BUFFER_FULL: + case BUFFER_FULL: return String("BUFFER_FULL"); - case INVALID_OSC: + case INVALID_OSC: return String("INVALID_OSC"); - case ALLOCFAILED: + case ALLOCFAILED: return String("ALLOCFAILED"); - case INDEX_OUT_OF_BOUNDS: + case INDEX_OUT_OF_BOUNDS: return String("INDEX_OUT_OF_BOUNDS"); - default: + default: return String("UNKOWN ERROR"); } - } +} - template - void msgOut(String topic, T data) - { +template +void msgOut(String topic, T data) { OSCMessage msg(topic.c_str()); msg.add(data); pUdp->beginPacket(remoteIP, outPort); - msg.send(*pUdp); // send the bytes + msg.send(*pUdp); // send the bytes pUdp->endPacket(); // mark the end of the OSC Packet - msg.empty(); // free space occupied by message - } + msg.empty(); // free space occupied by message +} - bool msgReceive() - { +bool msgReceive() { OSCBundle msg; int size = pUdp->parsePacket(); remoteIP = pUdp->remoteIP(); - if (size) - { - while (size--) { msg.fill(pUdp->read()); } - - #ifdef DEBUG - Serial.printf("OSC packet from: %s\n", - remoteIP.toString().c_str()); - #endif - - if (!msg.hasError()) - { - msg.dispatch("/att/xy", remoteMsgParser); - msg.dispatch("/att/e", remoteMsgParser); - msg.dispatch("/att/z", remoteMsgParser); + if (size) { + while (size--) { + msg.fill(pUdp->read()); + } - msg.dispatch("/mov/xy", remoteMsgParser); - msg.dispatch("/mov/z", remoteMsgParser); - return true; - } - else - { - #ifdef DEBUG - Serial.printf("error: %s\n", - getOSCErrorName(msg.getError()).c_str()); - #endif - } +#ifdef DEBUG + Serial.printf("OSC packet from: %s\n", remoteIP.toString().c_str()); +#endif + + if (!msg.hasError()) { + msg.dispatch("/att/xy", remoteMsgParser); + msg.dispatch("/att/e", remoteMsgParser); + msg.dispatch("/att/z", remoteMsgParser); + + msg.dispatch("/mov/xy", remoteMsgParser); + msg.dispatch("/mov/z", remoteMsgParser); + return true; + } + else { +#ifdef DEBUG + Serial.printf("error: %s\n", + getOSCErrorName(msg.getError()).c_str()); +#endif + } } return false; - } +} +} // namespace + +OSCRemote::OSCRemote(const Vbat &vbat) : _vbat(vbat) { + init(); } -OSCRemote::OSCRemote(const Vbat& vbat) -: _vbat(vbat) -{ - init(); +void OSCRemote::init() { + pOutput = &_output; + pUdp = new WiFiUDP(); + pUdp->begin(inPort); } -void OSCRemote::init() -{ - pOutput = &_output; - pUdp = new WiFiUDP(); - pUdp->begin(inPort); -} - -void OSCRemote::loop() -{ - if (msgReceive()) - { - for (auto c : _callbacks) - { - c(_output); +void OSCRemote::loop() { + if (msgReceive()) { + for (auto c : _callbacks) { + c(_output); + } } - } - unsigned long t = millis(); - static unsigned long tTrigger = t; - const unsigned long updateInterval = 1000; - if (t >= tTrigger) - { - // TODO: convert to string and append % - char voltage[5]; // "100%\n" - dtostrf(_vbat.getCurrentCharge(), - 3, // lenght [0, 100] - 0, // precision - voltage); - strcat(voltage, "%"); - msgOut("/label_bat", voltage); - tTrigger += updateInterval; - } + unsigned long t = millis(); + static unsigned long tTrigger = t; + const unsigned long updateInterval = 1000; + if (t >= tTrigger) { + // TODO: convert to string and append % + char voltage[5]; // "100%\n" + dtostrf(_vbat.getCurrentCharge(), + 3, // lenght [0, 100] + 0, // precision + voltage); + strcat(voltage, "%"); + msgOut("/label_bat", voltage); + tTrigger += updateInterval; + } } -const IRemote::Output& OSCRemote::output() -{ - return _output; +const IRemote::Output &OSCRemote::output() { + return _output; } -void OSCRemote::registerCallback(void (*callback)(const IRemote::Output&)) -{ - _callbacks.push_back(callback); +void OSCRemote::registerCallback(void (*callback)(const IRemote::Output &)) { + _callbacks.push_back(callback); } \ No newline at end of file diff --git a/src/Vbat.cpp b/src/Vbat.cpp index 66509e2..0db519b 100644 --- a/src/Vbat.cpp +++ b/src/Vbat.cpp @@ -1,15 +1,12 @@ #include "Vbat.h" -Vbat::Vbat() -{ +Vbat::Vbat() { } -float Vbat::getCurrentVoltage() const -{ - return 4.12; +float Vbat::getCurrentVoltage() const { + return 4.12; } -float Vbat::getCurrentCharge() const -{ - return 87.0; +float Vbat::getCurrentCharge() const { + return 87.0; } \ No newline at end of file diff --git a/src/body.cpp b/src/body.cpp index 20c2766..f466785 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -1,112 +1,121 @@ #include "body.h" -#include -#include #include +#include #include +#include #define CTRL_INACTIVE +#define SERVO_IIC_ADDR (0x40) -#define SERVO_IIC_ADDR (0x40) - -// Depending on your servo make, the pulse width min and max may vary, you +// Depending on your servo make, the pulse width min and max may vary, you // want these to be as small/large as possible without hitting the hard stop // for max range. You'll have to tweak them as necessary to match the servos you // have! -#define SERVOMIN 190 // This is the 'minimum' pulse length count (out of 4096) -#define SERVOMAX 540 // This is the 'maximum' pulse length count (out of 4096) -#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 190 -#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 540 +#define SERVOMIN 190 // This is the 'minimum' pulse length count (out of 4096) +#define SERVOMAX 540 // This is the 'maximum' pulse length count (out of 4096) +#define USMIN \ + 600 // This is the rounded 'minimum' microsecond length based on the minimum + // pulse of 190 +#define USMAX \ + 2400 // This is the rounded 'maximum' microsecond length based on the + // maximum pulse of 540 #define SERVO_FREQ 60 // Analog servos run at ~50 Hz updates Adafruit_PWMServoDriver servoDriver = Adafruit_PWMServoDriver(0x40); unsigned long freqWatchDog = 0; -unsigned long SuppressScamperUntil = 0; // if we had to wake up the servos, suppress the power hunger scamper mode for a while +unsigned long SuppressScamperUntil = + 0; // if we had to wake up the servos, suppress the power hunger scamper + // mode for a while void resetServoDriver() { - #ifndef CTRL_INACTIVE - servoDriver.begin(); - servoDriver.setPWMFreq(SERVO_FREQ); // Analog servos run at ~60 Hz updates - #endif +#ifndef CTRL_INACTIVE + servoDriver.begin(); + servoDriver.setPWMFreq(SERVO_FREQ); // Analog servos run at ~60 Hz updates +#endif } void checkForServoSleep() { - #ifndef CTRL_INACTIVE - if (millis() > freqWatchDog) - { +#ifndef CTRL_INACTIVE + if (millis() > freqWatchDog) { - // See if the servo driver module went to sleep, probably due to a short power dip - Wire.beginTransmission(SERVO_IIC_ADDR); - Wire.write(0); // address 0 is the MODE1 location of the servo driver, see documentation on the PCA9685 chip for more info - Wire.endTransmission(); - Wire.requestFrom((uint8_t)SERVO_IIC_ADDR, (uint8_t)1); - int mode1 = Wire.read(); - if (mode1 & 16) - { // the fifth bit up from the bottom is 1 if controller was asleep - // wake it up! - resetServoDriver(); - //beep(1200,100); // chirp to warn user of brown out on servo controller - SuppressScamperUntil = millis() + 10000; // no scamper for you! (for 10 seconds because we ran out of power, give the battery - // a bit of time for charge migration and let the servos cool down) + // See if the servo driver module went to sleep, probably due to a short + // power dip + Wire.beginTransmission(SERVO_IIC_ADDR); + Wire.write(0); // address 0 is the MODE1 location of the servo driver, + // see documentation on the PCA9685 chip for more info + Wire.endTransmission(); + Wire.requestFrom((uint8_t)SERVO_IIC_ADDR, (uint8_t)1); + int mode1 = Wire.read(); + if (mode1 & 16) { // the fifth bit up from the bottom is 1 if controller + // was asleep + // wake it up! + resetServoDriver(); + // beep(1200,100); // chirp to warn user of brown out on servo + // controller + SuppressScamperUntil = + millis() + + 10000; // no scamper for you! (for 10 seconds because we ran out + // of power, give the battery a bit of time for charge + // migration and let the servos cool down) + } + freqWatchDog = millis() + 100; } - freqWatchDog = millis() + 100; - } - #endif +#endif } -double modifiedMap(double x, double in_min, double in_max, double out_min, double out_max) -{ - double temp = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; - temp = (int) (4*temp + .5); - return (double) temp/4; +double modifiedMap( + double x, double in_min, double in_max, double out_min, double out_max) { + double temp = + (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + temp = (int)(4 * temp + .5); + return (double)temp / 4; } -Body::Body() -: legs() -{ - uint8_t numOfLegs = 6; - for (uint8_t i = 0; i < numOfLegs; ++i) - { - legs.push_back(Leg(i, i+6)); - } +Body::Body() : legs() { + uint8_t numOfLegs = 6; + for (uint8_t i = 0; i < numOfLegs; ++i) { + legs.push_back(Leg(i, i + 6)); + } } -void Body::init() -{ - resetServoDriver(); +void Body::init() { + resetServoDriver(); } -void Body::healthCheck() -{ - checkForServoSleep(); +void Body::healthCheck() { + checkForServoSleep(); } Leg::Leg(uint8_t hipp, uint8_t knee) -: _joints({{Joint::Hipp, {.index=hipp, .trim=0, .center=0.0, .pos=0}}, - {Joint::Knee, {.index=knee, .trim=0, .center=45.0, .pos=0}}}) -{ + : _joints( + {{Joint::Hipp, {.index = hipp, .trim = 0, .center = 0.0, .pos = 0}}, + {Joint::Knee, + {.index = knee, .trim = 0, .center = 45.0, .pos = 0}}}) { } -void Leg::setTrim(Joint j, uint8_t angle) -{ - _joints[j].trim = angle; +void Leg::setTrim(Joint j, uint8_t angle) { + _joints[j].trim = angle; } -void Leg::setPos(Joint j, double angle) -{ - //! @todo Trying to evaluate if one of these has better performance on the servo +void Leg::setPos(Joint j, double angle) { + //! @todo Trying to evaluate if one of these has better performance on the + //! servo - #ifndef CTRL_INACTIVE - auto angleToPulse = [](double a) - { return modifiedMap(a, -180.0, 180.0, SERVOMIN, SERVOMAX); }; - //servoDriver.setPWM(_joints[j].index, 0, angleToPulse(angle)); +#ifndef CTRL_INACTIVE + auto angleToPulse = [](double a) { + return modifiedMap(a, -180.0, 180.0, SERVOMIN, SERVOMAX); + }; + // servoDriver.setPWM(_joints[j].index, 0, angleToPulse(angle)); - auto angleToMicroPulse = [](double a) - { return modifiedMap(a, -180.0, 180.0, USMIN, USMAX); }; - servoDriver.writeMicroseconds(_joints[j].index, angleToMicroPulse(angle) + _joints[j].center); - #endif + auto angleToMicroPulse = [](double a) { + return modifiedMap(a, -180.0, 180.0, USMIN, USMAX); + }; + servoDriver.writeMicroseconds(_joints[j].index, + angleToMicroPulse(angle) + _joints[j].center); +#endif - _joints[j].pos = angle; + _joints[j].pos = angle; } \ No newline at end of file diff --git a/src/config.cpp b/src/config.cpp index 48e0fc4..a167859 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -1,48 +1,39 @@ #include "config.h" -void Config::init() -{ - EEPROM.begin(EEPROM_SIZE); +void Config::init() { + EEPROM.begin(EEPROM_SIZE); } -void Config::setSSID(String ssid) -{ - strcpy(_data.wifiSSID, ssid.c_str()); +void Config::setSSID(String ssid) { + strcpy(_data.wifiSSID, ssid.c_str()); }; -void Config::setWifiPass(String pass) -{ - strcpy(_data.wifiPass, pass.c_str()); +void Config::setWifiPass(String pass) { + strcpy(_data.wifiPass, pass.c_str()); }; -void Config::load() -{ - byte* pBuff = (byte*)(const void*)&_data; - for (size_t i = 0; i < sizeof(_data); ++i) - { - *pBuff++ = EEPROM.read(i); - } - Serial.println("EEPROM Loaded"); +void Config::load() { + byte *pBuff = (byte *)(const void *)&_data; + for (size_t i = 0; i < sizeof(_data); ++i) { + *pBuff++ = EEPROM.read(i); + } + Serial.println("EEPROM Loaded"); }; -void Config::save() -{ - const byte* pBuff = (const byte*)(const void*)&_data; - for (size_t i = 0; i < sizeof(_data); ++i) - { - EEPROM.write(i, *pBuff++); - } - EEPROM.commit(); - Serial.println("EEPROM Saved"); +void Config::save() { + const byte *pBuff = (const byte *)(const void *)&_data; + for (size_t i = 0; i < sizeof(_data); ++i) { + EEPROM.write(i, *pBuff++); + } + EEPROM.commit(); + Serial.println("EEPROM Saved"); }; -void Config::save(Config::Data& data) -{ - _data = data; - save(); +void Config::save(Config::Data &data) { + _data = data; + save(); } -const Config::Data& Config::data() -{ - return _data; +const Config::Data &Config::data() { + return _data; } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 91636a9..cffa83a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,108 +1,93 @@ #include #include -#include "body.h" -#include "config.h" #include "OSCRemote.h" #include "Vbat.h" +#include "body.h" +#include "config.h" + //! --- Start of onfiguration --- -const char* hostname = "Hexapod"; +const char *hostname = "Hexapod"; //! --- End of configuration --- - Body body = Body::instance(); Config config; -IRemote* remote; +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); - WiFi.begin(config.data().wifiSSID, config.data().wifiPass); - 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 (uint8_t legnum = 0; legnum < body.legs.size(); legnum++) { + body.legs[legnum].setPos(Leg::Hipp, 0); + body.legs[legnum].setPos(Leg::Knee, 0); } - for (long angle = 180*5; angle > 0; angle--) - { - body.legs[1].setPos(Leg::Knee, double(angle/5)); +} + +void connectWiFi() { + Serial.printf("Connecting to %s\n", config.data().wifiSSID); + WiFi.begin(config.data().wifiSSID, config.data().wifiPass); + WiFi.setHostname(hostname); + while (WiFi.status() != WL_CONNECTED) { + delay(500); + Serial.print("."); } - } - d++; + 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 setup() { - Serial.begin(9600); + Serial.begin(9600); - body.init(); - config.init(); - delay(250); + body.init(); + config.init(); + delay(250); - - servoTest(); - config.load(); + servoTest(); + config.load(); - Serial.printf("SSID: %s\n", config.data().wifiSSID); - Serial.printf("pass: %s\n", config.data().wifiPass); - connectWiFi(); - remote = new OSCRemote(vbat); - remote->registerCallback(testRemoteCallback); - //remote->init(); + Serial.printf("SSID: %s\n", config.data().wifiSSID); + Serial.printf("pass: %s\n", config.data().wifiPass); + connectWiFi(); + remote = new OSCRemote(vbat); + remote->registerCallback(testRemoteCallback); + // remote->init(); - - - // Only to save as a test - /* - config.setSSID("testSSID"); - config.setWifiPass("testPass"); - config.save(); - */ - sweepLeg(); + // Only to save as a test + /* + config.setSSID("testSSID"); + config.setWifiPass("testPass"); + config.save(); + */ + sweepLeg(); } - void loop() { - //Serial.print("."); - //sleep(1); - remote->loop(); - + // Serial.print("."); + // sleep(1); + remote->loop(); - - body.healthCheck(); + body.healthCheck(); } \ No newline at end of file