Math Library

Community General Discussions
1

Hi,

I'm having trouble with my formulas, I checked it many times but I dont seem to get the right codes to use. 
I am trying to move one leg of my Hexapod with 3DOF with positions(X,Y,Z) as pos(5,3,2) to find the angles A(coxa), B(femur) and C(tibia) but it always has errors.

My guess is that, I dont have math.h library on my sketch. But when I tried to download for one, there ain't no ready made library and I dont know how to do it on ( .h ) file... so anyone has it? Or if there really is problem with my code. Here's my code:

 

#include <Servo.h>

 

Servo servoa;             // Define a servo

Servo servob;

Servo servoc;

const int buttonPin = 7;  //sets pin 7 as button

int buttonState = 0; //sets button 1 as off

int pos[3] = {5,3,2}; //positions X,Y,Z

int f = 3;

int t = 4;

 

double l,C,Bp,B,Ap,A;

void setup() { 

  pinMode (buttonPin, INPUT);

  servoa.attach(10);  // Set a servo to digital pin 10

  servoa.write(90);

  servob.attach(9);  // Set a servo to digital pin 9

  servob.write(90);

  servoc.attach(8);  // Set a servo to digital pin 8

  servob.write(90);

  } 

 

void loop()  {           // Loop through motion tests

buttonState = digitalRead(buttonPin);

if (buttonState == HIGH){

  l = sqrt(pow(pos[0],2) + pow(pos[1],2));

  C = acos((pow(f,2) + pow(t,2) - pow(l,2))/(2*f*t));

  Bp = acos((pow(f,2) + pow(l,2) - pow(t,2))/(2*f*l));

  B = 90 + Bp;

  Ap = atan(pos[2]/pos[0]);

  A = 90+Ap;

  servoa.write(int(A));

  servob.write(int(B));

  servoc.write(int(C));

  delay(100);

}

else{

  servoa.write(90);

  servob.write(90);

  servoc.write(90);

  delay(1000);

}

}

Any help would be sooo much appreciated. Thank you!
2

Your code

compiled in both arduino 1.0 and 1.0.1 without error.

3

Same

In Arduino 1.0.1 it compiled really all right. But when I tried loading it to both Arduino Simulator and directly from Arduino Board, it does not do what it supposed to do. 

This should be the angles:

A = 112

B = 130

C = 112

Instead, A and C are fixated at 90deg and C moves from 90 to 180

4

Now i know

After series of thoughts and research…I finnaly able to solve this problem. 

It’s only about the delay…I changed it to 1000 and presto it works.

I felt like an idiot. Sorry for this post.

5

I am curious what some of the hard coded numbers are.

Specifically, f and t; 3 and 4.

Looking at the calculations, I see a few points that could be simplified.

 

  l = sqrt(pow(pos[0],2) + pow(pos[1],2));
  C = acos((pow(f,2) + pow(t,2) - pow(l,2))/(2ft));
  Bp = acos((pow(f,2) + pow(l,2) - pow(t,2))/(2fl));
  B = 90 + Bp;
  Ap = atan(pos[2]/pos[0]);
  A = 90+Ap;

My suggestions may not fix your problem, but, might help things run smoother, once you figure out your problem.

  sqrF = f * f; //of course you will need to declare sqrF, dblF, and sqrT somewhere
  sqrT = t * t;
  dblF = 2 * f;
  l = pow(pos[0],2) + pow(pos[1],2);
  C = acos((sqrF + sqrT - l)/(dblFt));
  Bp = acos(sqrF + l - sqrT)/(dblFsqrt(l)));
  B = 90 + Bp;
  Ap = atan(pos[2]/pos[0]);
  A = 90+Ap;