Skip to content

Commit

Permalink
Merge branch 'fixing-generalization'
Browse files Browse the repository at this point in the history
  • Loading branch information
@seanboe
seanboe committed Jun 13, 2021
2 parents 569b4c2 + 5b2615c commit bad85c4
Show file tree
Hide file tree
Showing 3 changed files with 93 additions and 162 deletions.
49 changes: 12 additions & 37 deletions Config.h
View file
Original file line number Diff line number Diff line change
@@ -1,39 +1,30 @@
#ifndef _CONFIG_
#define _CONFIG_

// degrees to microseconds sale factor (determined experimentally, MUST be the same for every motor)
#define DEGREES_TO_MICROS 7.5

// Maximum motor speed; millis per 180 degrees factor; NOT DEGREES PER MILLIS I.E. SPEED (determined experimentally) this is 0.6 sec / 180 degrees (actual value is 0.52 sec)
// Maximum motor speed; milliseconds per 180 degrees factor; NOT DEGREES PER MILLISECONDS I.E. SPEED (determined experimentally) this is 0.6 sec / 180 degrees (actual value is 0.52 sec)
#define MAX_SPEED_INVERSE 3.5
// #define MAX_SPEED_INVERSE 25

typedef enum {
DEGREES, MILLIS
} unitType;
// This is used to parse which motors are for which leg from the list of motors.
#define MOTORS_PER_LEG 3

// Do NOT change this! I'm only using this for documentation. This library is for QUADRUPEDS and NOTHING else!
#define ROBOT_LEG_COUNT 4

// leg numbering
#define LEG_1 1
#define LEG_2 2
#define LEG_3 3
#define LEG_4 4


// motor enums
typedef enum {
M1, M2, M3
} motorID;

LEG_1 = 1, LEG_2, LEG_3, LEG_4
} LegID;

// default axis lengths (the 'safe' position for all the motors)
#define DEFAULT_X 0
#define DEFAULT_Y 45 // This is the same as LIMB_1; I want the foot to be directly under the shoulder ie straight, not under the bearing
#define DEFAULT_Z 177 // This is the foot-shoulder length when the leg makes a 45-45-90 triangle
typedef enum {
M1 = 1, M2, M3
} MotorID;


//********* robot hardware constraints **********

