 # Trouble getting math functions to work inside a library

I’m trying to troubleshoot an issue with a library i am writing. The library uses Inverse Kinematics to compute a solution for a 3-degree of freedom robotic leg.

I successfully had the code working as a sketch, and wanted to make it into a library. That way I can more easily use it across multiple projects.

The issues i am seeing is that math functions inside my library don’t execute properly. A bit of an issue for a library whose job is to do trigonometry. For testing I have commented out the Inverse Kinematics functions and am just executing three simple math problems inside the helper variables. (2+2), 2 squared, and square root of 4.

(2+2) returns 2.
2 squared returns 4.
square root of 4 returns 4.

The code is being run on an Arudino Uno and outputting to the serial monitor. The Arudino isn’t hooked up to anything else yet.

I can help but feel i’ve screwed up something very simple, but I’ve been unable to find a solution elsewhere.

Please excuse the spelling in the comments… Spelling isn’t not my strong suit.

The .h file

``````/*
IKLeg.h - Library for computing IK solution for
a 3 degree of freedom robotic leg
Created by Michael Curry, April 3ed, 2013.
Released into the public domain.
*/

#ifndef IKLeg_h
#define IKLeg_h

#include "Arduino.h"
#include "math.h"

class IKLeg
{
public:
IKLeg(int coxalength, int femurlenght, int tibialenght, int xoff, int yoff, int zoff);
void solve(int x, int y, int z); //computes solution for given coridinates
double Alpha(); //Returns last computed Alpha value
double Beta(); //Returns last computed Beta value
double Gamma(); //Returnes last computed Gamma value
void calibrate(); //Sets calibration and rest point for servos
void rest(); //sets outputs to there rest points
void Debug();// outputs debug values

int coxalenght; //input Length in mm
int femurlenght; //input Length in mm
int tibilenght; //input Length in mm

private:

// Physical discription of elements of each arm
int Coxa; //Length in mm
int Femur; //Length in mm
int Tibia; //Length in mm

// target cordinates
int X; // Positive X value of cordinates, mesured in mm from origin
int Y;  // Positive Y value of cordinates, mesured in mm from origin
int Z;  // Positive Z value of cordinates, mesured in mm from origin

// angles to be solved
double _Alpha; // output angle in degrees between the Femur and a line perpindicual to the x,y plain
double _Beta; // output angle in degrees angle between the Tibia and Femur
double _Gamma;  // output angle in degrees angle between the Coxa and origin in the x,y plain

// helper variables

double a1; // output of first half of Alpha solver
double a2; // output second half of Alpha solver
double L; //lenth of helper line L for Alpha solver
//double yActual =0; //variable for converting y to a fixed frame of reference

// modifier variables

int xOffset;  // diffrence between robot centroid and leg root in mm
int yOffset; // diffrence between robot centroid and leg root in mm
int zOffset; // controls minimum distance between Coxa and x,y plain

};

#endif
``````

The .cpp file

``````/*
IKLeg.h - Library for computing IK solution for
a 3 degree of freedom robotic leg
Created by Michael Curry, April 3ed, 2013.
Released into the public domain.
*/

#include "Arduino.h"
#include "math.h"
#include "IKLeg.h"

IKLeg::IKLeg(int coxalength, int femurlenght, int tibialenght, int xoff, int yoff, int zoff)
{
Coxa = coxalenght; //Length in mm
Femur = femurlenght; //Length in mm
Tibia = tibilenght; //Length in mm

xOffset = xoff;  // diffrence between robot centroid and leg root in mm
yOffset = yoff; // diffrence between robot centroid and leg root in mm
zOffset = zoff; // controls minimum distance between Coxa and x,y plain

}

