const char maskSerial= B00000001;
const char maskStall = B00010000;
const char maskLimit = B00100000;
const char maskOverT = B00000001;
const char maskOverV = B00000010;
const char maskUnderV = B00000100;
const char maskShort = B00001000;
const char maskEmerg = B00010000;
long pauseBlink = 0;
int pauseLedStatus = LOW;
const int swPause = 2; // switch System Pause
const int swDirection = 3; // switch Dolly Direction
const int swUnitSelector = 24; // switch Display Unit Selector
const int swPedalCenter = 26; // switch Pedal Center Selector
long lastDebounceTime = 0; // the last time the output pin was toggled
long debounceDelay = 50; // required debounce delay
int lastSwPause = HIGH;
int lastSwDirection = HIGH;
int lastSwUnitSelector = HIGH;
int lastSwPedalCenter = HIGH;
int tmpReading;
int sensorValue = 0; // value read from the pedal Potentiometer
int sensorScaler = 0; // value read from the scaler Potentiometer
int sensorOffset = 0; // offset for the Potentiometer
int outputValue = 0; // value output to Motor
boolean Direction = true;
boolean Paused=false;
boolean Emergency=false;
int fFault = 0; // Fault Flag
int fStatus = 0; // Status Flag
String inputString; // a string to hold incoming data
boolean stringComplete = false;
int loopCnt = 0;
float dispVoltage1 = 0.0;
float dispMotAmps1 = 0.0;
float dispTemp1 = 0.0;
int dispCount1 = 0;
//=============== VOID SETUP ====================
void setup() {
Serial.begin(9600);
Serial.println("Initializing...");
pinMode(ledPower, OUTPUT);
pinMode(ledOverVolt, OUTPUT);
pinMode(ledUnderVolt, OUTPUT);
pinMode(ledOverTemp, OUTPUT);
pinMode(ledShort, OUTPUT);
pinMode(ledSerialCom, OUTPUT);
pinMode(ledStall, OUTPUT);
pinMode(ledErgencyStop, OUTPUT);
pinMode(ledLimitSwitch, OUTPUT);
pinMode(ledPause, OUTPUT);
pinMode(swPause, INPUT_PULLUP);
pinMode(swDirection, INPUT_PULLUP);
pinMode(swUnitSelector, INPUT_PULLUP);
pinMode(swPedalCenter, INPUT_PULLUP);
Serial.println("Led Check...");
Serial.println("Display Check...");
Serial1.begin(115200);
Serial2.begin(115200);
//Reserve 128 bytes for the serial input String
inputString.reserve(128);
stringComplete = false;
loopCnt = 0;
Serial.println("Ready!");
analogWrite(ledPower, 64);
}
//=============== VOID LOOP ====================
void loop() {
// ================================ Switch PAUSE
tmpReading = digitalRead(swPause);
if (tmpReading != lastSwPause)
lastDebounceTime = millis();
if ((millis() - lastDebounceTime) > debounceDelay) {
if(tmpReading==LOW)
Paused = true;
else
Paused = false;
}
lastSwPause = tmpReading;
// ================================ Switch Direction
tmpReading = digitalRead(swDirection);
if (tmpReading != lastSwDirection)
lastDebounceTime = millis();
if ((millis() - lastDebounceTime) > debounceDelay) {
if(tmpReading==LOW)
Direction = false;
else
Direction = true;
}
lastSwDirection = tmpReading;
// ================================= Center Potenziometer Switch
tmpReading = digitalRead(swPedalCenter);
if (tmpReading != lastSwPedalCenter)
lastDebounceTime = millis();
if ((millis() - lastDebounceTime) > debounceDelay) {
if(tmpReading==LOW)
sensorOffset = sensorValue - 512;
}
lastSwPedalCenter = tmpReading;
// ================================= read the analog in value:
sensorValue = analogRead(potPedal);
sensorScaler = analogRead(potRange);
int scalerValue = map(sensorScaler, 0, 1023, 0, 1002);
if(Direction)
outputValue = map(sensorValue-sensorOffset, 0, 1023, -1*scalerValue, scalerValue);
else
outputValue = map(sensorValue-sensorOffset, 0, 1023, scalerValue, -1*scalerValue);
if(outputValue <= -1000)
outputValue = -1000;
if(outputValue >= +1000)
outputValue = 1000;
if(outputValue >= -5 && outputValue <= 5)
outputValue = 0;
// ========================== Command the Motor & Get back information by the Board
if(Paused) {
outputValue = 0;
if(millis()-pauseBlink > 250) {
if (pauseLedStatus == LOW)
pauseLedStatus = HIGH;
else
pauseLedStatus = LOW;
digitalWrite(ledPause, pauseLedStatus);
pauseBlink = millis();
}
} else if(pauseLedStatus=HIGH) {
pauseLedStatus = LOW;
digitalWrite(ledPause, pauseLedStatus);
}
//=================================== Motor Control
Serial1.print("!G 1 " );
Serial1.println(outputValue);
if(loopCnt++ % 20) {
delay(10);
// ================================= FeedBack from Roboteq Board
Serial1.println("?V_?A_?C 1_?T_?FF_?FS\r" );
unsigned long recTime = millis();
while (Serial1.available() > 0 || millis()-recTime<50) {
char inChar = (char)Serial1.read(); // get the new byte:
inputString += inChar;
if(inputString.indexOf("FS=")<inputString.lastIndexOf('\r'))
stringComplete = true;
}
// ================================= Leds Management
if(stringComplete) {
//Serial.println(inputString); // Print entire string from controller
if(dispVoltage1 == 0.0 || abs(parseReceivedValue(inputString,"V=",":",":",10.0) - dispVoltage1) <= 10.0)
dispVoltage1 = parseReceivedValue(inputString,"V=",":",":",10.0);
//Serial.println(dispVoltage1);
if(parseReceivedValue(inputString,"A=","=",":",10.0) != -1.0)
dispMotAmps1 = parseReceivedValue(inputString,"A=","=",":",10.0);
//Serial.println(dispMotAmps1);
if(parseReceivedValue(inputString,"T=","=",":",1.0) != -1.0)
dispTemp1 = parseReceivedValue(inputString,"T=","=",":",1.0);
//Serial.println(dispTemp1);
if(parseReceivedValue(inputString,"C=","=",":",1.0) != -1.0)
dispCount1 = parseReceivedValue(inputString,"C=","=",":", 1.0);
Serial.println(dispCount1);
fFault = (int) parseReceivedValue(inputString,"FF=","=","\r",1.0);
fStatus = (int) parseReceivedValue(inputString,"FS=","=","\r",1.0);
/*
if(fFault & maskEmerg)
Emergency = true;
else
Emergency = false;
*/
digitalWrite(ledSerialCom, (fStatus & maskSerial));
digitalWrite(ledStall, (fStatus & maskStall));
digitalWrite(ledLimitSwitch, (fStatus & maskLimit));
//Serial.println(fStatus & maskSerial);
digitalWrite(ledOverTemp, (fFault & maskOverT));
digitalWrite(ledOverVolt, (fFault & maskOverV));
digitalWrite(ledUnderVolt, (fFault & maskUnderV));
digitalWrite(ledShort, (fFault & maskShort));
digitalWrite(ledErgencyStop, (fFault & maskEmerg));
inputString = "";
stringComplete = false;
}
loopCnt=0;
}
// wait 10 milliseconds before the next loop
// for the analog-to-digital converter to settle
// after the last reading:
delay(10); // Aqui estava 25ms
}
float parseReceivedValue(String input,String answerHeader, String startDelimiter, String endDelimiter, float divider) {
float retValue;
int startPos = input.indexOf(startDelimiter,input.indexOf(answerHeader)) + 1;
int endPos=input.indexOf(endDelimiter,startPos);
if(startPos==-1 || endPos==-1)
retValue = -1.0;
else
retValue = StrToFloat(input.substring(startPos,endPos))/divider;
return retValue;
}
float StrToFloat(String str){
char carray[str.length() + 1];
str.toCharArray(carray, sizeof(carray));
return atof(carray);
}
float StrToInt(String str){
char carray[str.length() + 1];
str.toCharArray(carray, sizeof(carray));
return atoi(carray);
}