# log 6 - 3 joystick axis xyz coordinate control

A project log for Quadruped learning project

This experiment is an xyz coordinate for foot position, which is moved by affecting the joysticks to move the coordinates at intervals related to the deflection of the joysticks. There are two joysticks, but one axis is ignored for this experiment. The leg initiates at "90 degrees extended and comfortably bent", and then you can move it around a 3d plane by pushing the joysticks, and when you do not push the joysticks the leg stays put. (there are several experiments that connect the foot position directly to joystick position, but in my experiment series this is done using potentiometers,, this experiment is specifically to allow the user to push up, and the foot moves up, etc.).

This experiment will hopefully move my programming toward a selectable foot which is placed by joysticks, with the body of the quadruped also moving to account for the new balance and shared stretch (not just one leg will move, all 4 legs will move to help the robot reach the new foot position and maintain balance and/yet share the extension,,,) . This leg experiment is only a very small piece of the required programming, but is a fundamental concept for my desired operator capability.

I had one error where I put a plus sign instead of an equal sign,, otherwise the bulk of the program was identical to the potentiometer experiment (which also saw me doing similar things). ,, otherwise the method of joystick input is fairly easy. Again in my code are some variables that are not used, but I will have to rework the program again, to fix the kinematics. This is because although it would be wonderful if my robot were just straight lines from and at each angle, this is not the case. The servos cause each joint to have both an angle,, and an offset (which is because the servo takes up space in my case, but in reality all robots will need to have little offsets figured in everywhere). I tried to program a kinematics equation the would put my foot exactly where assigned, rather than just moving according to my inputs, but I had quite a bit of trouble with stability until I erased all of these extra lines, and just went ahead with the bare minimum kinematics which does put the angles basically correct, but places the foot about 20mm away from the assigned xyz.

Below the code is a video demonstrating the build. Only just now I thought that I do not describe the wiring,, but I will have to leave it out for now, I don't make those kind of diagrams and would have to learn it.... The pins are, however, assigned in the code.

#include <Servo.h>

#include <math.h>

Servo hipRotate;

Servo hipBend;

Servo kneeBend;

float hipServo = 30;

float femur = 100;

float tibia = 90;

float coxaout = 40;

float coxaback = 10;

float L;

float A;

float B;

float G1;

float G;

float D;

float D2;

float x = 90;

float y = 0;

float z = 0;

void setup() {

// put your setup code here, to run once:

hipRotate.attach(9);

hipBend.attach(10);

kneeBend.attach(11);

Serial.begin(9600);

}

void loop() {

// put your main code here, to run repeatedly:

int xShift = map(joyx, 0, 1023, -10, 10);

int yShift = map(joyy, 0, 1023, -10, 10);

int zShift = map(joyz, 0, 1023, -10, 10);

if (xShift == -1 || xShift == 1) xShift = 0;

if (yShift == -1 || yShift == 1) yShift = 0;

if (zShift == -1 || zShift == 1) zShift = 0;

x = x + xShift; y = y + yShift; z = z + zShift;

if (x >= 180) x = 180; if (x <= 0) x = 0;

if (y >= 180) y = 180; if (y <= -180) y = -180;

if (z >= 160) z = 160; if (z<= -200) z = -200;

G1 = atan(x/y) ;

float G2 = degrees (G1);

if (G2 <= 0) {G2 = 180 + G2;}

D = sqrt ((x*x) + (y*y));

D2 = sqrt (((D+coxaback)*(D+coxaback))+(coxaout*coxaout));

A = acos(((femur*femur)+(tibia*tibia)-(L*L))/(2*femur*tibia));

Serial.print (x);

Serial.print(" ");

Serial.print(y);

Serial.print(" ");

Serial.println(z);

hipRotate.write(G2);

hipBend.write(degrees(B));

kneeBend.write(degrees(A));

}