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

Removed deg for rad and set physical limits

Browse Source
This commit is contained in:
Philip Johansson 2021-01-02 23:03:03 +01:00
parent 5530e0b946
commit c241ff4db3
4 changed files with 38 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
include/IRemote.h
View File

@ -7,9 +7,9 @@ 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]
//! @arg roll/pitch Tilt it around. [-1, 1]
//! @arg yaw Twist it. [-1, 1]
//! @arg elevator Lower or raise it. [-1, 1]
typedef struct {
float roll;
float pitch;
@ -19,9 +19,9 @@ public:
//! @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]
//! @arg vx Vel back/forward. [-1, 1]
//! @arg vy Vel left/right. [-1, 1]
//! @arg w Rotational speed. [-1, 1]
typedef struct {
float vx;
float vy;
@ -36,6 +36,8 @@ public:
virtual const Output& output() = 0;
virtual void init() = 0;
virtual void loop() = 0;
virtual void registerCallback(void (*callback)(const Output&)) = 0;

14
include/body.h
View File

@ -12,18 +12,18 @@ public:
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 rad to center this joint
void setTrim(Joint, float angle);
//! @brief position in deg
void setPos(Joint, double angle);
//! @brief position in rad
void setPos(Joint, float angle);
private:
struct JointInfo {
uint8_t index;
uint8_t trim;
double center;
double pos;
float trim;
float center;
float pos;
};
std::map<Joint, JointInfo> _joints;

32
src/body.cpp
View File

@ -7,6 +7,8 @@
#include <SPI.h>
#include <Wire.h>
#include <cmath>
// 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
@ -63,13 +65,15 @@ void checkForServoSleep() {
#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;
float modifiedMap(float x, float in_min, float in_max, float out_min, float out_max) {
float temp = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
temp = (int)(4 * temp + .5);
return (double)temp / 4;
return (float)temp / 4;
}
Body::Body() : legs() {
Body::Body()
: legs()
{
uint8_t numOfLegs = 6;
for (uint8_t i = 0; i < numOfLegs; ++i) {
legs.push_back(Leg(i, i + 6));
@ -85,24 +89,22 @@ void Body::healthCheck() {
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}}}) {
{Joint::Knee, {.index = knee, .trim = 0, .center = pi/4, .pos = 0}}}) {
}
void Leg::setTrim(Joint j, uint8_t angle) {
void Leg::setTrim(Joint j, float 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, float 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));
const float maxAngle = 1.43117; // both directions (visually observed)
auto angleToMicroPulse = [](double a) { return modifiedMap(a, -180.0, 180.0, USMIN, USMAX); };
servoDriver.writeMicroseconds(_joints[j].index, angleToMicroPulse(angle) + _joints[j].center);
#endif
angle = std::min(std::max(angle, -maxAngle), maxAngle);
servoDriver.writeMicroseconds(
_joints[j].index,
modifiedMap(angle, -maxAngle, maxAngle, USMIN, USMAX) + _joints[j].center);
_joints[j].pos = angle;
}

18
src/main.cpp
View File

@ -37,23 +37,17 @@ void connectWiFi() {
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);
body.legs[i].setPos(Leg::Hipp, double(o.attitude.roll) * pi / 2 );
body.legs[i].setPos(Leg::Knee, double(o.attitude.pitch) * pi / 2 );
// Serial.printf("CB: %f, %f\n", o.attitude.roll, o.attitude.pitch);
}
}
// Just a movement with one leg to show that we have booted
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++;
body.legs[1].setPos(Leg::Knee, pi / 2.0f);
sleep(1);
body.legs[1].setPos(Leg::Knee, 0.0f);
}
void checkConfig() {