I'm incrementally building the code for a project I'm working on and I've got a head scratcher with the following code:
/*
Code to perform coordinate transform for 5-axis arm. Measures angles, computes coordiante transform, then outputs data to LCD. When button is pressed it also sends data via Serial to PC, with repeated data transmission for long button press.
*/
// include the library code:
#include <LiquidCrystal.h>
// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(7, 6, 5, 4, 3, 2);
char xposc[7];
char xposlc[7];
char yposc[7];
char yposlc[7];
char zposc[7];
char zposlc[7];
int tim = 0;
int timd;
int buttonState = 0;
int buttonStateLast = 0; // 0 means low last, 1 means new high, 2 means long-press
int dummy = 0;
int txlast = 0; // Time (ms) of last serial data transmission
int deltxlast = 0; // Time since last transmission
int db1 = 0;
float xpos = 0., ypos = 0., zpos = 0.;
float xposl = 1., yposl = 2., 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 theta1r = 0., theta2r = 0., theta3r = 0., theta4r = 0., theta5r = 0.;
float S1 = 0., S2 = 0., S3 = 0., S4 = 0., S5 = 0.;
float C1 = 0., C2 = 0., C3 = 0., C4 = 0., C5 = 0.;
// Set constants
const int buttonPin = 9;
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() {
// Initialize button pin as input
pinMode(buttonPin, INPUT);
// set up the LCD's number of columns and rows:
lcd.begin(20, 4);
// Print column headers
lcd.print("Tip [mm] Curr Last");
lcd.setCursor(0,1);
lcd.print("X-Pos 0 0");
lcd.setCursor(0,2);
lcd.print("Y-Pos 0 0");
lcd.setCursor(0,3);
lcd.print("Z-Pos 0 0");
tim=millis();
Serial.begin(9600); //initialize serial communication
}
void loop() {
char buffy[21];
timd = millis() - tim; // Time since last screen update
// Convert degrees to radians
theta1r=theta1*DEG_TO_RAD;
theta2r=(theta2-90)*DEG_TO_RAD;
theta3r=(theta3+90)*DEG_TO_RAD;
theta4r=theta4*DEG_TO_RAD;
theta5r=theta5*DEG_TO_RAD;
// Compute trig functions
S1=sin(theta1r);
S2=sin(theta2r);
S3=sin(theta3r);
S4=sin(theta4r);
S5=sin(theta5r);
C1=cos(theta1r);
C2=cos(theta2r);
C3=cos(theta3r);
C4=cos(theta4r);
C5=cos(theta5r);
// 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;
// Check for button depressed
db1 = digitalRead(buttonPin);
buttonRaw = (0.95*buttonRaw)+(db1*0.05);
if (buttonRaw > 0.9) {
buttonState = 1;
}
else {
buttonState = 0;
// lcd.setCursor(5,2);
// lcd.print(" ");
}
deltxlast = millis()-txlast; // Time since last transmission
if (buttonState == 1) {
if (buttonStateLast == 2) {
// Check for long button press
if (deltxlast >= 1000) {
// Write x,y,z to Serial
Serial.print(xpos);
Serial.print(",");
Serial.print(ypos);
Serial.print(",");
Serial.println(zpos);
// Update last position and tx times
txlast = millis();
xposl = xpos;
yposl = ypos;
zposl = zpos;
// lcd.setCursor(6,1); Debug
// lcd.print("2");
}
}
else if (buttonStateLast == 1) { // Check for start of long press
if (deltxlast >= 3000) {
// Write x,y,z to Serial
Serial.print(xpos);
Serial.print(",");
Serial.print(ypos);
Serial.print(",");
Serial.println(zpos);
// Update last position and tx times
txlast = millis();
xposl = xpos;
yposl = ypos;
zposl = zpos;
buttonStateLast = 2;
// lcd.setCursor(6,1); debug
// lcd.print("1");
}
}
else if (buttonStateLast == 0) { // Check for new button press with debounce
// Write x,y,z to Serial
Serial.print(xpos);
Serial.print(",");
Serial.print(ypos);
Serial.print(",");
Serial.println(zpos);
// Update last position and tx times
txlast = millis();
xposl = xpos;
yposl = ypos;
zposl = zpos;
buttonStateLast = 1;
// lcd.setCursor(6,1); Debug
// lcd.print("0");
}
}
else if (buttonState == 0) {
buttonStateLast = 0;
}
// Update screen measurements
if (timd > 200) {
// Convert x-, y-, and z-positions to strings
dtostrf(xpos, 5, 1, xposc);
dtostrf(ypos, 5, 1, yposc);
dtostrf(zpos, 5, 1, zposc);
dtostrf(xposl, 5, 1, xposlc);
dtostrf(yposl, 5, 1, yposlc);
dtostrf(zposl, 5, 1, zposlc);
// Update Current and Last columns
lcd.setCursor(9,1);
sprintf(buffy, "%s %s", xposc, xposlc);
lcd.print(buffy);
lcd.setCursor(9,2);
sprintf(buffy, "%s %s", yposc, yposlc);
lcd.print(buffy);
lcd.setCursor(9,3);
sprintf(buffy, "%s %s", zposc, zposlc);
lcd.print(buffy);
tim=millis();
}
}
I have an section (lines 96-105) that debounces the button press (simulated with a jumper I move between 5V and GND) and everything works fine as long as the lcd code at lines 103 and 104 is there. If I try to comment those lines out or delete them, the code constantly transmits data as soon as the jumper isn't tied to ground. When it's not tied to ground, pin 9 flips between high and low rapidly and buttonRaw sits around 0.5, but buttonState should only be 1 if buttonRaw hits 0.9.
As best I can figure, removing lines 103 and 104 is somehow causing buttonState to rapidly flow between 1 and 0. Any thoughts on why this is happening? It's not the end of the world if I have to leave some dummy lines in there, it's just puzzling.
Thanks,
Reed