philip/Hexapod
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Clang-format on everything

Browse Source
This commit is contained in:
Philip Johansson 2020-04-05 17:42:08 +02:00
parent 3fbac7f8a4
commit c9c87ae704
10 changed files with 370 additions and 420 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
include/IRemote.h
View File

@ -2,43 +2,41 @@
#include <Arduino.h> #include <Arduino.h>
class IRemote class IRemote {
{
public: 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 //! @brief Movement consists of the target velocity for
//! without movement //!
//! //! @arg vx Vel back/forward. m [-?, ?]
//! @arg roll/pitch Tilt it around. Deg [-45, 45] //! @arg vy Vel left/right. m [-?, ?]
//! @arg yaw Twist it. Deg [-45, 45] //! @arg w Rotational speed. deg/s [-20, 20]
//! @arg elevator Lower or raise it. m [0, 0.1] typedef struct {
typedef struct { float vx;
float roll; float vy;
float pitch; float w;
float yaw; } Movement;
float elevator;
} Attitude;
//! @brief Movement consists of the target velocity for typedef struct {
//! Attitude attitude;
//! @arg vx Vel back/forward. m [-?, ?] Movement movement;
//! @arg vy Vel left/right. m [-?, ?] bool isNew;
//! @arg w Rotational speed. deg/s [-20, 20] } Output;
typedef struct {
float vx;
float vy;
float w;
} Movement;
typedef struct { virtual const Output &output() = 0;
Attitude attitude;
Movement movement;
bool isNew;
} Output;
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;
}; };

30
include/OSCRemote.h
View File

@ -3,32 +3,32 @@
#include "Vbat.h" #include "Vbat.h"
#include <WiFiUdp.h>
#include <OSCMessage.h>
#include <OSCBundle.h> #include <OSCBundle.h>
#include <OSCData.h> #include <OSCData.h>
#include <OSCMessage.h>
#include <WiFiUdp.h>
#include <list> #include <list>
class OSCRemote : public IRemote class OSCRemote : public IRemote {
{
public: public:
//! Needs to be called in "Setup" //! Needs to be called in "Setup"
OSCRemote(const Vbat&); 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: private:
//! Place this after WiFi.begin() in main //! Place this after WiFi.begin() in main
void init(); void init();
IRemote::Output _output;
std::list<void(*)(const IRemote::Output&)> _callbacks; IRemote::Output _output;
const Vbat& _vbat; std::list<void (*)(const IRemote::Output &)> _callbacks;
const Vbat &_vbat;
}; };

9
include/Vbat.h
View File

@ -1,6 +1,5 @@
#pragma once #pragma once
//! @todo: //! @todo:
//! * Actual reading of battery //! * Actual reading of battery
//! * Some simple calibration method e.g via terminal //! * Some simple calibration method e.g via terminal
@ -9,10 +8,10 @@
//! @brief Keep track of the battery status //! @brief Keep track of the battery status
class Vbat { class Vbat {
public: public:
Vbat(); Vbat();
float getCurrentVoltage() const; float getCurrentVoltage() const;
//! @return Charge in % //! @return Charge in %
float getCurrentCharge() const; float getCurrentCharge() const;
}; };

69
include/body.h
View File

@ -1,58 +1,53 @@
#pragma once #pragma once
#include <vector>
#include <map> #include <map>
#include <vector>
class Leg
{ class Leg {
public: public:
enum Joint {
Hipp,
Knee,
};
enum Joint Leg(uint8_t hippIndex, uint8_t kneeIndex);
{
Hipp,
Knee,
};
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 //! @brief position in deg
void setTrim(Joint, uint8_t angle); void setPos(Joint, double angle);
//! @brief position in deg
void setPos(Joint, double angle);
private: private:
struct JointInfo { struct JointInfo {
uint8_t index; uint8_t index;
uint8_t trim; uint8_t trim;
double center; double center;
double pos; double pos;
}; };
std::map<Joint, JointInfo> _joints; std::map<Joint, JointInfo> _joints;
}; };
typedef std::vector<Leg> Legs; typedef std::vector<Leg> Legs;
class Body class Body {
{
public: public:
static Body& instance() static Body &instance() {
{ static Body instance;
static Body instance; return instance;
return instance; }
}
//! @brief Nescessary to configure the driver //! @brief Nescessary to configure the driver
void init(); void init();
//! @brief Check if driver has frozen or lost contact/sync //! @brief Check if driver has frozen or lost contact/sync
//! Resets the driver if nescesarry //! Resets the driver if nescesarry
void healthCheck(); void healthCheck();
Legs legs; Legs legs;
private: private:
Body(); Body();
}; };

