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>
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;
};

30
include/OSCRemote.h
View File

@ -3,32 +3,32 @@
#include "Vbat.h"
#include <WiFiUdp.h>
#include <OSCMessage.h>
#include <OSCBundle.h>
#include <OSCData.h>
#include <OSCMessage.h>
#include <WiFiUdp.h>
#include <list>
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<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
//! @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;
};

69
include/body.h
View File

@ -1,58 +1,53 @@
#pragma once
#include <vector>
#include <map>
#include <vector>
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<Joint, JointInfo> _joints;
std::map<Joint, JointInfo> _joints;
};
typedef std::vector<Leg> 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();
};

27
include/config.h
View File

@ -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;
};

227
src/OSCRemote.cpp
View File

@ -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<class T>
void msgOut(String topic, T data)
{
template <class T>
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);
}

13
src/Vbat.cpp
View File

@ -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;
}

149
src/body.cpp
View File

@ -1,112 +1,121 @@
#include "body.h"
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Arduino.h>
#include <SPI.h>
#include <Wire.h>
#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;
}

57
src/config.cpp
View File

@ -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;
}

145
src/main.cpp
View File

@ -1,108 +1,93 @@
#include <Arduino.h>
#include <WiFi.h>
#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();
}