Today I made to working order (very simple, no stability engineering) a joystick controlled x,y directed foot for a 2d 2 servo leg. (as pictured previously).

I will post an 8 minute video below the code. I do have some trouble. If I move the foot outside of the constraints, it sends both servos to 0 degrees, however if I move the x,y back within the constraints the foot returns to where it got lost, and drives as directed. (mostly). There are still some issues with position,, if the hip servo can't move past 180, the tibia continues to swing upward, placing the foot further away than the directed x,y coordinate. I don't understand constrain,, I was under the impression if I constrained x and y, then they should remain at their limitations, instead the program must assign some random value. I will probably need to use "if" statements to set x and y to their constraint values if the arduino tries to set them outside of the constraints. (unfortunate because then the constraint function is pointless?? I could simple use the "if" statements alone in that case.

The basics of the program are good. I will start thinking about 3 dimensional equations now. I have made a simple wheelie bot that uses a differential drive equation, so I am slowly picking up on the basic kinematics of a few different degrees of motion. I can basically visualize all of my successful experiments thus far.

My code is pretty basic, and I think it is easy to understand. There are a lot of "Serial.print" functions which I used for troubleshooting, but I am leaving them in there for now. I like the idea of being able to see each step of the processing. the more servos there are, the nicer it will be to see the chart of each step of the computation.

I have been seeking others codes, and aim to collect any codes written by others that accomplish similar tasks that my codes, and prospective codes will do. As I am new to programming, I will not use any more libraries that absolutely necessary, and so am especially hoping to see more codes that do not reference libraries, or overly shorten each math statement with too many variables that ONLY shorten those important lines. (for example, making a variable for squared variables,, I understand that many are simply more accustomed to seeing these shorthands throughout a code,, however for my visualization, I prefer to skip them, and hope to see more codes where only the absolutely required language is used.

In this experiment, I had trouble with organization and syntax, and although I didn't change any of the "meat and potatoes", I did have to sit more than once after arriving at the proper functions to result in a working code.

//servo leg experiment. 2 servos moving on a 2 dimensional x,y coordinate plane. hip servo at 0,0

//hip servo oriented such that 0 degrees is straight downward, and 180 is straight upward

//knee servo oriented so that 0 is directly toward femur, and that 180 is directly away from tibia (so 180 fully extends the leg)

//my model is backward, with the leg extending to my left, so I input negative values and it seems to want to work

//there are still problems with this code, the solutions it may pick may not be within the range of motion of the servos.

//all measurements are in milimeters.

//open serial monitor to see angle B then angle A, B is hip angle so printed first

#include <Servo.h>

#include <math.h>

Servo hip;

Servo knee;

int femur = 100;

int tibia = 90;

int joyx ;

int joyy ;

int x = 90;

int y = 90;

float L;

float A;

float B;

void setup() {

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

hip.attach(9);

knee.attach(10);

Serial.begin(9600);

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

A = acos(((femur*femur)+(tibia*tibia)-(L*L))/(2*femur*tibia));//outputs number in radians as A

B = acos(((femur*femur)+(L*L)-(tibia*tibia))/(2*femur*L))+acos(((L*L)+(y*y)-(x*x))/(2*L*y));//outputs number in radians as B

Serial.print(degrees(B));//converts radians to degrees

Serial.print(" ");

Serial.println(degrees(A));//converts radians to degrees

hip.write(degrees(B));//converts radians to degrees

knee.write(degrees(A));//converts radians to degrees

}

void loop() {

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

int joyx = analogRead(A0);

int joyy = analogRead(A1);

Serial.println ("------------------");

Serial.print (joyx) ;

Serial.print (" ");

Serial.println (joyy);

constrain (x, 0,140) ;

constrain (y, -100, 100);

if (joyx >= 400 && joyx <= 600 && joyy <= 400 && joyy >= 600)

{ Serial.println ("no joy"); }

else {

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

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

Serial.print (xchange);

Serial.print (" ");

Serial.println (ychange);

x = x + xchange ;

y = y + ychange ;

Serial.print (x);

Serial.print (" llll ");

Serial.println (y);

//assuming servo hip is oriented, 0 straight down, 180 straight up, y is inverted, so -y is above the x axis

float L = sqrt((x*x)+(y*y));

float A = acos(((femur*femur)+(tibia*tibia)-(L*L))/(2*femur*tibia));//outputs number in radians as A

float B = acos(((femur*femur)+(L*L)-(tibia*tibia))/(2*femur*L))+acos(((L*L)+(y*y)-(x*x))/(2*L*y));//outputs number in radians as B

Serial.print(degrees(B));//converts radians to degrees

Serial.print(" ");

Serial.println(degrees(A));//converts radians to degrees

hip.write(degrees(B));//converts radians to degrees

knee.write(degrees(A));//converts radians to degrees

delay(0);

}}