Hey there!
I (supposedly) have a problem with the function asin() from math.h: The following code is (or better should be) equating angles in an isoceles triangle and setting those as angles for servo rotation:
ServoMotor& Schulter;
ServoMotor& Ellenbogen;
SchrittMotor& Linearachse;
const unsigned char shoulderHeight;
const float xUpperLimit;
const float yUpperLimit;
const float zUpperLimit;
short z_height = z-shoulderHeight;
Serial.print("z-height: ");
Serial.println(z_height);
float endeff_vector = sqrt(square(x)+square(z_height));
Serial.print("vector length: ");
Serial.println(endeff_vector);
float beta_radian = endeff_vector/xUpperLimit;
Serial.print("half beta in radian: ");
Serial.println(beta_radian);
float beta = 2*asin(beta_radian);
Serial.print("beta: ");
Serial.println(beta);
float alpha = (180-beta)/2 - x/sqrt(2*(square(x)+square(z_height)));
Serial.print("alpha: ");
Serial.println(alpha);
if(alpha<-30||alpha>90||beta<0||beta>120){ //TODO: nochmal alle Funktionen für drive überprüfen!
Serial.println("Result of kinematic equation: parameters outside limits.");
return;
}
Serial.print("Driving shoulder to (deg) ");
Serial.println(alpha);
Schulter.drive(alpha);
Serial.print("Driving ellbow to (deg) ");
Serial.println(beta);
Ellenbogen.drive(beta);
Serial.print("Driving y-axis to (mm) ");
Serial.println(y);
Linearachse.driveStepper(y);
The code produces the following output:
15:41:25.610 -> z-height: 0
15:41:25.657 -> vector length: 250.00
15:41:25.657 -> half beta in radian: ⸮⸮
I suppose the program crashes at this point. My idea was already to seperate the equation of beta_radian from beta (was in the function call of asin() before). Since the program now crashes even before the equation with asin(), maybe I have another problem that I don't see. Any ideas?
What values are given to those constants?
It looks like you expect asin() to return an angle in degrees, rather than radians.
Why ignore the fractional part of the result?
short z_height = z-shoulderHeight;
116, 300, 380, and 300. Sorry, that whole thing is in a class, I didn't include the constructor.
I don't expect the height to be fractional, the robot that is controlled is not at all precise.
Maybe the radian-thing causes issues. I'll convert the values, then. Reading documentations was never my strong point...
You are using floats, which always have a fractional component (it may be zero).
Also, operations with integers will overflow. Declaring z to be short limits the range of values to -32768 to 32768.
Ok, I corrected everything. Now I get:
16:27:39.540 -> z-height: 0.00
16:27:39.540 -> vector length: 250.00
16:27:39.587 -> half beta in sine: 0.⸮
from
float z_height = z-shoulderHeight;
Serial.print("z-height: ");
Serial.println(z_height);
float endeff_vector = sqrt(square(x)+square(z_height));
Serial.print("vector length: ");
Serial.println(endeff_vector);
float beta_sin = endeff_vector/xUpperLimit;
Serial.print("half beta in sine: ");
Serial.println(beta_sin);
float beta = 2*asin(beta_sin)*180/PI;
Serial.print("beta: ");
Serial.println(beta);
float alpha = (180-beta)/2 - x/sqrt(2*(square(x)+square(z_height)));
Serial.print("alpha: ");
Serial.println(alpha);
if(alpha<-30||alpha>90||beta<0||beta>120){ //TODO: nochmal alle Funktionen für drive überprüfen!
Serial.println("Result of kinematic equation: parameters outside limits.");
return;
}
Serial.print("Driving shoulder to (deg) ");
Serial.println(alpha);
Schulter.drive(alpha);
Serial.print("Driving ellbow to (deg) ");
Serial.println(beta);
Ellenbogen.drive(beta);
Serial.print("Driving y-axis to (mm) ");
Serial.println(y);
Linearachse.driveStepper(y);
Oh and before I forget, I enter 116 for z, so z_height is 0.
I know that, but z will never exceed ~-100...~400. Anyway, I now declared it as float value - the short was a holdover from when all the const-values where shorts two.
400*400 = 160000, which overflows a short. Also, the input to asin() must be in the range {-1, 1}.
The following code produces this output:
z-height: 0.00
float square(float x) {
return x * x;
}
void setup() {
Serial.begin(9600);
while (!Serial);
const float z_height = 0.;
const float shoulderHeight = 116.;
const float x = 250.;
const float xUpperLimit = 300.;
const float yUpperLimit = 380.;
const float zUpperLimit = 300.;
Serial.print("z-height: ");
Serial.println(z_height);
float endeff_vector = sqrt(square(x) + square(z_height));
Serial.print("vector length: ");
Serial.println(endeff_vector);
float beta_sin = endeff_vector / xUpperLimit;
Serial.print("half beta in sine: ");
Serial.println(beta_sin);
float beta = 2 * asin(beta_sin) * 180 / PI;
Serial.print("beta: ");
Serial.println(beta);
}
It looks like your sketch is crashing. Put "Serial.flush();" after every serial output to make sure the message is sent before going on to the next calculation. You may find that the crash is much further in your sketch and messages in the output buffer are getting lost.
You should brush up on your documentation reading. Specifically look up what a radian is and how many angular degrees a truncated radian result will be.
With that attitude, it will be difficult to get far in this hobby.
system
October 27, 2021, 9:54pm
13
This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.