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.