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