Help fixing my project, hopefully me misunderstanding the math library

Projects Programming
1 

//yup, those sure are libraries
#include <math.h>
#include <Servo.h>

//yup, those sure are servos
Servo Shoulder;
Servo Elbow;

int x = 30;                //x position desired(input)
int y = 30;                //y position desired(input)
int shoulderAngle = 0;     //self-explanitory
int elbowAngle = 0;        //self-explanitory
int bicep_Length = 60;     //arm attached to first servo
int foremarm_Length = 70;  //arm attached to second servo
float P = 0;               //angle between the ground and the line from the endpoint to the base
float B = 0;               //angle between the endpoint and the base
int shoulderPin = (3);     //self-explanitory
int elbowPin = (5);        //self-explanitory



void setup() {
  pinMode(2, INPUT);
  pinMode(4, INPUT);
  Shoulder.attach(shoulderPin);
  Elbow.attach(elbowPin);
  Serial.begin(9600);
}

void loop() {

  //im using a pretty simple PWM pre-built controller because i couldnt be arsed to design and build one myself
  unsigned long moveRLButton = (pulseIn(7, HIGH));
  unsigned long moveFBButton = (pulseIn(8, HIGH));

  if (1300 <= moveFBButton <= 1500) {
    x = x;
  }
  if (1300 >= moveFBButton) {
    x = x - 1;
  }
  if (1500 <= moveFBButton) {
    x = x + 1;
  }
  if (1300 <= moveRLButton <= 1500) {
    y = y;
  }
  if (moveRLButton < 1200) {
    y = y + 1;
  }
  if (moveRLButton > 1500) {
    y = y - 1;
  }
  //these just stop the arm from attempting to move out of bounds
  if (x >= 60) {
    x = 60;
  }
  if (x <= 10) {
    x = 10;
  }
  if (y >= 60) {
    y = 60;
  }
  if (y <= 10) {
    y = 10;
  }

  //(allegedly) super simple inverse kinematics for 2dof
  int C2 = (x ^ 2 + y ^ 2);
  float C = sqrt(C2);   
  float P = atan(y / x);
  float B = acos((foremarm_Length ^ 2 - bicep_Length ^ 2 - C2) / -2 * bicep_Length * C);


  elbowAngle = acos((C2 - bicep_Length ^ 2 - foremarm_Length ^ 2) / -2 * bicep_Length * foremarm_Length);
  shoulderAngle = (B + P);

  //writes the desired angle to the servos
  Shoulder.write(shoulderAngle);
  Elbow.write(elbowAngle);

  //tells me what to cry about(only here so i can see if im getting the right #'s)
  Serial.println(shoulderAngle);
  Serial.println(elbowAngle);
  Serial.println(C);
  Serial.println(C2);
  Serial.println(P);
  Serial.println(B);
  delay(400000);
}

i think i might be using a wrong variable type or have an operation in some way formatted incorrectly(ie i see ((x^2) + (y^2)) but code sees ((x) ^ (2+y) ^ (2)) (<--tried this, did not fix the issue) but for the life of me i cant figure out what is the issue, though B almost always outputs not a number which is what id like help with to start off with; im so sorry if tis cripplingly obvious im very new to this

2

Use x * x

3

Your problems are not with the math library.

Your problems are with not understanding basic C programming concepts.

Things like what the ^ operator does, and how to properly test multiple conditions.

I would suggest setting your compile warning level to ALL, and then doing some basic reading on C.

1 Like
4

If you are following some guide or other resource, it is best to include a link to it so it becomes clearer what you are trying to achieve.

5

As others have said, "^" doesn't do what you think it does in C language. It is a numeric operator, so you won't get an error message, but it has a completely different purpose to some other languages. Look it up.

6

The C language doesn't allowed multiple conditions this way, use two separate condition and logic operators:

if ((1300 <= moveFBButton ) && (moveFBButton <= 1500))