Hi guys. i do have the problem that my formula in my sketch is somehow wrong.
The gPosGoal is a float.
gPosGoal = (160160+160160-226.27226.27)/(2160*160);
the Serial.print does say 4.57
when i type the exact same thing into my calculator i get 3.6857*10^-5 which is correct. any ideas what i did wrong?
Please, post your codes. We want to see the style/syntax of your coding.
Do you want to see this: 0.00003685710 on the Serial Monitor or you want to remain happy just with 0.000037 (rounded)?
What Arduino you are using.
Turn your compiler warnings up to the max and you'll get a warning about integer overflow. Make some of those 160s 160.0 and the problem should go away.
the formula is in the CALCULATE G-AXIS POSITION part
#include <math.h>
#include <Servo.h>
#include <SPI.h>
#include <SD.h>
//Konstanten
const int mainLength = 160;
const int secLength = 160;
const int armLength = 130;
//Z-Servo
Servo zServo;
Servo mainServo;
Servo mainServo2;
Servo gServo;
int zPos = 45;
int mainPos = 90;
int gPos = 5;
int zPosNow = 45;
int mainPosNow = 90;
int gPosNow = 5;
int zPosGoal = 45;
int mainPosGoal = 90;
float gPosGoal = 0; //------------------------------ <- Defining the Variable
int steps = 0;
//Koordinaten
String koordinaten = "";
char koords[50];
int gMode = 0;
int xCoord = 160;
int yCoord = 0;
int zCoord = 160;
int F = 100;
char delimiter[] = " ";
String line[] = "160 0 160";
int lines = 0;
//button
#define button 5
#define led 6
//SD KARTE
File dataFile;
String copyFile;
void setup() {
// put your setup code here, to run once:
zServo.attach(2);
mainServo.attach(3);
gServo.attach(4);
mainServo2.attach(8);
Serial.begin(9600);
Serial.println("Gebe die Zielpunktkoordinaten ein: (X Y Z Angriffswinkel");
zServo.write(map(zPosGoal,0,90,180,0));
mainServo.write(map(mainPosGoal,0,90,0,180));
mainServo2.write(map(mainPosGoal,0,90,180,0));
gServo.write(map(gPosGoal,0,90,0,180));
//SD KARTE
SD.begin(7);
sdRead(); //Read the File and Splits in
pinMode(button, INPUT);
pinMode(led, OUTPUT);
}
void loop() {
// put your main code here, to run repeatedly:
//###################################################### READ SERIAL MONITOR #####################################################
if (Serial.available() > 0) {
koordinaten = Serial.readString();
koordinaten.toCharArray(koords,50);
Serial.print("KOORDINATENEINGABE: ");
Serial.println(koords);
readLines();
zAxis(); //Call void to Caluclate Z-Axis
xAxis(); //Call void to Calculate Main-Axis
gAxis();
servoWrite(); //Move servos depending on steps
Serial.println("MOVED");
}
if (digitalRead(button) == HIGH) {
digitalWrite(led,HIGH);
Serial.print("ZEILEN: ");
Serial.println(lines);
callProgramm();
digitalWrite(led,LOW);
}
delay(100);
}
//##################################################### CALCULATE Z-AXIS POSITION ##################################################
void zAxis() {
float hypo = float(sqrt(sq(float(xCoord))+sq(float(yCoord))));
zPosGoal = 45+(asin((yCoord/hypo))*180/PI);
Serial.print("MOTORWINKEL: ");
Serial.println(zPosGoal);
steps = sqrt(sq(zPosGoal-zPosNow));
//zServo.write(map(zPos,0,90,180,0));
}
//##################################################### CALCULATE MAIN-AXIS POSITION ##################################################
void xAxis() {
float dist = float(sqrt(sq(float(xCoord))+sq(float(yCoord))));
float hypo = sqrt(sq(dist)+sq(float(zCoord)));
mainPosGoal = (acos((sq(mainLength)+sq(hypo)-sq(secLength))/(2*mainLength*hypo))*180/PI+asin(zCoord/hypo)*180/PI);
Serial.print("MAINWINKEL: ");
Serial.println(mainPosGoal);
int stp = sqrt(sq(mainPosGoal-mainPosNow));
if (stp > steps) {
steps = stp;
Serial.println("STEPS MAIN HIGHER THAN Z");
}
//mainServo.write(map(mainPos,0,90,0,180));
}
//##################################################### CALCULATE G-AXIS POSITION ##################################################
void gAxis() {
float dist = float(sqrt(sq(float(xCoord))+sq(float(yCoord))));
float hypo = sqrt(sq(dist)+sq(float(zCoord)));
gPosGoal = (2*sq(160)-226.27*226.27)/(2*160*160); // ----------------- Here is the Formula.
//gPosGoal = sin(90*PI/180);
Serial.print("HYPO: ");
Serial.println(hypo);
Serial.print("G POS: ");
Serial.println(gPosGoal); //------------------ Here im printing it to Serial
int stp = sqrt(sq(gPosGoal-gPosNow));
if (stp > steps) {
steps = stp;
Serial.println("STEPS G HIGHER THAN Z");
}
//mainServo.write(map(mainPos,0,90,0,180));
}
//##################################################### WRITE SERVO POSITION ##################################################
void servoWrite() {
float zmove = float(zPosGoal-zPosNow)/steps;
float mainmove = float(mainPosGoal-mainPosNow)/steps;
float gmove = float(gPosGoal-gPosNow)/steps;
Serial.println(steps);
Serial.println(zmove);
Serial.println(mainmove);
Serial.println(gmove);
int i;
for(i = 0; i<steps; i++) {
zPos = zPosNow+i*zmove;
mainPos = mainPosNow+i*mainmove;
gPos = gPosNow+i*gmove;
zServo.write(map(zPos,0,90,180,0));
mainServo.write(map(mainPos,0,90,0,180));
mainServo2.write(map(mainPos,0,90,180,0));
gServo.write(map(gPos,0,90,0,180));
//Serial.print("ZPOS: ");
//Serial.println(zPos);
delay(10); //SPEED OF THE MOTORS (maybe 10 but to fast)
}
zServo.write(map(zPosGoal,0,90,180,0));
mainServo.write(map(mainPosGoal,0,90,0,180));
mainServo2.write(map(mainPosGoal,0,90,180,0));
gServo.write(map(gPosGoal,0,90,0,180));
mainPosNow = mainPosGoal;
zPosNow = zPosGoal;
gPosNow = gPosGoal;
steps = 0;
}
//######################################################## SD CARD READ ################################################
void sdRead() {
dataFile = SD.open("Test.TXT",FILE_READ);
int i = 0;
while (true) {
line[i] = dataFile.readStringUntil('\n');
Serial.println(line[i]);
if (line[i] == "") {
break;
}
i++;
lines ++;
}
}
void callProgramm() {
int i;
for(i = 0; i < lines; i++) {
koordinaten = line[i];
readLines();
zAxis(); //Call void to Caluclate Z-Axis
xAxis(); //Call void to Calculate Main-Axis
gAxis();
servoWrite(); //Move servos depending on steps
Serial.print("LINE EXECUTED: ");
Serial.println(i+1);
delay(200);
}
//digitalWrite(led,LOW);
}
If I ran your equation as posted I too get the same figure. However, if I make all terms explicit floats with decimal places:
gPosGoal = (160.0*160.0+160.0*160.0-226.27*226.27)/(2.0*160.0*160.0);
and print it with:
Serial.println( gPosGoal, 10 );
I get
0.0000368500
Damn. thats a simple Solution. Thank you guys! But whats the Reason for it? Wildbill said something about Integer Overflow.
I need to modify that formula to use variables instead of the 160.0´s. but the Variables are Integers. do i have to define these as floats instead of int?
I have forgotten the precise rules for promoting constants in an expression, but from what the compiler is saying, it seems clear that it’s working with int until it’s forced to promote to float when it encounters 226.27. So it calculates 60160+160160 as an integer expression and then notes that it overflows. Same with the divisor.
You could make those variables float, or just cast your integer variables to float for the sake of the calculation. Either way, if the result of the calc is the magnitude you’re expecting, you’re going to have to ask the compiler to work entirely in float.
I am really curious to know what the final result that the OP wants:
0.00003685710
OR
0.000037
OR
0.0000368500
OR
0.00
Weisbrot233:
I need to modify that formula to use variables instead of the 160.0´s. but the Variables are Integers. do i have to define these as floats instead of int?
Even you do so, are you sure that you going to have this result: 0.00003685710?