// limb length in millimeters
// The library assumes that your robot is symmetric.
#define LIMB_1 45
#define LIMB_2 125
#define LIMB_3 125
Expand All @@ -43,20 +34,4 @@ typedef enum {
#define SHOULDER_FOOT_MAX 230
#define SHOULDER_FOOT_MIN 100


//********* Constants for legs 2 & 3 **********************
// motor angular offsets (0 position is different than what is assumed for triangle calcs)
#define M1_OFFSET 90
#define M2_OFFSET 90
#define M3_OFFSET 90


// motor angular limits (degrees)
#define M1_MAX 120
#define M1_MIN 45
#define M2_MAX 270
#define M2_MIN 0
#define M3_MAX 130 //130
#define M3_MIN 45

#endif
162 changes: 56 additions & 106 deletions Kinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,57 +5,57 @@

/*!
* @param legID Leg number. Numbering follows the quadrants of a unit circle.
* @param inputX startup x-axis coordinate
* @param inputY startup y-axis coordinate
* @param inputZ startup z-axis coordinate
* @param motor1CalibOffset Calibration offset for the first motor
* @param motor2CalibOffset Calibration offset for the second motor
* @param motor3CalibOffset Calibration offset for the third motor
*/
Kinematics::Kinematics(uint8_t legID, int16_t inputX, int16_t inputY, int16_t inputZ, uint16_t motor1CalibOffset, uint16_t motor2CalibOffset, uint16_t motor3CalibOffset) {
Kinematics::Kinematics(LegID legID) {
_legID = legID;
};


// This solves for the motor angles in degrees and micros
solveFootPosition(inputX, inputY, inputZ, &motor1.angleDegrees, &motor2.angleDegrees, &motor3.angleDegrees);
// *****************Private Functions*****************

motor1.angleMicros = _degreesToMicros(motor1.angleDegrees, motor1CalibOffset);
motor1.angleDegrees = motor1.dynamicDegrees;
motor1.dynamicMicros = _degreesToMicros(motor1.dynamicDegrees, motor1CalibOffset);
motor1.previousDegrees = 360; // 360 is a magic number. It just must be different than the start positions so that a call to updateDynamicPositions() works
motor1.calibOffset = motor1CalibOffset;
/*!
* @brief Returns the index of a motor in the motor list given
* the leg it is in and the motor number of the leg
* you want.
* @param leg The leg that the motor is in.
* @param motor The motor in the leg you are trying to access
* @return The index of the motor
*/
uint16_t Kinematics::_indexOfMotor(LegID leg, MotorID motor) {
return ((leg - 1) * MOTORS_PER_LEG + motor) - 1;
};

motor2.angleMicros = _degreesToMicros(motor2.angleDegrees, motor2CalibOffset);
motor2.angleDegrees = motor2.dynamicDegrees;
motor2.dynamicMicros = _degreesToMicros(motor2.dynamicDegrees, motor2CalibOffset);
motor2.previousDegrees = 360; // 360 is a magic number. It just must be different than the start positions so that a call to updateDynamicPositions() works
motor2.calibOffset = motor2CalibOffset;

motor3.angleMicros = _degreesToMicros(motor3.angleDegrees, motor3CalibOffset);
motor3.angleDegrees = motor3.dynamicDegrees;
motor3.dynamicMicros = _degreesToMicros(motor3.dynamicDegrees, motor3CalibOffset);
motor3.previousDegrees = 360; // 360 is a magic number. It just must be different than the start positions so that a call to updateDynamicPositions() works
motor3.calibOffset = motor3CalibOffset;
/*!
* @brief Initializes the motor angles given the default position. Also does setup
* to integrate the external array of motors in the class.
* @param inputX startup x-axis coordinate
* @param inputY startup y-axis coordinate
* @param inputZ startup z-axis coordinate
* @param legMotors array of Motor variables for each motor
*/
void Kinematics::init(int16_t inputX, int16_t inputY, int16_t inputZ, Motor legMotors[]) {
_motors = legMotors;

dynamicX.go(inputX);
dynamicY.go(inputY);
dynamicZ.go(inputZ);
// Solve for the initial foot position
solveFootPosition(inputX, inputY, inputZ, &_motors[_indexOfMotor(_legID, M1)].angleDegrees, &_motors[_indexOfMotor(_legID, M2)].angleDegrees, &_motors[_indexOfMotor(_legID, M3)].angleDegrees);

};
// Motor 1
_motors[_indexOfMotor(_legID, M1)].dynamicDegrees = _motors[_indexOfMotor(_legID, M1)].angleDegrees;
_motors[_indexOfMotor(_legID, M1)].previousDegrees = 360; // 360 just needs to an angle that the motor can't be at... the motors can never achieve 360!

// Motor 2
_motors[_indexOfMotor(_legID, M2)].dynamicDegrees = _motors[_indexOfMotor(_legID, M2)].angleDegrees;
_motors[_indexOfMotor(_legID, M2)].previousDegrees = 360; // 360 just needs to an angle that the motor can't be at... the motors can never achieve 360!

// *****************Private Functions*****************
// Motor 3
_motors[_indexOfMotor(_legID, M3)].dynamicDegrees = _motors[_indexOfMotor(_legID, M3)].angleDegrees;
_motors[_indexOfMotor(_legID, M3)].previousDegrees = 360; // 360 just needs to an angle that the motor can't be at... the motors can never achieve 360!

/*!
* @brief Converts degrees to microseconds for the motors.
* @param inputDegrees Degrees to be converted to microseconds
* @param calibOffset Each motor's calibration offset
* @return The Microseconds converted from inputDegrees
*/
uint16_t Kinematics::_degreesToMicros(uint8_t inputDegrees, uint8_t calibOffset) {
int microsecondsInput = ((DEGREES_TO_MICROS * inputDegrees) + 500 + calibOffset); // 500 is a "magic number" of micros for the motors; before that they do nothing
return microsecondsInput;
};
dynamicX.go(inputX);
dynamicY.go(inputY);
dynamicZ.go(inputZ);
}


// *****************Public Functions*****************
Expand All @@ -68,19 +68,22 @@ uint16_t Kinematics::_degreesToMicros(uint8_t inputDegrees, uint8_t calibOffset)
*/
void Kinematics::setFootEndpoint(int16_t inputX, int16_t inputY, int16_t inputZ) {

solveFootPosition(inputX, inputY, inputZ, &motor1.angleDegrees, &motor2.angleDegrees, &motor3.angleDegrees);
solveFootPosition(inputX, inputY, inputZ, &_motors[_indexOfMotor(_legID, M1)].angleDegrees, &_motors[_indexOfMotor(_legID, M2)].angleDegrees, &_motors[_indexOfMotor(_legID, M3)].angleDegrees);

uint16_t motor1AngleDelta = abs(motor1.angleDegrees - motor1.previousDegrees);
uint16_t motor2AngleDelta = abs(motor2.angleDegrees - motor2.previousDegrees);
uint16_t motor3AngleDelta = abs(motor3.angleDegrees - motor3.previousDegrees);
uint16_t motor1AngleDelta = abs(_motors[_indexOfMotor(_legID, M1)].angleDegrees - _motors[_indexOfMotor(_legID, M1)].previousDegrees);
uint16_t motor2AngleDelta = abs(_motors[_indexOfMotor(_legID, M2)].angleDegrees - _motors[_indexOfMotor(_legID, M2)].previousDegrees);
uint16_t motor3AngleDelta = abs(_motors[_indexOfMotor(_legID, M3)].angleDegrees - _motors[_indexOfMotor(_legID, M3)].previousDegrees);
uint16_t demandTime = lrint(MAX_SPEED_INVERSE * max(max(motor1AngleDelta, motor2AngleDelta), motor3AngleDelta));


// determine whether motor angles have been updated i.e. new end angle, and update final positions accordingly
if ((motor1.previousDegrees != motor1.angleDegrees) || (motor2.previousDegrees != motor2.angleDegrees) || (motor3.previousDegrees != motor3.angleDegrees)) {
motor1.previousDegrees = motor1.angleDegrees;
motor2.previousDegrees = motor2.angleDegrees;
motor3.previousDegrees = motor3.angleDegrees;
if ((_motors[_indexOfMotor(_legID, M1)].previousDegrees != _motors[_indexOfMotor(_legID, M1)].angleDegrees)
|| (_motors[_indexOfMotor(_legID, M2)].previousDegrees != _motors[_indexOfMotor(_legID, M2)].angleDegrees)
|| (_motors[_indexOfMotor(_legID, M3)].previousDegrees != _motors[_indexOfMotor(_legID, M3)].angleDegrees)) {

_motors[_indexOfMotor(_legID, M1)].previousDegrees = _motors[_indexOfMotor(_legID, M1)].angleDegrees;
_motors[_indexOfMotor(_legID, M2)].previousDegrees = _motors[_indexOfMotor(_legID, M2)].angleDegrees;
_motors[_indexOfMotor(_legID, M3)].previousDegrees = _motors[_indexOfMotor(_legID, M3)].angleDegrees;

dynamicX.go(inputX, demandTime, LINEAR, ONCEFORWARD);
dynamicY.go(inputY, demandTime, LINEAR, ONCEFORWARD);
Expand All @@ -94,10 +97,8 @@ void Kinematics::setFootEndpoint(int16_t inputX, int16_t inputY, int16_t inputZ)
*/
void Kinematics::updateDynamicFootPosition() {

solveFootPosition(dynamicX.update(), dynamicY.update(), dynamicZ.update(), &motor1.dynamicDegrees, &motor2.dynamicDegrees, &motor3.dynamicDegrees);
motor1.dynamicMicros = _degreesToMicros(motor1.dynamicDegrees, motor1.calibOffset);
motor2.dynamicMicros = _degreesToMicros(motor2.dynamicDegrees, motor2.calibOffset);
motor3.dynamicMicros = _degreesToMicros(motor3.dynamicDegrees, motor3.calibOffset);
solveFootPosition(dynamicX.update(), dynamicY.update(), dynamicZ.update(), &_motors[_indexOfMotor(_legID, M1)].dynamicDegrees, &_motors[_indexOfMotor(_legID, M2)].dynamicDegrees, &_motors[_indexOfMotor(_legID, M3)].dynamicDegrees);

}

/*!
Expand Down Expand Up @@ -172,6 +173,9 @@ void Kinematics::solveYMove(int16_t inputY, int16_t inputZ, float *demandAngle1,
*demandAngle1 += (float)abs((float)90 - (alpha - theta)); // since both triangles (refer to drawings) have the same hypotenuse, alpha > theta for all inputY
}


// NEGATIVE signifies a direction: the foot is moving TOWARDS THE ROBOT
// POSITIVE signifies AWAY FROM ROBOT
if (inputY < LIMB_1)
*demandAngle1 *= -1;

Expand All @@ -188,7 +192,7 @@ void Kinematics::solveYMove(int16_t inputY, int16_t inputZ, float *demandAngle1,
* @param motor2AngleP The motor 2 angle output
* @param motor3AngleP The motor 3 angle output
*/
void Kinematics::solveFootPosition(int16_t inputX, int16_t inputY, int16_t inputZ, uint16_t *motor1AngleP, uint16_t *motor2AngleP, uint16_t *motor3AngleP) {
void Kinematics::solveFootPosition(int16_t inputX, int16_t inputY, int16_t inputZ, int16_t *motor1AngleP, int16_t *motor2AngleP, int16_t *motor3AngleP) {
float demandAngle1 = 0;
float demandAngle2 = 0;
float demandAngle3 = 0;
Expand All @@ -207,15 +211,6 @@ void Kinematics::solveFootPosition(int16_t inputX, int16_t inputY, int16_t input
demandAngle2 = lrint(demandAngle2);
demandAngle3 = lrint(demandAngle3);

// Calculate final demand angles suited to motors by applying necessary offsets
demandAngle1 += M1_OFFSET;
demandAngle2 += M2_OFFSET;
demandAngle3 = (M3_OFFSET - demandAngle3) + M3_OFFSET;

// Constrain motor angles
demandAngle1 = _applyConstraints(1, demandAngle1);
demandAngle2 = _applyConstraints(2, demandAngle2);
demandAngle3 = _applyConstraints(3, demandAngle3);

// Set live motor angles to the newly calculated ones

Expand All @@ -227,49 +222,4 @@ void Kinematics::solveFootPosition(int16_t inputX, int16_t inputY, int16_t input

// motor 3:
*motor3AngleP = demandAngle3; // In degrees!
};


/*!
* @param motor Motor that the angle should be constrained for
* @param demandAngle Angle to be constrained
* @returns Constrained angle
*/
float Kinematics::_applyConstraints(uint8_t motor, float demandAngle) {
if (motor == 1) {
if (demandAngle > M1_MAX){
return M1_MAX;
}
else if (demandAngle < M1_MIN){
return M1_MIN;
}
else
return demandAngle;
}
else if (motor == 2) {
if (demandAngle > M2_MAX){
return M2_MAX;
}
else if (demandAngle < M2_MIN){
return M2_MIN;
}
else
return demandAngle;
}
else if (motor == 3) {
if (demandAngle > M3_MAX) {
return M3_MAX;
}
else if (demandAngle < M3_MIN) {
return M3_MIN;
}
else
return demandAngle;
}
else {
if (Serial)
Serial.println("Motor argument in _apply constraints wrong, so constraints can't be applied! Terminating program."); //need a better way to report error
while(1); // terminate the program to avoid unpredictable movement (which could break stuff)
return demandAngle;
}
};
44 changes: 25 additions & 19 deletions Kinematics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
// Copyright (C) 2021 Sean Boerhout



#ifndef _KINEMATICS_
#define _KINEMATICS_

Expand All @@ -14,37 +13,44 @@
#include <Ramp.h>

typedef struct {
uint16_t angleDegrees;
uint16_t previousDegrees; // previous degrees since last call to updateDynamicPositions()
uint16_t angleMicros;
uint16_t dynamicDegrees;
uint16_t dynamicMicros;
uint16_t calibOffset;
} motor;
uint8_t controlPin;

// Angle/calculation stuff
int16_t angleDegrees;
int16_t previousDegrees; // previous degrees since last call to updateDynamicPositions()
int16_t dynamicDegrees;

// Calibration
uint16_t calibOffset; // This is an offset for calibration (to keep the motor accurate)
uint16_t maxPos;
uint16_t minPos;
uint16_t applicationOffset; // This is an offset for putting the calculated angles in contex.
// It is likely that the zero positions of the motors isn't where
// calculations assumes it to be, so you need an offset to make
// sure that the angle is correct relative to the motor's zero.
} Motor;


class Kinematics {

private:

uint8_t _legID;

uint16_t _degreesToMicros(uint8_t inputDegrees, uint8_t calibOffset);

float _applyConstraints(uint8_t motor, float demandAngle);
LegID _legID;

uint16_t _indexOfMotor(LegID leg, MotorID motor);

rampInt dynamicX;
rampInt dynamicY;
rampInt dynamicZ;

public:
Motor * _motors; // list of ALL robot motors

motor motor1;
motor motor2;
motor motor3;
public:

// Kinematics(uint8_t legID, uint16_t motor1CalibOffset, uint16_t motor1StartPos, uint16_t motor2CalibOffset, uint16_t motor2StartPos, uint16_t motor3CalibOffset, uint16_t motor3StartPos);
Kinematics(uint8_t legID, int16_t inputX, int16_t inputY, int16_t inputZ, uint16_t motor1CalibOffset, uint16_t motor2CalibOffset, uint16_t motor3CalibOffset);
Kinematics(LegID legID);

void init(int16_t inputX, int16_t inputY, int16_t inputZ, Motor legMotors[]);

// calculates all relevant motor angles (the angles of motors 2 & 3) to achieve vertical translation
void solveFtShldrLength(float demandFtShldr, float *demandAngle2, float *demandAngle3);
Expand All @@ -56,7 +62,7 @@ class Kinematics {
void solveYMove(int16_t inputY, int16_t inputZ, float *demandAngle1, float *yPlaneZOutput);

// general kinematics function; uses all positioning functions to place foot in 3d space
void solveFootPosition(int16_t inputX, int16_t inputY, int16_t inputZ, uint16_t *motor1AngleP, uint16_t *motor2AngleP, uint16_t *motor3AngleP);
void solveFootPosition(int16_t inputX, int16_t inputY, int16_t inputZ, int16_t *motor1AngleP, int16_t *motor2AngleP, int16_t *motor3AngleP);

// This sets the foot endpoints for when you are updating the foot position dynamically. (Interpolating Foot position, not angle)
void setFootEndpoint(int16_t inputX, int16_t inputY, int16_t inputZ);
Expand Down

0 comments on commit bad85c4

Please sign in to comment.