I don't have the ability to solder, so instead I braided the wires together and then crushed them together with a screw terminal.
Update - The issue seems to happen when the value of Ch3 is highest, and Ch1 and Ch2 are fluctuating near their extreme values.
My latest code is posted here:
// ------- Function to convert degrees to servo microseconds -------
unsigned int deg2ms(unsigned int degrees) {
return 1000 + (degrees * 150 + 13) / 27;
}
void clik() {
tone(24, 3000, 1);
}
void beep(int freq, int dur) {
tone(24, freq, dur);
delay(dur);
noTone(24);
}
// ======= CONFIGURATION =======
// This robot has multiple functions that you can configure in this section
int endStopPosRotX = 100; // X axis rotation upper limit
int endStopNegRotX = -100; // X axis rotation lower limit
int endStopPosRotY = 100; // Y axis rotation upper limit
int endStopNegRotY = -100; // Y axis rotation lower limit
int endStopPosRotZ = 100; // Z axis rotation upper limit
int endStopNegRotZ = -100; // Z axis rotation lower limit
int endStopPosTrsX = 100; // X axis translation upper limit
int endStopNegTrsX = -100; // X axis translation lower limit
int endStopPosTrsY = 100; // Y axis translation upper limit
int endStopNegTrsY = -100; // Y axis translation lower limit
int endStopPosTrsZ = 100; // Z axis translation upper limit
int endStopNegTrsZ = -100; // Z axis translation lower limit
// ------- Hardware -------
// --- RC receiver pins
int RCCh1 = 30;
int RCCh2 = 31;
int RCCh3 = 32;
int RCCh4 = 33;
int RCCh5 = 34;
int RCCh6 = 35;
// --- Multipliers for gear ratios
float multiplierKneesRight = 1;
float multiplierShouldersRight = 1;
float multiplierHipsRight = 1;
float multiplierKneesLeft = 1;
float multiplierShouldersLeft = 1;
float multiplierHipsLeft = 1;
// ------- Software -------
// --- Values for filtering
int filterKneesRight = 15;
int filterShouldersRight = 15;
int filterHipsRight = 25;
int filterKneesLeft = 15;
int filterShouldersLeft = 15;
int filterHipsLeft = 25;
// Servo offsets for default position - knees at 45', hips at 0', shoulders at 45'
int servo1Offset = 1200; // Hips
int servo2Offset = 1490;
int servo3Offset = 954;
int servo4Offset = 1950;
int servo5Offset = 1850; // Shoulders
int servo6Offset = 900;
int servo7Offset = 2150;
int servo8Offset = 650;
int servo9Offset = 700; // Knees
int servo10Offset = 2000;
int servo11Offset = 1000;
int servo12Offset = 2100;
// --- Include libraries
#include <Servo.h>
#include <PID_v1.h> //PID loop from http://playground.arduino.cc/Code/PIDLibrary
// --- Containers
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// --- Define variables
double Pk1 = 10;
double Ik1 = 30;
double Dk1 = 0.1;
float posX = 0;
float posY = 0;
float posZ = 0;
float rotYaw = 0;
float rotPitch = 0;
float rotRoll = 0;
// --- PID setup
double Setpoint1, Input1, Output1; // PID variables
PID PID1(&Input1, &Output1, &Setpoint1, Pk1, Ik1 , Dk1, DIRECT); // PID Setup
double Pk2 = 5;
double Ik2 = 0;
double Dk2 = 0;
double Setpoint2, Input2, Output2; // PID variables
PID PID2(&Input2, &Output2, &Setpoint2, Pk2, Ik2 , Dk2, DIRECT); // PID Setup
double Pk3 = 5;
double Ik3 = 0;
double Dk3 = 0;
double Setpoint3, Input3, Output3; // PID variables
PID PID3(&Input3, &Output3, &Setpoint3, Pk3, Ik3 , Dk3, DIRECT); // PID Setup
const byte addresses[][6] = {"00001", "00002"};
// --- More variables
float pitch;
float roll;
float rollFiltered;
// --- Arduino doesn't know Pi, apparently, so it's defined here
#define PI 3.1415926535897932384626433832795
// --- More variables
int IMUdataReady = 0;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
// --- Initialize servos
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
Servo servo7;
Servo servo8;
Servo servo9;
Servo servo10;
Servo servo11;
Servo servo12;
// --- Timers
unsigned long currentMillis;
unsigned long previousMillis;
unsigned long previousWalkMillis;
unsigned long previousSafetyMillis;
// --- Even more variables
// global joint threshold value for hall effects
int threshholdGlobal = 5;
int phase = 1;
float servo1Pos; // initial servo values
float servo2Pos;
float servo3Pos;
float servo4Pos;
float servo5Pos;
float servo6Pos;
float servo7Pos;
float servo8Pos;
float servo9Pos;
float servo10Pos;
float servo11Pos;
float servo12Pos;
float servo1PosTrack; // ongoing tracking values
float servo2PosTrack;
float servo3PosTrack;
float servo4PosTrack;
float servo5PosTrack;
float servo6PosTrack;
float servo7PosTrack;
float servo8PosTrack;
float servo9PosTrack;
float servo10PosTrack;
float servo11PosTrack;
float servo12PosTrack;
float servo1PosFiltered; // filtered values
float servo2PosFiltered;
float servo3PosFiltered;
float servo4PosFiltered;
float servo5PosFiltered;
float servo6PosFiltered;
float servo7PosFiltered;
float servo8PosFiltered;
float servo9PosFiltered;
float servo10PosFiltered;
float servo11PosFiltered;
float servo12PosFiltered;
float xAxisValue = 0;
float yAxisValue = 0;
float zAxisValue = 0;
float elevationValue = 0;
float speedValue = 0;
int mode = 1;
int xAxisPwmValue = 0;
int yAxisPwmValue = 0;
int zAxisPwmValue = 0;
int elevationPwmValue = 0;
int speedPwmValue = 0;
int modePwmValue = 0;
// --- Walk test position variables
int initStart;
int state;
float rate;
int targetLeg1x;
int targetLeg1z;
int prevLeg1x;
int prevLeg1z;
float currentLeg1x;
float currentLeg1z;
float stepDiffLeg1x;
float stepDiffLeg1z;
int targetLeg2x;
int targetLeg2z;
int prevLeg2x;
int prevLeg2z;
float currentLeg2x;
float currentLeg2z;
float stepDiffLeg2x;
float stepDiffLeg2z;
int targetLeg3x;
int targetLeg3z;
int prevLeg3x;
int prevLeg3z;
float currentLeg3x;
float currentLeg3z;
float stepDiffLeg3x;
float stepDiffLeg3z;
int targetLeg4x;
int targetLeg4z;
int prevLeg4x;
int prevLeg4z;
float currentLeg4x;
float currentLeg4z;
float stepDiffLeg4x;
float stepDiffLeg4z;
int targetLeg1y;
int prevLeg1y;
float currentLeg1y;
float stepDiffLeg1y;
int walkXPos1;
int walkXPos2;
int walkXPos3;
int walkXPos4;
int walkXPos5;
int walkXPos6;
int walkXPos7;
int walkXPos8;
int walkYPos1;
int walkYPos2;
int walkYPos3;
int walkYPos4;
int walkYPos5;
int walkYPos6;
int walkYPos7;
int walkYPos8;
int walkZPos1;
int walkZPos2;
int walkZPos3;
int walkZPos4;
int walkZPos5;
int walkZPos6;
int walkZPos7;
int walkZPos8;
// Function to filter motions
float filter(float prevValue, float currentValue, int filter) {
float lengthFiltered = (prevValue + (currentValue * filter)) / (filter + 1);
return lengthFiltered;
}
void setup() {
clik();
// PID stuff
PID1.SetMode(AUTOMATIC);
PID1.SetOutputLimits(-50, 50);
PID1.SetSampleTime(10);
PID2.SetMode(AUTOMATIC);
PID2.SetOutputLimits(-50, 50);
PID2.SetSampleTime(10);
PID3.SetMode(AUTOMATIC);
PID3.SetOutputLimits(-50, 50);
PID3.SetSampleTime(10);
Serial.begin(9600);
// Write default positions
beep(1500,1500);
delay(500);
beep(1500,500);
delay(500);
servo1.attach(2); // hips
servo2.attach(3);
servo3.attach(4);
servo4.attach(5);
servo5.attach(6);
servo6.attach(7);
servo7.attach(8);
servo8.attach(9);
servo9.attach(10);
servo10.attach(11);
servo11.attach(12);
servo12.attach(13);
servo1.writeMicroseconds(servo1Offset);
servo2.writeMicroseconds(servo2Offset);
servo3.writeMicroseconds(servo3Offset);
servo4.writeMicroseconds(servo4Offset);
servo5.writeMicroseconds(servo5Offset);
servo6.writeMicroseconds(servo6Offset);
servo7.writeMicroseconds(servo7Offset);
servo8.writeMicroseconds(servo8Offset);
servo9.writeMicroseconds(servo9Offset);
servo10.writeMicroseconds(servo10Offset);
servo11.writeMicroseconds(servo11Offset);
servo12.writeMicroseconds(servo12Offset);
beep(250,250);
beep(500,250);
beep(1000,250);
beep(1500,250);
delay(1000);
beep(2000,50);
int i;
while (pulseIn(RCCh1,HIGH) <= 500) {
delay(250);
beep(2000,50);
if (i > 20) {
while (true) {
beep(300,150);
beep(230,550);
delay(300);
}
}
i += 1;
}
delay(500);
/*
kinematics(1,0,20,0,100,0,0,0);
kinematics(2,0,20,0,100,0,0,0);
kinematics(3,0,20,0,100,0,0,0);
kinematics(4,0,20,0,100,0,0,0);
delay(1000);
kinematics(1,0,-20,0,100,0,0,0);
kinematics(2,0,-20,0,100,0,0,0);
kinematics(3,0,-20,0,100,0,0,0);
kinematics(4,0,-20,0,100,0,0,0);
delay(1000);
kinematics(1,0,0,20,100,0,0,0);
kinematics(2,0,0,20,100,0,0,0);
kinematics(3,0,0,20,100,0,0,0);
kinematics(4,0,0,20,100,0,0,0);
delay(1000);
kinematics(1,0,0,-20,100,0,0,0);
kinematics(2,0,0,-20,100,0,0,0);
kinematics(3,0,0,-20,100,0,0,0);
kinematics(4,0,0,-20,100,0,0,0);
delay(1000);
kinematics(1,0,0,0,50,0,0,0);
kinematics(2,0,0,0,50,0,0,0);
kinematics(3,0,0,0,50,0,0,0);
kinematics(4,0,0,0,50,0,0,0);
delay(1000);
kinematics(1,0,0,0,150,0,0,0);
kinematics(2,0,0,0,150,0,0,0);
kinematics(3,0,0,0,150,0,0,0);
kinematics(4,0,0,0,150,0,0,0);
delay(1000);
kinematics(1,0,0,0,100,20,0,0);
kinematics(2,0,0,0,100,20,0,0);
kinematics(3,0,0,0,100,20,0,0);
kinematics(4,0,0,0,100,20,0,0);
delay(1000);
kinematics(1,0,0,0,100,-20,0,0);
kinematics(2,0,0,0,100,-20,0,0);
kinematics(3,0,0,0,100,-20,0,0);
kinematics(4,0,0,0,100,-20,0,0);
delay(1000);
kinematics(1,0,0,0,100,0,20,0);
kinematics(2,0,0,0,100,0,20,0);
kinematics(3,0,0,0,100,0,20,0);
kinematics(4,0,0,0,100,0,20,0);
delay(1000);
kinematics(1,0,0,0,100,0,-20,0);
kinematics(2,0,0,0,100,0,-20,0);
kinematics(3,0,0,0,100,0,-20,0);
kinematics(4,0,0,0,100,0,-20,0);
delay(1000);
kinematics(1,0,0,0,100,0,0,20);
kinematics(2,0,0,0,100,0,0,20);
kinematics(3,0,0,0,100,0,0,20);
kinematics(4,0,0,0,100,0,0,20);
delay(1000);
kinematics(1,0,0,0,100,0,0,-20);
kinematics(2,0,0,0,100,0,0,-20);
kinematics(3,0,0,0,100,0,0,-20);
kinematics(4,0,0,0,100,0,0,-20);
delay(1000);
kinematics(1,0,0,0,100,0,0,0);
kinematics(2,0,0,0,100,0,0,0);
kinematics(3,0,0,0,100,0,0,0);
kinematics(4,0,0,0,100,0,0,0);
*/
}
// --- So many variables
int x;
int y;
int z;
int r;
int p;
int yaw;
int Ch1;
int Ch2;
int Ch3;
int Ch4;
int Ch5;
int Ch6;
float Ch1val;
float Ch2val;
float Ch3val;
float Ch4val;
float Ch5val;
float Ch6val;
void loop() {
currentMillis = millis(); // Reset timer
if (currentMillis - previousMillis >= 10) { // If x milliseconds have passed
previousMillis = currentMillis; // Record the current time
// Read the values from the RC receiver
Ch1 = pulseIn(RCCh1, HIGH);
Ch2 = pulseIn(RCCh2, HIGH);
Ch3 = pulseIn(RCCh3, HIGH);
Ch4 = pulseIn(RCCh4, HIGH);
Ch6 = pulseIn(RCCh6, HIGH);
// Select which mode using the PWM switch.
if (Ch6 <= 1250) {
mode = 1; // Rotate mode
}
else if (Ch6 > 1250 and Ch6 < 1750) {
mode = 2; // Translate mode
}
else if (Ch6 >= 1750) {
mode = 3; // Walk mode
}
// The X Axis of the right joystick on the receiver has a drifting issue due to a defective potentiometer, this code compensates for that.
Ch1 = Ch1 - 1500;
Ch1val = map(Ch1, -550, 300, -100, 100);
if (Ch1val >= -10 and Ch1 <= 10) {
Ch1val = 0;
}
// Map the other PWM values to their respective places (PWM input is from roughly 990 to 1990. This maps each one to -100 to 100 or from 0 to 100)
Ch2val = map(Ch2, 990, 1990, -100, 100);
Ch3val = map(Ch3, 990, 1990, -100, 100);
Ch4val = map(Ch4, 990, 1990, -100, 100);
if (mode == 1) { // Rotation mode
rotYaw = map(Ch1val, -100, 100, endStopNegRotX, endStopPosRotX); // Map the horizontal right joystick to the X axis rotation
rotPitch = map(Ch2val, -100, 100, endStopNegRotY, endStopPosRotY); // Map the vertical right joystick to the Y axis rotation
rotRoll = map(Ch4val, -100, 100, endStopNegRotZ, endStopPosRotZ); // Map the
posZ = map(Ch3val, -100, 100, endStopNegTrsZ, endStopPosTrsZ);
} else if (mode == 2) { // Translation mode
posX = map(Ch2val, -100, 100, endStopNegRotX, endStopPosRotX);
posY = map(Ch1val, -100, 100, endStopNegRotY, endStopPosRotY);
posZ = map(Ch3val, -100, 100, endStopNegTrsZ, endStopPosTrsZ);
} else if (mode == 3) { // Walk mode
// To be added
posX = map(Ch2val, -100, 100, endStopNegRotX, endStopPosRotX);
posY = map(Ch1val, -100, 100, endStopNegRotY, endStopPosRotY);
rotYaw = map(Ch1val, -100, 100, endStopNegRotX, endStopPosRotX); // Map the horizontal right joystick to the X axis rotation
}
roll = (ypr[1] * 180 / M_PI) - 1.5;
pitch = (ypr[2] * 180 / M_PI) + 0.7;
if (mode == 1 or mode == 2) {
// convert sticks to measurements in mm or degrees
z = map(posZ, -100, 100, 15, 200); // overall height of the robot | Higher number makes the leg longer
z = constrain(z, 15, 200);
x = map(posX, -100, 100, -60, 60); // front/back | Higher number moves the foot forward
x = constrain(x, -60, 60);
y = map(posY, -100, 100, -60, 60); // side/side | Higher number moves the foot left
y = constrain(y, -60, 60);
r = map(rotRoll, -100, 100, -30, 30); // ROLL - covert to degrees of rotation
r = constrain(r, -20, 20);
p = (map(rotPitch, -100, 100, 30, -30)) - 1; // PITCH - covert to degrees of rotation (there is a weird offset on my joystick hence -1)
p = constrain(p, -20, 20);
yaw = map(rotYaw, -100, 100, -30, 30); // YAW - covert to degrees of rotation (there is a weird offset on my joystick hence -1)
yaw = constrain(yaw, -20, 20);
// print control data for debug
// send data to kinematic model function, compliance engine, and eventually write out to servos
} else if (mode == 3) {
//Walk mode, nothing here yet
}
}
kinematics (1, 0, x, y, z, r, p, yaw); // front left leg
kinematics (2, 0, x, y, z, r, p, yaw); // front right leg
kinematics (3, 0, x, y, z, r, p, yaw); // back left leg
kinematics (4, 0, x, y, z, r, p, yaw); // back right leg
}
And, my kinematics code is posted here:
void kinematics (int leg, int mode, float x, float y, float z, float roll, float pitch, float yaw) {
// *** TRANSLATION AXIS ***
// moving the foot sideways on the end plane
#define hipOffset 57.91
float lengthY;
float hipAngle1a;
float hipAngle1b;
float hipAngle1;
float hipAngle1Degrees;
float hipHyp;
// moving the foot forwards or backwardes in the side plane
float shoulderAngle2;
float shoulderAngle2Degrees;
float z2;
// side plane of individual leg only
#define shinLength 122.53
#define thighLength 108.45
float z3;
float shoulderAngle1;
float shoulderAngle1Degrees;
float shoulderAngle1a;
float shoulderAngle1b;
float shoulderAngle1c;
float shoulderAngle1d;
float kneeAngle;
float kneeAngleDegrees;
// *** ROTATION AXIS
// roll axis
#define bodyWidth 38.34 // half the distance from the middle of the body to the hip pivot
float legDiffRoll; // differnece in height for each leg
float bodyDiffRoll; // how much shorter the 'virtual body' gets
float footDisplacementRoll; // where the foot actually is
float footDisplacementAngleRoll; // smaller angle
float footWholeAngleRoll; // whole leg angle
float hipRollAngle; // angle for hip when roll axis is in use
float rollAngle; // angle in RADIANS that the body rolls
float zz1a; // hypotenuse of final triangle
float zz1; // new height for leg to pass onto the next bit of code
float yy1; // new position for leg to move sideways
// pitch axis
#define bodyLength 123.32 // distance from centre of body to shoulder pivot
float legDiffPitch; // differnece in height for each leg
float bodyDiffPitch; // how much shorter the 'virtual body' gets
float footDisplacementPitch; // where the foot actually is
float footDisplacementAnglePitch; // smaller angle
float footWholeAnglePitch; // whole leg angle
float shoulderPitchAngle; // angle for hip when roll axis is in use
float pitchAngle; // angle in RADIANS that the body rolls
float zz2a; // hypotenuse of final triangle
float zz2; // new height for the leg to pass onto the next bit of code
float xx1; // new position to move the leg fowwards/backwards
// yaw axis
float yawAngle; // angle in RADIANs for rotation in yaw
float existingAngle; // existing angle of leg from centre
float radius; // radius of leg from centre of robot based on x and y from sticks
float demandYaw; // demand yaw postion - existing yaw plus the stick yaw
float xx3; // new X coordinate based on demand angle
float yy3; // new Y coordinate based on demand angle
// ******************************************************************************************************
// ***************************** KINEMATIC MODEL CALCS START HERE ***************************************
// ******************************************************************************************************
// convert degrees to radians for the calcs
yawAngle = (PI/180) * yaw;
// put in offsets from robot's parameters so we can work out the radius of the foot from the robot's centre
if (leg == 1) { // front left leg
y = y - (bodyWidth+hipOffset);
x = x - bodyLength;
}
else if (leg == 2) { // front right leg
y = y + (bodyWidth+hipOffset);
x = x - bodyLength;
}
else if (leg == 3) { // back left leg
y = y - (bodyWidth+hipOffset);
x = x + bodyLength;
}
else if (leg == 4) { // back left leg
y = y + (bodyWidth+hipOffset);
x = x + bodyLength;
}
//calc existing angle of leg from cetre
existingAngle = atan(y/x);
// calc radius from centre
radius = y/sin(existingAngle);
//calc demand yaw angle
demandYaw = existingAngle + yawAngle;
// calc new X and Y based on demand yaw angle
xx3 = radius * cos(demandYaw); // calc new X and Y based on new yaw angle
yy3 = radius * sin(demandYaw);
// remove the offsets so we pivot around 0/0 x/y
if (leg == 1) { // front left leg
yy3 = yy3 + (bodyWidth+hipOffset);
xx3 = xx3 + bodyLength;
}
else if (leg == 2) { // front right leg
yy3 = yy3 - (bodyWidth+hipOffset);
xx3 = xx3 + bodyLength;
}
else if (leg == 3) { // back left leg
yy3 = yy3 + (bodyWidth+hipOffset);
xx3 = xx3 - bodyLength;
}
else if (leg == 4) { // back left leg
yy3 = yy3 - (bodyWidth+hipOffset);
xx3 = xx3 - bodyLength;
}
// *** PITCH AXIS ***
//turn around the pitch for front or back of the robot
if (leg == 1 || leg == 2) {
pitch = 0-pitch;
}
else if (leg == 3 || leg == 4) {
pitch = 0+pitch;
xx3 = xx3*-1; // switch over x for each end of the robot
}
// convert pitch to degrees
pitchAngle = (PI/180) * pitch;
//calc top triangle sides
legDiffPitch = sin(pitchAngle) * bodyLength;
bodyDiffPitch = cos(pitchAngle) * bodyLength;
// calc actual height from the ground for each side
legDiffPitch = z - legDiffPitch;
// calc foot displacement
footDisplacementPitch = ((bodyDiffPitch - bodyLength)*-1)+xx3;
//calc smaller displacement angle
footDisplacementAnglePitch = atan(footDisplacementPitch/legDiffPitch);
//calc distance from the ground at the displacement angle (the hypotenuse of the final triangle)
zz2a = legDiffPitch/cos(footDisplacementAnglePitch);
// calc the whole angle for the leg
footWholeAnglePitch = footDisplacementAnglePitch + pitchAngle;
//calc actual leg length - the new Z to pass on
zz2 = cos(footWholeAnglePitch) * zz2a;
//calc new Z to pass on
xx1 = sin(footWholeAnglePitch) * zz2a;
if (leg == 3 || leg == 4 ){ // switch back X for the back of the robot
xx1 = xx1 *-1;
}
// *** ROLL AXIS ***
//turn around roll angle for each side of the robot
if (leg == 1 || leg == 3) {
roll = 0+roll;
yy3 = yy3*-1;
}
else if (leg == 2 || leg == 4) {
roll = 0-roll;
}
// convert roll angle to radians
rollAngle = (PI/180) * roll; //covert degrees from the stick to radians
// calc the top triangle sides
legDiffRoll = sin(rollAngle) * bodyWidth;
bodyDiffRoll = cos(rollAngle) * bodyWidth;
// calc actual height from the ground for each side
legDiffRoll = zz2 - legDiffRoll;
// calc foot displacement
footDisplacementRoll = ((bodyDiffRoll - bodyWidth)*-1)-yy3;
//calc smaller displacement angle
footDisplacementAngleRoll = atan(footDisplacementRoll/legDiffRoll);
//calc distance from the ground at the displacement angle (the hypotenuse of the final triangle)
zz1a = legDiffRoll/cos(footDisplacementAngleRoll);
// calc the whole angle for the leg
footWholeAngleRoll = footDisplacementAngleRoll + rollAngle;
//calc actual leg length - the new Z to pass on
zz1 = cos(footWholeAngleRoll) * zz1a;
//calc new Y to pass on
yy1 = sin(footWholeAngleRoll) * zz1a;
// *** TRANSLATION AXIS ***
// calculate the hip joint and new leg length based on how far the robot moves sideways
hipAngle1 = atan(yy1/zz1);
hipAngle1Degrees = ((hipAngle1 * (180/PI))); // convert to degrees and take off rest position
z2 = zz1/cos(hipAngle1);
// ****************
// calculate the shoulder joint offset and new leg length based on now far the foot moves forward/backwards
shoulderAngle2 = atan(xx1/z2); // calc how much extra to add to the shoulder joint
shoulderAngle2Degrees = shoulderAngle2 * (180/PI);
z3 = z2/cos(shoulderAngle2); // calc new leg length to feed to the next bit of code below
// ****************
// calculate leg length based on shin/thigh length and knee and shoulder angle
shoulderAngle1a = sq(thighLength) + sq(z3) - sq(shinLength);
shoulderAngle1b = 2 * thighLength * z3;
shoulderAngle1c = shoulderAngle1a / shoulderAngle1b;
shoulderAngle1 = acos(shoulderAngle1c); // radians
kneeAngle = PI - (shoulderAngle1 *2); // radians
//calc degrees from angles
shoulderAngle1Degrees = shoulderAngle1 * (180/PI); // degrees
kneeAngleDegrees = kneeAngle * (180/PI); // degrees
// ******************************************************************************************************
// ***************** compliance / filtering / write out servo positoion below this point *****************
// ******************************************************************************************************
if (leg == 1) { // *front left leg*
// convert degrees to servo microSeconds
servo11PosTrack = (kneeAngleDegrees-90) * -25; // positive scaler
servo7PosTrack = ((shoulderAngle1Degrees-45) * 25) - (shoulderAngle2Degrees * 25) ;
servo3PosTrack = (hipAngle1Degrees * -25);
// no compliance mode
if (mode == 0) {
servo3Pos = servo3PosTrack; // front left hip
servo7Pos = servo7PosTrack; // front left shoulder
servo11Pos = servo11PosTrack; // front right knee
}
// filter motions
servo3PosFiltered = filter(servo3Pos, servo3PosFiltered, filterHipsLeft);
servo3PosFiltered = constrain(servo3PosFiltered,-900,900);
servo7PosFiltered = filter(servo7Pos, servo7PosFiltered, filterShouldersLeft);
servo7PosFiltered = constrain(servo7PosFiltered,-900,900);
servo11PosFiltered = filter(servo11Pos, servo11PosFiltered, filterKneesLeft);
servo11PosFiltered = constrain(servo11PosFiltered,-900,900);
// write out servo positions
servo3.writeMicroseconds(servo3Offset + servo3PosFiltered); // front left hip
servo7.writeMicroseconds(servo7Offset - servo7PosFiltered); // front left shoulder
servo11.writeMicroseconds(servo11Offset + servo11PosFiltered); // front right knee
}
else if (leg == 2) { // *front right leg*
// convert degrees to servo microSeconds
servo12PosTrack = (kneeAngleDegrees-90) * 25;
servo8PosTrack = ((shoulderAngle1Degrees-45) * -25) + (shoulderAngle2Degrees * 25);
servo4PosTrack = (hipAngle1Degrees * 25);
// no compliance mode
if (mode == 0) {
servo4Pos = servo4PosTrack; // front right hip
servo8Pos = servo8PosTrack; // front right shoulder
servo12Pos = servo12PosTrack; // front right knee
}
// filter motions
servo4PosFiltered = filter(servo4Pos, servo4PosFiltered, filterHipsRight);
servo4PosFiltered = constrain(servo4PosFiltered,-900,900);
servo8PosFiltered = filter(servo8Pos, servo8PosFiltered, filterShouldersRight);
servo8PosFiltered = constrain(servo8PosFiltered,-900,900);
servo12PosFiltered = filter(servo12Pos, servo12PosFiltered, filterKneesRight);
servo12PosFiltered = constrain(servo12PosFiltered,-900,900);
// write out servo potitions
servo4.writeMicroseconds(servo4Offset + servo4PosFiltered); // front right hip
servo8.writeMicroseconds(servo8Offset - servo8PosFiltered); // front right shoulder
servo12.writeMicroseconds(servo12Offset + servo12PosFiltered); // front right knee
}
else if (leg == 3) { // *back left leg*
// convert degrees to servo microSeconds
servo9PosTrack = (kneeAngleDegrees-90) * -25;
servo5PosTrack = ((shoulderAngle1Degrees-45) * 25) - (shoulderAngle2Degrees * 25);
servo1PosTrack = (hipAngle1Degrees * -25);
// no compliance mode
if (mode == 0) {
servo1Pos = servo1PosTrack; // back left hip
servo5Pos = servo5PosTrack; // back left shoulder
servo9Pos = servo9PosTrack; // back left knee
}
// filter motions
servo1PosFiltered = filter(servo1Pos, servo1PosFiltered, filterHipsLeft);
servo1PosFiltered = constrain(servo1PosFiltered,-900,900);
servo5PosFiltered = filter(servo5Pos, servo5PosFiltered, filterShouldersLeft);
servo5PosFiltered = constrain(servo5PosFiltered,-900,900);
servo9PosFiltered = filter(servo9Pos, servo9PosFiltered, filterKneesLeft);
servo9PosFiltered = constrain(servo9PosFiltered,-900,900);
// write out servo positions
servo1.writeMicroseconds(servo1Offset + servo1PosFiltered); // back left hip
servo5.writeMicroseconds(servo5Offset - servo5PosFiltered); // back left shoulder
servo9.writeMicroseconds(servo9Offset + servo9PosFiltered); // front left knee
}
else if (leg == 4) { // *back right leg*
// convert degrees to servo microSeconds
servo10PosTrack = (kneeAngleDegrees-90) * 25;
servo6PosTrack = ((shoulderAngle1Degrees-45) * -25) + (shoulderAngle2Degrees * 25);
servo2PosTrack = (hipAngle1Degrees * 25);
// no compliance mode
if (mode == 0) {
servo2Pos = servo2PosTrack; // back right hip
servo6Pos = servo6PosTrack; // back right shoulder
servo10Pos = servo10PosTrack; // back right knee
}
// filter motions
servo2PosFiltered = filter(servo2Pos, servo2PosFiltered, filterHipsRight);
servo2PosFiltered = constrain(servo2PosFiltered,-900,900);
servo6PosFiltered = filter(servo6Pos, servo6PosFiltered, filterShouldersRight);
servo6PosFiltered = constrain(servo6PosFiltered,-900,900);
servo10PosFiltered = filter(servo10Pos, servo10PosFiltered, filterKneesRight);
servo10PosFiltered = constrain(servo10PosFiltered,-900,900);
// write out servo positions
servo2.writeMicroseconds(servo2Offset + servo2PosFiltered); // back right hip
servo6.writeMicroseconds(servo6Offset - servo6PosFiltered); // back left shoulder
servo10.writeMicroseconds(servo10Offset + servo10PosFiltered); // front left knee
}
}
Tested solutions:
- Making the endstops lower - The servos crash less often, but still do it sometimes. I want as big a range of motion as possible
- Editing constraint values - Same result as above
- Changing the servo offsets - Sometimes works, depending on the offset
I don't see how this can work at all. You constrain movement to +/- 900 microseconds and have an offset for each servo. The default limits on the Servo object are 544 to 2400 (a range of 1856). That will work IF your offsets are between 1444 and 1500... but they aren't. Your offsets run from 650 to 2150 giving you values from -250(!?!) on servo7 to +3050 on servo8.
Those will get truncated to the 544 to 2400 range so it won't harm the sevos (IF they support that range). It will, however, limit the range of motion of the servo.
Rather than mounting the servos and then finding an offset that moves them to the middle of the desired range, you should move the servos to 1472 (middle of the available range) and then mount the servos with the arms at the desired midpoint. REMEMBER: If any of your offsets is outside the 1444 to 1500 range you will hit the limits on the servo library (544 to 2400) before you hit the ends of your +/-900 range.
Thanks, I'll try this.
This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.