Hello all,
I am quite new to the world of Arduino but I've been working to learn as much as I can the last few days. I'm trying to perform a coordinate transform for x,y,z through 5 coordinate systems. I've worked out the transform equations and checked it in Excel. I was able to simply paste the equation into my sketch due to how I assigned variables, so there shouldn't be any typos.
The problem I'm encountering is when the code is put in the setup() part of the sketch it computes correctly (should be 200.0 or so):
/*
LiquidCrystal Library - Hello World
*/
// include the library code:
#include <LiquidCrystal.h>
// initialize the library by associating any needed LCD interface pin
// with the arduino pin number it is connected to
const int rs = 7, en = 6;
LiquidCrystal lcd(rs, en, 5, 4, 3, 2);
float xpos;
float xposl = 1;
float ypos;
float yposl = 2;
float zpos;
float zposl = 3;
float buttonRaw = 0;
float theta1 = 74.215;
float theta2 = -19.155;
float theta3 = -89.646;
float theta4 = -31.66;
float theta5 = -32.348;
float S1 = 0;
float S2 = 0;
float S3 = 0;
float S4 = 0;
float S5 = 0;
float C1 = 0;
float C2 = 0;
float C3 = 0;
float C4 = 0;
float C5 = 0;
// Set constants
const float Px5 = 6.;
const float Py5 = 35.;
const float Pz5 = 197.;
const float d1 = 112.;
const float a2 = -403.;
const float d3 = 7.;
const float d4 = 404.;
const float d5 = -7.;
void setup() {
// set up the LCD's number of columns and rows:
lcd.begin(20, 4);
// Print a message to the LCD.
char buffy[5];
// Covert deg to rad
theta1=theta1*DEG_TO_RAD;
theta2=(theta2-90)*DEG_TO_RAD;
theta3=(theta3+90)*DEG_TO_RAD;
theta4=theta4*DEG_TO_RAD;
theta5=theta5*DEG_TO_RAD;
// compute sin and cos
S1=sin(theta1);
S2=sin(theta2);
S3=sin(theta3);
S4=sin(theta4);
S5=sin(theta5);
C1=cos(theta1);
C2=cos(theta2);
C3=cos(theta3);
C4=cos(theta4);
C5=cos(theta5);
// Calculate x-, y-, and z-positions
xpos=(((C1*C2*C3-C1*S2*S3)*C4-S1*S4)*C5-(C1*C2*S3+C1*S2*C3)*S5)*Px5-((C1*C2*C3-C1*S2*S3)*S4+S1*C4)*Py5+((S1*S4-(C1*C2*C3-C1*S2*S3)*C4)*S5-(C1*C2*S3+C1*S2*C3)*C5)*Pz5+((C1*C2*C3-C1*S2*S3)*S4+S1*C4)*d5-(C1*C2*S3+C1*S2*C3)*d4+S1*d3+C1*C2*a2;
ypos=(((S1*C2*C3-S1*S2*S3)*C4+C1*S4)*C5-(S1*C2*S3+S1*S2*C3)*S5)*Px5+(C1*C4-(S1*C2*C3-S1*S2*S3)*S4)*Py5-((C1*S4+(S1*C2*C3-S1*S2*S3)*C4)*S5+(S1*C2*S3+S1*S2*C3)*C5)*Pz5+((S1*C2*C3-S1*S2*S3)*S4-C1*C4)*d5-(S1*C2*S3+S1*S2*C3)*d4+S1*C2*a2-C1*d3;
zpos=((S2*C3+C2*S3)*C4*C5+(C2*C3-S2*S3)*S5)*Px5-(S2*C3+C2*S3)*S4*Py5+((C2*C3-S2*S3)*C5-(S2*C3+C2*S3)*C4*S5)*Pz5+(S2*C3+C2*S3)*S4*d5+(C2*C3-S2*S3)*d4+d1+S2*a2;
dtostrf(xpos, 5, 1, buffy);
lcd.setCursor(0,0);
lcd.print(buffy);
}
void loop() {
// set the cursor to column 0, line 1
// (note: line 1 is the second row, since counting begins with 0):
lcd.setCursor(0, 1);
// print the number of seconds since reset:
lcd.print(millis() / 1000);
}
However, if I cut and paste it into the loop() part of the sketch I get a bad answer (17.2):
/*
LiquidCrystal Library - Hello World
*/
// include the library code:
#include <LiquidCrystal.h>
// initialize the library by associating any needed LCD interface pin
// with the arduino pin number it is connected to
const int rs = 7, en = 6;
LiquidCrystal lcd(rs, en, 5, 4, 3, 2);
float xpos;
float xposl = 1;
float ypos;
float yposl = 2;
float zpos;
float zposl = 3;
float buttonRaw = 0;
float theta1 = 74.215;
float theta2 = -19.155;
float theta3 = -89.646;
float theta4 = -31.66;
float theta5 = -32.348;
float S1 = 0;
float S2 = 0;
float S3 = 0;
float S4 = 0;
float S5 = 0;
float C1 = 0;
float C2 = 0;
float C3 = 0;
float C4 = 0;
float C5 = 0;
// Set constants
const float Px5 = 6.;
const float Py5 = 35.;
const float Pz5 = 197.;
const float d1 = 112.;
const float a2 = -403.;
const float d3 = 7.;
const float d4 = 404.;
const float d5 = -7.;
void setup() {
// set up the LCD's number of columns and rows:
lcd.begin(20, 4);
// Print a message to the LCD.
}
void loop() {
char buffy[5];
// Covert deg to rad
theta1=theta1*DEG_TO_RAD;
theta2=(theta2-90)*DEG_TO_RAD;
theta3=(theta3+90)*DEG_TO_RAD;
theta4=theta4*DEG_TO_RAD;
theta5=theta5*DEG_TO_RAD;
// compute sin and cos
S1=sin(theta1);
S2=sin(theta2);
S3=sin(theta3);
S4=sin(theta4);
S5=sin(theta5);
C1=cos(theta1);
C2=cos(theta2);
C3=cos(theta3);
C4=cos(theta4);
C5=cos(theta5);
// Calculate x-, y-, and z-positions
xpos=(((C1*C2*C3-C1*S2*S3)*C4-S1*S4)*C5-(C1*C2*S3+C1*S2*C3)*S5)*Px5-((C1*C2*C3-C1*S2*S3)*S4+S1*C4)*Py5+((S1*S4-(C1*C2*C3-C1*S2*S3)*C4)*S5-(C1*C2*S3+C1*S2*C3)*C5)*Pz5+((C1*C2*C3-C1*S2*S3)*S4+S1*C4)*d5-(C1*C2*S3+C1*S2*C3)*d4+S1*d3+C1*C2*a2;
ypos=(((S1*C2*C3-S1*S2*S3)*C4+C1*S4)*C5-(S1*C2*S3+S1*S2*C3)*S5)*Px5+(C1*C4-(S1*C2*C3-S1*S2*S3)*S4)*Py5-((C1*S4+(S1*C2*C3-S1*S2*S3)*C4)*S5+(S1*C2*S3+S1*S2*C3)*C5)*Pz5+((S1*C2*C3-S1*S2*S3)*S4-C1*C4)*d5-(S1*C2*S3+S1*S2*C3)*d4+S1*C2*a2-C1*d3;
zpos=((S2*C3+C2*S3)*C4*C5+(C2*C3-S2*S3)*S5)*Px5-(S2*C3+C2*S3)*S4*Py5+((C2*C3-S2*S3)*C5-(S2*C3+C2*S3)*C4*S5)*Pz5+(S2*C3+C2*S3)*S4*d5+(C2*C3-S2*S3)*d4+d1+S2*a2;
dtostrf(xpos, 5, 1, buffy);
lcd.setCursor(0,0);
lcd.print(buffy);
// set the cursor to column 0, line 1
// (note: line 1 is the second row, since counting begins with 0):
lcd.setCursor(0, 1);
// print the number of seconds since reset:
lcd.print(millis() / 1000);
}
What on earth is going on here? Why is it computing differently and how can I get it to compute the same in the loop?
Thank you,
Reed