27
include/config.h
View File

@ -5,26 +5,25 @@
class Config { class Config {
public: public:
typedef struct {
char wifiSSID[16];
char wifiPass[16];
} Data;
typedef struct { Config(){};
char wifiSSID[16];
char wifiPass[16];
} Data;
Config() {}; void init();
void init(); void load();
void load(); void save();
void save(Data &);
void save(); void setSSID(String);
void save(Data&); void setWifiPass(String);
void setSSID(String); const Data &data();
void setWifiPass(String);
const Data& data();
private: private:
Data _data; Data _data;
}; };

227
src/OSCRemote.cpp
View File

@ -3,179 +3,156 @@
//! --- Start of onfiguration --- //! --- Start of onfiguration ---
//#define DEBUG // Enables CM prints if commented //#define DEBUG // Enables CM prints if commented
const unsigned int outPort = 9999; const unsigned int outPort = 9999;
const unsigned int inPort = 8888; const unsigned int inPort = 8888;
//! --- End of configuration --- //! --- End of configuration ---
IRemote::Output* pOutput; IRemote::Output *pOutput;
namespace { namespace {
WiFiUDP* pUdp; WiFiUDP *pUdp;
IPAddress remoteIP; IPAddress remoteIP;
// Scaling // Scaling
//! @todo This is just temporary placement //! @todo This is just temporary placement
const float rollFactor = 45; const float rollFactor = 45;
const float pitchFactor = 45; const float pitchFactor = 45;
const float yawFactor = 45; const float yawFactor = 45;
const float elevatorFactor = 0.1; const float elevatorFactor = 0.1;
const float vxFactor = 0.1; const float vxFactor = 0.1;
const float vyFactor = 0.1; const float vyFactor = 0.1;
const float wFactor = 10; const float wFactor = 10;
void remoteMsgParser(OSCMessage &msg, int offset) void remoteMsgParser(OSCMessage &msg, int offset) {
{
char topic[20]; char topic[20];
msg.getAddress(topic); msg.getAddress(topic);
Serial.println(topic); Serial.println(topic);
if (!strcmp(topic, "/att/xy")) if (!strcmp(topic, "/att/xy")) {
{ pOutput->attitude.pitch = msg.getFloat(0) * pitchFactor;
pOutput->attitude.pitch = msg.getFloat(0)*pitchFactor; pOutput->attitude.roll = msg.getFloat(1) * rollFactor;
pOutput->attitude.roll = msg.getFloat(1)*rollFactor;
} }
else if (!strcmp(topic, "/att/z")) else if (!strcmp(topic, "/att/z")) {
{ pOutput->attitude.yaw = msg.getFloat(0) * yawFactor;
pOutput->attitude.yaw = msg.getFloat(0)*yawFactor;
}
else if (!strcmp(topic, "/att/e"))
{
pOutput->attitude.elevator = msg.getFloat(0)*elevatorFactor;
} }
else if (!strcmp(topic, "/mov/xy")) else if (!strcmp(topic, "/att/e")) {
{ pOutput->attitude.elevator = msg.getFloat(0) * elevatorFactor;
pOutput->movement.vy = msg.getFloat(0)*vxFactor;
pOutput->movement.vx = msg.getFloat(1)*vyFactor;
} }
else if (!strcmp(topic, "/mov/z")) else if (!strcmp(topic, "/mov/xy")) {
{ pOutput->movement.vy = msg.getFloat(0) * vxFactor;
pOutput->movement.w = msg.getFloat(0)*wFactor; pOutput->movement.vx = msg.getFloat(1) * vyFactor;
} }
else else if (!strcmp(topic, "/mov/z")) {
{ pOutput->movement.w = msg.getFloat(0) * wFactor;
Serial.println("OSC Unkown topic");
} }
} else {
Serial.println("OSC Unkown topic");
}
}
void remoteMsgParser(OSCMessage &msg) void remoteMsgParser(OSCMessage &msg) {
{
remoteMsgParser(msg, 0); remoteMsgParser(msg, 0);
} }
String getOSCErrorName(OSCErrorCode error) String getOSCErrorName(OSCErrorCode error) {
{ switch (error) {
switch(error) case OSC_OK:
{
case OSC_OK:
return String("OSC_OK"); return String("OSC_OK");
case BUFFER_FULL: case BUFFER_FULL:
return String("BUFFER_FULL"); return String("BUFFER_FULL");
case INVALID_OSC: case INVALID_OSC:
return String("INVALID_OSC"); return String("INVALID_OSC");
case ALLOCFAILED: case ALLOCFAILED:
return String("ALLOCFAILED"); return String("ALLOCFAILED");
case INDEX_OUT_OF_BOUNDS: case INDEX_OUT_OF_BOUNDS:
return String("INDEX_OUT_OF_BOUNDS"); return String("INDEX_OUT_OF_BOUNDS");
default: default:
return String("UNKOWN ERROR"); return String("UNKOWN ERROR");
} }
} }
template<class T> template <class T>
void msgOut(String topic, T data) void msgOut(String topic, T data) {
{
OSCMessage msg(topic.c_str()); OSCMessage msg(topic.c_str());
msg.add(data); msg.add(data);
pUdp->beginPacket(remoteIP, outPort); 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 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; OSCBundle msg;
int size = pUdp->parsePacket(); int size = pUdp->parsePacket();
remoteIP = pUdp->remoteIP(); remoteIP = pUdp->remoteIP();
if (size) if (size) {
{ while (size--) {
while (size--) { msg.fill(pUdp->read()); } 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);
msg.dispatch("/mov/xy", remoteMsgParser); #ifdef DEBUG
msg.dispatch("/mov/z", remoteMsgParser); Serial.printf("OSC packet from: %s\n", remoteIP.toString().c_str());
return true; #endif
}
else if (!msg.hasError()) {
{ msg.dispatch("/att/xy", remoteMsgParser);
#ifdef DEBUG msg.dispatch("/att/e", remoteMsgParser);
Serial.printf("error: %s\n", msg.dispatch("/att/z", remoteMsgParser);
getOSCErrorName(msg.getError()).c_str());
#endif 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; return false;
} }
} // namespace
OSCRemote::OSCRemote(const Vbat &vbat) : _vbat(vbat) {
init();
} }
OSCRemote::OSCRemote(const Vbat& vbat) void OSCRemote::init() {
: _vbat(vbat) pOutput = &_output;
{ pUdp = new WiFiUDP();
init(); pUdp->begin(inPort);
} }
void OSCRemote::init() void OSCRemote::loop() {
{ if (msgReceive()) {
pOutput = &_output; for (auto c : _callbacks) {
pUdp = new WiFiUDP(); c(_output);
pUdp->begin(inPort); }
}
void OSCRemote::loop()
{
if (msgReceive())
{
for (auto c : _callbacks)
{
c(_output);
} }
}
unsigned long t = millis(); unsigned long t = millis();
static unsigned long tTrigger = t; static unsigned long tTrigger = t;
const unsigned long updateInterval = 1000; const unsigned long updateInterval = 1000;
if (t >= tTrigger) if (t >= tTrigger) {
{ // TODO: convert to string and append %
// TODO: convert to string and append % char voltage[5]; // "100%\n"
char voltage[5]; // "100%\n" dtostrf(_vbat.getCurrentCharge(),
dtostrf(_vbat.getCurrentCharge(), 3, // lenght [0, 100]
3, // lenght [0, 100] 0, // precision
0, // precision voltage);
voltage); strcat(voltage, "%");
strcat(voltage, "%"); msgOut("/label_bat", voltage);
msgOut("/label_bat", voltage); tTrigger += updateInterval;
tTrigger += updateInterval; }
}
} }
const IRemote::Output& OSCRemote::output() const IRemote::Output &OSCRemote::output() {
{ return _output;
return _output;
} }
void OSCRemote::registerCallback(void (*callback)(const IRemote::Output&)) void OSCRemote::registerCallback(void (*callback)(const IRemote::Output &)) {
{ _callbacks.push_back(callback);
_callbacks.push_back(callback);
} }

13
src/Vbat.cpp
View File

@ -1,15 +1,12 @@
#include "Vbat.h" #include "Vbat.h"
Vbat::Vbat() Vbat::Vbat() {
{
} }
float Vbat::getCurrentVoltage() const float Vbat::getCurrentVoltage() const {
{ return 4.12;
return 4.12;
} }
float Vbat::getCurrentCharge() const float Vbat::getCurrentCharge() const {
{ return 87.0;
return 87.0;
} }

149
src/body.cpp
View File

@ -1,112 +1,121 @@
#include "body.h" #include "body.h"
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h> #include <Adafruit_PWMServoDriver.h>
#include <Arduino.h>
#include <SPI.h> #include <SPI.h>
#include <Wire.h>
#define CTRL_INACTIVE #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 // 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 // for max range. You'll have to tweak them as necessary to match the servos you
// have! // have!
#define SERVOMIN 190 // This is the 'minimum' pulse length count (out of 4096) #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 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 USMIN \
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 540 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 #define SERVO_FREQ 60 // Analog servos run at ~50 Hz updates
Adafruit_PWMServoDriver servoDriver = Adafruit_PWMServoDriver(0x40); Adafruit_PWMServoDriver servoDriver = Adafruit_PWMServoDriver(0x40);
unsigned long freqWatchDog = 0; 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() { void resetServoDriver() {
#ifndef CTRL_INACTIVE #ifndef CTRL_INACTIVE
servoDriver.begin(); servoDriver.begin();
servoDriver.setPWMFreq(SERVO_FREQ); // Analog servos run at ~60 Hz updates servoDriver.setPWMFreq(SERVO_FREQ); // Analog servos run at ~60 Hz updates
#endif #endif
} }
void checkForServoSleep() { void checkForServoSleep() {
#ifndef CTRL_INACTIVE #ifndef CTRL_INACTIVE
if (millis() > freqWatchDog) if (millis() > freqWatchDog) {
{
// See if the servo driver module went to sleep, probably due to a short power dip // See if the servo driver module went to sleep, probably due to a short
Wire.beginTransmission(SERVO_IIC_ADDR); // power dip
Wire.write(0); // address 0 is the MODE1 location of the servo driver, see documentation on the PCA9685 chip for more info Wire.beginTransmission(SERVO_IIC_ADDR);
Wire.endTransmission(); Wire.write(0); // address 0 is the MODE1 location of the servo driver,
Wire.requestFrom((uint8_t)SERVO_IIC_ADDR, (uint8_t)1); // see documentation on the PCA9685 chip for more info
int mode1 = Wire.read(); Wire.endTransmission();
if (mode1 & 16) Wire.requestFrom((uint8_t)SERVO_IIC_ADDR, (uint8_t)1);
{ // the fifth bit up from the bottom is 1 if controller was asleep int mode1 = Wire.read();
// wake it up! if (mode1 & 16) { // the fifth bit up from the bottom is 1 if controller
resetServoDriver(); // was asleep
//beep(1200,100); // chirp to warn user of brown out on servo controller // wake it up!
SuppressScamperUntil = millis() + 10000; // no scamper for you! (for 10 seconds because we ran out of power, give the battery resetServoDriver();
// a bit of time for charge migration and let the servos cool down) // 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 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; double temp =
temp = (int) (4*temp + .5); (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
return (double) temp/4; temp = (int)(4 * temp + .5);
return (double)temp / 4;
} }
Body::Body() Body::Body() : legs() {
: legs() uint8_t numOfLegs = 6;
{ for (uint8_t i = 0; i < numOfLegs; ++i) {
uint8_t numOfLegs = 6; legs.push_back(Leg(i, i + 6));
for (uint8_t i = 0; i < numOfLegs; ++i) }
{
legs.push_back(Leg(i, i+6));
}
} }
void Body::init() void Body::init() {
{ resetServoDriver();
resetServoDriver();
} }
void Body::healthCheck() void Body::healthCheck() {
{ checkForServoSleep();
checkForServoSleep();
} }
Leg::Leg(uint8_t hipp, uint8_t knee) Leg::Leg(uint8_t hipp, uint8_t knee)
: _joints({{Joint::Hipp, {.index=hipp, .trim=0, .center=0.0, .pos=0}}, : _joints(
{Joint::Knee, {.index=knee, .trim=0, .center=45.0, .pos=0}}}) {{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) void Leg::setTrim(Joint j, uint8_t angle) {
{ _joints[j].trim = angle;
_joints[j].trim = angle;
} }
void Leg::setPos(Joint j, double angle) void Leg::setPos(Joint j, double angle) {
{ //! @todo Trying to evaluate if one of these has better performance on the
//! @todo Trying to evaluate if one of these has better performance on the servo //! servo
#ifndef CTRL_INACTIVE #ifndef CTRL_INACTIVE
auto angleToPulse = [](double a) auto angleToPulse = [](double a) {
{ return modifiedMap(a, -180.0, 180.0, SERVOMIN, SERVOMAX); }; return modifiedMap(a, -180.0, 180.0, SERVOMIN, SERVOMAX);
//servoDriver.setPWM(_joints[j].index, 0, angleToPulse(angle)); };
// servoDriver.setPWM(_joints[j].index, 0, angleToPulse(angle));
auto angleToMicroPulse = [](double a) auto angleToMicroPulse = [](double a) {
{ return modifiedMap(a, -180.0, 180.0, USMIN, USMAX); }; return modifiedMap(a, -180.0, 180.0, USMIN, USMAX);
servoDriver.writeMicroseconds(_joints[j].index, angleToMicroPulse(angle) + _joints[j].center); };
#endif servoDriver.writeMicroseconds(_joints[j].index,
angleToMicroPulse(angle) + _joints[j].center);
#endif
_joints[j].pos = angle; _joints[j].pos = angle;
} }

57
src/config.cpp
View File

@ -1,48 +1,39 @@
#include "config.h" #include "config.h"
void Config::init() void Config::init() {
{ EEPROM.begin(EEPROM_SIZE);
EEPROM.begin(EEPROM_SIZE);
} }
void Config::setSSID(String ssid) void Config::setSSID(String ssid) {
{ strcpy(_data.wifiSSID, ssid.c_str());
strcpy(_data.wifiSSID, ssid.c_str());
}; };
void Config::setWifiPass(String pass) void Config::setWifiPass(String pass) {
{ strcpy(_data.wifiPass, pass.c_str());
strcpy(_data.wifiPass, pass.c_str());
}; };
void Config::load() void Config::load() {
{ byte *pBuff = (byte *)(const void *)&_data;
byte* pBuff = (byte*)(const void*)&_data; for (size_t i = 0; i < sizeof(_data); ++i) {
for (size_t i = 0; i < sizeof(_data); ++i) *pBuff++ = EEPROM.read(i);
{ }
*pBuff++ = EEPROM.read(i); Serial.println("EEPROM Loaded");
}
Serial.println("EEPROM Loaded");
}; };
void Config::save() void Config::save() {
{ const byte *pBuff = (const byte *)(const void *)&_data;
const byte* pBuff = (const byte*)(const void*)&_data; for (size_t i = 0; i < sizeof(_data); ++i) {
for (size_t i = 0; i < sizeof(_data); ++i) EEPROM.write(i, *pBuff++);
{ }
EEPROM.write(i, *pBuff++); EEPROM.commit();
} Serial.println("EEPROM Saved");
EEPROM.commit();
Serial.println("EEPROM Saved");
}; };
void Config::save(Config::Data& data) void Config::save(Config::Data &data) {
{ _data = data;
_data = data; save();
save();
} }
const Config::Data& Config::data() const Config::Data &Config::data() {
{ return _data;
return _data;
} }

145
src/main.cpp
View File

@ -1,108 +1,93 @@
#include <Arduino.h> #include <Arduino.h>
#include <WiFi.h> #include <WiFi.h>
#include "body.h"
#include "config.h"
#include "OSCRemote.h" #include "OSCRemote.h"
#include "Vbat.h" #include "Vbat.h"
#include "body.h"
#include "config.h"
//! --- Start of onfiguration --- //! --- Start of onfiguration ---
const char* hostname = "Hexapod"; const char *hostname = "Hexapod";
//! --- End of configuration --- //! --- End of configuration ---
Body body = Body::instance(); Body body = Body::instance();
Config config; Config config;
IRemote* remote; IRemote *remote;
Vbat vbat; Vbat vbat;
void servoTest() { void servoTest() {
for (uint8_t legnum = 0; legnum < body.legs.size(); legnum++) for (uint8_t legnum = 0; legnum < body.legs.size(); legnum++) {
{ body.legs[legnum].setPos(Leg::Hipp, 0);
body.legs[legnum].setPos(Leg::Hipp, 0); body.legs[legnum].setPos(Leg::Knee, 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 (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(".");
} }
} Serial.println("");
d++; 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() { void setup() {
Serial.begin(9600); Serial.begin(9600);
body.init(); body.init();
config.init(); config.init();
delay(250); delay(250);
servoTest();
servoTest(); config.load();
config.load();
Serial.printf("SSID: %s\n", config.data().wifiSSID); Serial.printf("SSID: %s\n", config.data().wifiSSID);
Serial.printf("pass: %s\n", config.data().wifiPass); Serial.printf("pass: %s\n", config.data().wifiPass);
connectWiFi(); connectWiFi();
remote = new OSCRemote(vbat); remote = new OSCRemote(vbat);
remote->registerCallback(testRemoteCallback); remote->registerCallback(testRemoteCallback);
//remote->init(); // remote->init();
// Only to save as a test
/*
// Only to save as a test config.setSSID("testSSID");
/* config.setWifiPass("testPass");
config.setSSID("testSSID"); config.save();
config.setWifiPass("testPass"); */
config.save(); sweepLeg();
*/
sweepLeg();
} }
void loop() { void loop() {
//Serial.print("."); // Serial.print(".");
//sleep(1); // sleep(1);
remote->loop(); remote->loop();
body.healthCheck();
body.healthCheck();
} }