Code is below..
It is not totally practical to write out debug code to the serial port connected to the computer due to the fact I am interfacing with a closed source app. For basic debugging I can bypass this app by substituting my own "dummy" client. I will give this a try next but the dummy client operates minimally like the real app being interfaced to. I currently don't have another device to write out debug code.
maxLat, maxLong, and maxMag are integers. For this particular print, there recently was an erroneous line printed one row up and a few columns to the right. It seem like it generally messes up in the same way when it does happen.
There is another print line:
lcd.print("Simulator offf");
When this one messes up there is an extra "f" printed to the LCD ("Simulator off").
#include <LiquidCrystal.h>
// (rs, enable, d0, d1, d2, d3, d4, d5, d6, d7);
LiquidCrystal lcd(40, 38, 22, 24, 26, 28, 30, 32, 34, 36);
char debugLine [100];
int DEBUG = 0;
int prevLowPin = -1;
int prevHighPin = -1;
const int lowestPin = 2;
const int highestPin = 13;
const byte midPoint = 127;
const int limitOfMagnitude = 165; // about 3v when used as PWM duty cycle
const int clipLED = 52;
const int magPot = 0; // increase the force by turning potentiometer
const int magPotValue = 0;
const float magMultiplier = 0.048; // scale the pot value (0-1023) down by this amount (adds at most 50)
unsigned long lastTimeWasRecord = 0;
// these variables track "peak hold" updates to LCD
const int whenToUpdate = 4;
int numUpdates = 0;
unsigned long clipStart = 0;
int maxLat = 0;
int maxLong = 0;
int maxMag = 0;
int holdIterations = 120;
int holdLatCount = 0;
int holdLongCount = 0;
int holdMagCount = 0;
void setup() {
// set pins as outputs:
for (int thisPin =lowestPin; thisPin <= highestPin; thisPin++) {
pinMode(thisPin, OUTPUT);
}
pinMode(clipLED, OUTPUT);
pinMode(magPot, INPUT);
Serial.begin(115200);
// seems like once I hooked up LCD I needed to start
// zeroing out the vibra pins
for (int thisPin =lowestPin; thisPin <= highestPin; thisPin++) {
analogWrite(thisPin, 0);
}
// cycle through for hardware check
for (int thisPin =lowestPin; thisPin <= highestPin; thisPin++) {
analogWrite(thisPin, 80);
delay(500); // 0.5 sec
analogWrite(thisPin, 0);
}
lcd.begin(20, 4);
blankLCD();
}
void loop() {
// loop will be complete with one record processed
short int latForce = 0;
short int longForce = 0;
int magnitude = 0;
float angle = 0;
int lowBound = 0;
//int highBound = 0;
if ( !isHeader() ) {
// if no data in a bit turn off vibras
if (millis() - lastTimeWasRecord > 3000 && lastTimeWasRecord != 0) {
for (int thisPin =lowestPin; thisPin <= highestPin; thisPin++) {
analogWrite(thisPin, 0);
}
blankLCD();
lastTimeWasRecord = 0;
}
return;
}
latForce = getByte() - midPoint;
longForce = getByte() - midPoint;
// Now have a lat/long pair. Process it. Go after if more fields
// get added later (additional capability)
magnitude = sqrt( sq(latForce) + sq(longForce) ) + analogRead(magPot) * 0.048;
angle = atan2( longForce, latForce ) * 180 / M_PI;
if (angle < 0) {
angle = 360 + angle;
} // angle should be from 0 to 360 degrees
lowBound = int (angle / 30) * 30; // map to a angle where a vibra (every 30 deg)
int vibraLow = angle / 30 + 2; // turn into vibrator pin number
int vibraHigh = (vibraLow==highestPin?lowestPin:vibraLow+1);
float relLowBound = float ((angle - lowBound) / 30 );
float relHighBound = 1.0 - relLowBound;
int vibraLowDutyCycle = magnitude * (1.0 - relLowBound);
int vibraHighDutyCycle = magnitude * (1.0 - relHighBound);
analogWrite(vibraLow, vibraLowDutyCycle);
analogWrite(vibraHigh, vibraHighDutyCycle);
// less efficient then commented out approach for turning off prev vibra
for (int thisPin =lowestPin; thisPin <= highestPin; thisPin++) {
if (thisPin != vibraLow && thisPin != vibraHigh) {
analogWrite(thisPin, 0);
}
}
lastTimeWasRecord = millis();
// turn off previous vibra if it wasn't turned on this pass
/* if (prevLowPin != vibraLow && prevLowPin != vibraHigh) {
analogWrite(prevLowPin, 0);
}
if (prevHighPin != vibraLow && prevHighPin != vibraHigh) {
analogWrite(prevHighPin, 0);
}
*/
//prevLowPin = vibraLow;
//prevHighPin = vibraHigh;
if (magnitude > limitOfMagnitude) {
if (DEBUG) {
sprintf(debugLine, "clipped: lat: %d long: %d magnitude: %d",
latForce, longForce, magnitude);
Serial.println(debugLine);
}
magnitude = limitOfMagnitude; //this is about 3v
//digitalWrite(clipLED, HIGH);
//digitalWrite(clipLED, LOW);
lcd.setCursor(0,2);
lcd.print("CLIP");
clipStart = millis();
}
if ( millis() - clipStart > 3000) {
lcd.setCursor(0,2);
lcd.print(" ");
clipStart = 999999999;
}
if (latForce > maxLat) {
maxLat = latForce;
holdLatCount = 0;
}
else {
holdLatCount++;
if (holdLatCount > holdIterations) {
maxLat = latForce;
}
}
if (longForce > maxLong) {
maxLong = longForce;
holdLongCount = 0;
}
else {
holdLongCount++;
if (holdLongCount > holdIterations) {
maxLong = longForce;
}
}
if (magnitude > maxMag) {
maxMag = magnitude;
holdMagCount = 0;
}
else {
holdMagCount++;
if (holdMagCount > holdIterations) {
maxMag = magnitude;
}
}
sprintf(debugLine, "X%+4d Y%+4d M%3d",
maxLat, maxLong, maxMag);
lcd.setCursor(0,3);
lcd.print(debugLine);
numUpdates = 0;
if (DEBUG) {
sprintf(debugLine, "lat: %d (%d) long: %d (%d) mag: %d angle: ",
latForce, latForce+127, longForce, longForce+127, magnitude);
Serial.print(debugLine);
Serial.print(angle);
sprintf(debugLine, " low: %d vibraLow: %d relLowBound: ",
lowBound, vibraLow, relLowBound);
Serial.print(debugLine);
Serial.print(relLowBound);
sprintf(debugLine, " vibraHigh: %d, vibraLowDutyCycle: %d vibraHighDutyCycle: %d",
vibraHigh, vibraLowDutyCycle, vibraHighDutyCycle);
Serial.println(debugLine);
}
// DEL 7E 127
}
byte isHeader() {
byte theByte = -1;
if (Serial.available() == 0) {
return 0;
}
// read the incoming byte:
theByte = Serial.read();
if ( theByte == 0 ) {
if (DEBUG) {
Serial.println("got record start");
}
return 1;
}
else {
if (DEBUG) {
Serial.println("invalid record start");
}
return 0;
}
}
byte getByte() {
while (Serial.available() == 0) {
// do nothing
}
return Serial.read();
}
void blankLCD() {
lcd.clear();
lcd.setCursor(4,0);
lcd.print("Force Belt");
lcd.setCursor(0,3);
lcd.print("Simulator off");
}