void IKLeg::solve(int x, int y, int z) //computes leg solution for given coridinates
{

X = x - xOffset; // Positive X value of cordinates, mesured in mm from origin
Y = y - yOffset;  // Positive Y value of cordinates, mesured in mm from origin
Z = z - zOffset;  // Positive Z value of cordinates, mesured in mm from origin

//simple math inside the helper variables, to help find the problem

a1 = (2 + 2);
a2 = sq(2);
L = sqrt(4);

//L = sqrt( sq(Y - Coxa) + sq(Z)); //Solve for helper line L

/*
if(L > (Femur + Tibia)){  // Check to see if solution is possable,
Serial.println("Values out of range");
}
*/

//a1 = atan((Y - Coxa) / Z);  //Solve for Alpha
// a2 =  acos( (sq(Tibia) + sq(Femur) - sq(L)) / (2 * Femur * L));
// _Alpha = a1 + a2;

// _Beta = acos((sq(L) - sq(Tibia) - sq(Femur)) / (-2 * Tibia * Femur)); //Solve for Beta

//_Gamma = atan(300/500); //Solve for Gamma

Serial.println("SOLVED");
Serial.println(" ");

}

double IKLeg::Alpha(){ //Returns last computed Alpha value in degrees
(_Alpha * 57.3);
}

double IKLeg::Beta(){ //Returns last computed Beta value in degrees
(_Beta * 57.3);
}

double IKLeg::Gamma(){ //Returnes last computed Gamma value in degrees
(_Gamma * 57.3);
}

void IKLeg::Debug(){

Serial.println("version 0.011");

Serial.println("Inputs");

Serial.print("X : ");
Serial.print(X);
Serial.print(", Y : ");
Serial.print(Y);
Serial.print(", Z : ");
Serial.println(Z);

Serial.println("Helpers");

Serial.print("L : ");
Serial.print(L);
Serial.print(" a1 : ");
Serial.print(a1);
Serial.print(" a2 : ");
Serial.println(a2);

Serial.println("Outputs");

Serial.print("Alpha : ");
Serial.print(_Alpha);
Serial.print(" Beta : ");
Serial.print(_Beta);
Serial.print(" Gamma : ");
Serial.println(_Gamma);

Serial.println();
}

/* incompleat functions
void calibrate(); //Sets calibration and rest point for servos
void rest(); //sets outputs to the rest points

*/
``````

A simple sketch that runs a known set of coordinates through library for testing. Right now its just telling the library to compute the functions and then output the debugger.

``````/* This is a sketch of a Invers Kinamatics Solver for a 3 degree of freedom leg.
The intent is to get the equations function before intergrating it into a larger system.

The solver will provide the new positon of the the joints,
Transition from current postion to new postion will be handeled by another function.

*/
#include<IKLeg.h>  //includes the ikleg libary
#include<math.h>

IKLeg leftArm(40,88,190,29,24,0);
// creates an instance of the IKLeg libary for the robot's Left Arm
//Inputs needed (coxa lenght, femur lenght, tibia lenght, x offset, y offset, z offset)
// all Inputs are in mm,  Offsets are distance from robots cetroid to the legs root

void setup()
{

// sets serial port for debug output
Serial.begin(9600);
Serial.println("IK Leg Sketch");

}

void loop()
{

leftArm.solve(106,142,132);  //send a set of xyz cordinates to leg

leftArm.Debug();  //outputs libarys vaiables to serial port, so we can see whats happening.

}
``````

I don't see anything wrong in the code (except the missing return statements, but they're in functions you don't currently use). What output do you get?

I get

L : 2.00 a1 : 4.00 a2 : 4.00

... which is what I'd expect from this code:

``````        a1 = (2 + 2);
a2 = sq(2);
L = sqrt(4);
``````
``````double IKLeg::Gamma(){ //Returnes last computed Gamma value in degrees
(_Gamma * 57.3);
}
``````

Multiply Gamma by 57.3, discarding the result. Return nothing. What is the point of this useless function (and the other two like it)?

You did notice that you calculated a1, a2 and L and then printed out L, a1 and a2 ?

I think that is your problem, right there . You are actually getting the correct answers, 2, 4 and 4, in the order that you print them.