Servos "crashing"

So I stumbled across another issue on my robot dog…
(Seriously, there needs to be another category for Proxy303’s robot dog)

If I am moving the motors quickly and to an extreme position, they will sometimes “crash” and possibly damage themselves or the robot.

Here’s a video (Converted to gif)
It’s pretty obvious what the problem is

Hnet.com-image

My code is available here:
SpotMain.zip (7.1 KB)

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 92.25      // 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
    }            
        
              

}

float filter(float prevValue, float currentValue, int filter) {  
  float lengthFiltered =  (prevValue + (currentValue * filter)) / (filter + 1);  
  return lengthFiltered;  
}

// ------- Function to convert degrees to servo microseconds -------

unsigned int deg2ms(unsigned int degrees) {
  return  1000 + (degrees * 150  + 13) / 27;
}






// ======= 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 = deg2ms(50);     // Hips
int servo2Offset = deg2ms(100);
int servo3Offset = 954;
int servo4Offset = deg2ms(155);

int servo5Offset = deg2ms(130);    // Shoulders
int servo6Offset = deg2ms(15);
int servo7Offset = deg2ms(165);
int servo8Offset = deg2ms(0);

int servo9Offset = deg2ms(5);      // Knees
int servo10Offset = deg2ms(105);
int servo11Offset = deg2ms(60);
int servo12Offset = deg2ms(90);





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

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;



void setup() {


  // 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

  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);
  delay(5000);

  /*
  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 >= 0) {  // 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);
    Ch5 = pulseIn(RCCh5, 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);

    Ch5val = map(Ch5, 990, 1990, 0, 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


    }
    roll = (ypr[1] * 180/M_PI) - 1.5;
    pitch = (ypr[2] * 180/M_PI) + 0.7;

    // convert sticks to measurements in mm or degrees

    z = map(posZ, -100, 100, 61.31, 229.011); // overall height of the robot | Higher number makes the leg longer
    z = constrain(z, 61.31, 229.011);

    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

    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
  }

}

So, what should I do?

More power.

I don’t think power is the issue, I’m running these off of a LiPo and have plenty of power. They are very strong too.
Plus, I’m already pushing the limits of these 7.2v servos on a 8v lipo.
I think it’s a software problem, given that it only happens at one point.

Think you answered your own query here…

IGNORE - it occurs to me you did this intentionally for testing purposes.
I need to read up on using the new forum, cannot find the strike through command, and obviously need to research how to post code since the old method does not work.

May be unrelated to the problem you are asking about, but you should listen to the compile here:

[code]
/home/test/Downloads/SpotMain/SpotMain.ino:482:38: warning: comparison of unsigned expression >= 0 is always true [-Wtype-limits]

if (currentMillis - previousMillis >= 0) { // If x milliseconds have passed
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~[/code]

The if statement is always true because unsigned integer arithmetic can never produce a negative answer.

(odd, code tags no longer appear to work)

This is purposeful, for testing purposes.

In the final version of the code, there will instead be a delay there.

The square bracket “tags” must each be on a separate line. Strikethrough uses “s” and “/s” in square brackets. There is a considerable amount of mangling taking place in the new forum.

Code blocks are used ``` before and after the text.

Like this

Or Like this

Hi,
Have you looked at James Bruton YouTube channel.
To do what you are doing he has some solutions, particularly Mini Robot Dog.

Tom… :grinning: :+1: :coffee: :australia:

Actually, I’m using his code, so yes I’ve seen this.

Markdown uses two tildes to enclose stricken text.

~ ~ strike through tildes ~ ~

strike through tildes

[ s]strike through brackets [ /s]

strike through brackets

I put spaces in the lines that didn’t get stricken to show like ~~

We all learning a bit of the old Markdown mark up language.

a7

So I think I may have found the problem.
It seems like my degrees-to-microseconds code caused a problem with the servos, because when I calculated the degrees to microseconds myself instead of using the function, it worked perfectly.

That looks correct to me. What wrong answers were you getting?

Using the servo library and not limiting servo travel I get 11.11us per degree.

Are you using the MG-995 Servo Motors?

MG-996r, actually.

I’m not entirely sure, but the code worked fine as soon as I removed this.

And how about the supply current and voltages to the servo. Is it sufficiently given. I have once faced this issue due to excessive supply voltage

The start/stall current of the MG996R is 2.5 Amperes. To avoid erratic behavior, the servo power supply must be able to supply more than 2.5A * (number of servos), and the power must be delivered through solid, preferably soldered connections, not a breadboard.

My servos are rated for 4.8 to 7.2 volts. I am running them off of a 2S LiPo (7.4v nominal), with no issues except that the motors just get a little warm.
There’s plenty of current.