Newer
Older
Copyright 2014 James LeRoy getSurreal
https://github.com/getSurreal/XV_Lidar_Controller
getSurreal
committed
Modified to add CRC checking - Doug Hilton, WD0UG November, 2015 mailto: six.speed (at) yahoo (dot) com
getSurreal
committed
Modified to add ShowErrors / HideErrors - DSH
Modified to add ShowAngles - DSH
Modified SerialCommand.h to increase # of characters allowed to be input and command length - DSH
#define SERIALCOMMAND_BUFFER 100 // ORIGINAL: 32 MODIFIED BY DSH
#define SERIALCOMMAND_MAXCOMMANDLENGTH 20 // ORIGINAL: 8
****************************************************************************************
NOTE: DON'T FORGET TO COPY THE NEW SerialCommand.h TO YOUR ARDUINO LIBRARY DIRECTORY!!!
****************************************************************************************
Modified to merge ShowAngles command into ShowAngle Command - DSH
Modified to add ShowInterval and HideInterval command (show interval, in milliseconds, every revolution, at angle=0
James LeRoy
committed
The F() macro in the Serial statements tells the compiler to keep your strings in PROGMEM
#include <TimerThree.h> // used for ultrasonic PWM motor control
const int N_ANGLES = 360; // # of angles (0..359)
const int SHOW_ALL_ANGLES = N_ANGLES; // value means 'display all angle data, 0..359'
getSurreal
committed
James LeRoy
committed
char version[6];
int motor_pwm_pin; // pin connected to mosfet for motor speed control
double rpm_setpoint; // desired RPM (uses double to be compatible with PID library)
double rpm_min;
double rpm_max;
double pwm_max; // max analog value. probably never needs to change from 1023
double pwm_min; // min analog pulse value to spin the motor
int sample_time; // how often to calculate the PID values
boolean motor_enable; // to spin the laser or not. No data when not spinning
boolean raw_data; // to retransmit the seiral data to the USB port
boolean show_dist; // controlled by ShowDist and HideDist commands
boolean show_rpm; // controlled by ShowRPM and HideRPM commands
James LeRoy
committed
unsigned int show_angle; // controlled by ShowAngle (0 - 359, 360 shows all)
getSurreal
committed
const byte EEPROM_ID = 0x05; // used to validate EEPROM initialized
unsigned long now;
unsigned long motor_check_timer = millis();
getSurreal
committed
unsigned long motor_check_interval = 200;
unsigned int rpm_err_thresh = 10; // 2 seconds (10 * 200ms) to shutdown motor with improper RPM and high voltage
unsigned int rpm_err = 0;
James LeRoy
committed
unsigned long curMillis;
unsigned long lastMillis = millis();
getSurreal
committed
// Added by DSH
const unsigned char COMMAND = 0xFA; // Start of new packet
const int INDEX_LO = 0xA0; // lowest index value
const int INDEX_HI = 0xF9; // highest index value
getSurreal
committed
const int N_DATA_QUADS = 4; // there are 4 groups of data elements
const int N_ELEMENTS_PER_QUAD = 4; // viz., 0=distance LSB; 1=distance MSB; 2=sig LSB; 3=sig MSB
getSurreal
committed
// Offsets to bytes within 'Packet'
const int OFFSET_TO_START = 0;
const int OFFSET_TO_INDEX = OFFSET_TO_START + 1;
getSurreal
committed
const int OFFSET_TO_SPEED_LSB = OFFSET_TO_INDEX + 1;
const int OFFSET_TO_SPEED_MSB = OFFSET_TO_SPEED_LSB + 1;
const int OFFSET_TO_4_DATA_READINGS = OFFSET_TO_SPEED_MSB + 1;
const int OFFSET_TO_CRC_L = OFFSET_TO_4_DATA_READINGS + (N_DATA_QUADS * N_ELEMENTS_PER_QUAD);
const int OFFSET_TO_CRC_M = OFFSET_TO_CRC_L + 1;
const int PACKET_LENGTH = OFFSET_TO_CRC_M + 1; // length of a complete packet
getSurreal
committed
// Offsets to the (4) elements of each of the (4) data quads
const int OFFSET_DATA_DISTANCE_LSB = 0;
const int OFFSET_DATA_DISTANCE_MSB = OFFSET_DATA_DISTANCE_LSB + 1;
const int OFFSET_DATA_SIGNAL_LSB = OFFSET_DATA_DISTANCE_MSB + 1;
const int OFFSET_DATA_SIGNAL_MSB = OFFSET_DATA_SIGNAL_LSB + 1;
int Packet[PACKET_LENGTH]; // an input packet
int ixPacket = 0; // index into 'Packet' array
const int VALID_PACKET = 0;
const int INVALID_PACKET = VALID_PACKET + 1;
getSurreal
committed
const byte INVALID_DATA_FLAG = (1 << 7); // Mask for byte 1 of each data quad "Invalid data"
boolean bShowInterval; // true = show time interval, once per revolution, at angle=0
getSurreal
committed
boolean aryAngles[N_ANGLES]; // true if we're supposed to display the corresponding angle
/* REF: https://github.com/Xevel/NXV11/wiki
The bit 7 of byte 1 seems to indicate that the distance could not be calculated.
It's interesting to see that when this bit is set, the second byte is always 80, and the values of the first byte seem to be
only 02, 03, 21, 25, 35 or 50... When it's 21, then the whole block is 21 80 XX XX, but for all the other values it's the
data block is YY 80 00 00 maybe it's a code to say what type of error ? (35 is preponderant, 21 seems to be when the beam is
interrupted by the supports of the cover) .
*/
const byte STRENGTH_WARNING_FLAG = (1 << 6); // Mask for byte 1 of each data quat "Strength Warning"
/*
The bit 6 of byte 1 is a warning when the reported strength is greatly inferior to what is expected at this distance.
This may happen when the material has a low reflectance (black material...), or when the dot does not have the expected
size or shape (porous material, transparent fabric, grid, edge of an object...), or maybe when there are parasitic
reflections (glass... ).
*/
const byte BAD_DATA_MASK = (INVALID_DATA_FLAG | STRENGTH_WARNING_FLAG);
getSurreal
committed
const byte eState_Find_COMMAND = 0; // 1st state: find 0xFA (COMMAND) in input stream
const byte eState_Build_Packet = eState_Find_COMMAND + 1; // 2nd state: build the packet
int eState = eState_Find_COMMAND;
PID rpmPID(&motor_rpm, &pwm_val, &xv_config.rpm_setpoint, xv_config.Kp, xv_config.Ki, xv_config.Kd, DIRECT);
uint8_t inByte = 0; // incoming serial byte
uint8_t motor_rph_high_byte = 0;
uint8_t motor_rph_low_byte = 0;
uint16_t aryDist[N_DATA_QUADS] = {0,0,0,0}; // thre are (4) distances, one for each data quad
// so the maximum distance is 16383 mm (0x3FFF)
uint16_t aryQuality[N_DATA_QUADS] = {0,0,0,0}; // same with 'quality'
uint16_t motor_rph = 0;
getSurreal
committed
uint16_t startingAngle = 0; // the first scan angle (of group of 4, based on 'index'), in degrees (0..359)
boolean bShowErrors = false; // true = show 3 kinds of errors
getSurreal
committed
const int ledPin = 11;
James LeRoy
committed
boolean ledState = LOW;
getSurreal
committed
// initialization (before 'loop')
if( xv_config.id != EEPROM_ID) { // verify EEPROM values have been initialized
initEEPROM();
}
pinMode(xv_config.motor_pwm_pin, OUTPUT);
getSurreal
committed
Serial.begin(115200); // USB serial
Serial1.begin(115200); // XV LDS data
getSurreal
committed
Timer3.initialize(30); // set PWM frequency to 32.768kHz
rpmPID.SetOutputLimits(xv_config.pwm_min,xv_config.pwm_max);
rpmPID.SetSampleTime(xv_config.sample_time);
rpmPID.SetTunings(xv_config.Kp, xv_config.Ki, xv_config.Kd);
rpmPID.SetMode(AUTOMATIC);
James LeRoy
committed
pinMode(ledPin, OUTPUT);
getSurreal
committed
eState = eState_Find_COMMAND;
for (ixPacket = 0; ixPacket < PACKET_LENGTH; ixPacket++) // Initialize
Packet[ixPacket] = 0;
setLowRPM(xv_config.rpm_min); // Make the motor go as slow as possible
//hideRaw(); // DSH ONLY!!!!!!!!!!!!
getSurreal
committed
// Main loop (forever)
byte aryInvalidDataFlag[N_DATA_QUADS] = {0,0,0,0}; // non-zero = INVALID_DATA_FLAG or STRENGTH_WARNING_FLAG is set
sCmd.readSerial(); // check for incoming serial commands
getSurreal
committed
if (Serial1.available() > 0) { // read byte from LIDAR and relay to USB
inByte = Serial1.read(); // get incoming byte:
if (xv_config.raw_data)
Serial.print(inByte, BYTE); // relay
getSurreal
committed
// Switch, based on 'eState':
// State 1: We're scanning for 0xFA (COMMAND) in the input stream
// State 2: Build a complete data packet
if (eState == eState_Find_COMMAND) { // flush input until we get COMMAND byte
if(inByte == COMMAND) {
eState++; // switch to 'build a packet' state
Packet[ixPacket++] = inByte; // store 1st byte of data into 'Packet'
}
else { // eState == eState_Build_Packet
Packet[ixPacket++] = inByte; // keep storing input into 'Packet'
if (ixPacket == PACKET_LENGTH) { // we've got all the input bytes, so we're done building this packet
if (eValidatePacket() == VALID_PACKET) { // Check packet CRC
startingAngle = processIndex(); // get the starting angle of this group (of 4), e.g., 0, 4, 8, 12, ...
processSpeed(); // process the speed
// process each of the (4) sets of data in the packet
for (int ix = 0; ix < N_DATA_QUADS; ix++) // process the distance
aryInvalidDataFlag[ix] = processDistance(ix);
for (int ix = 0; ix < N_DATA_QUADS; ix++) { // process the signal strength (quality)
aryQuality[ix] = 0;
if (aryInvalidDataFlag[ix] == 0)
processSignalStrength(ix);
}
if (xv_config.show_dist) { // the 'ShowDistance' command is active
for (int ix = 0; ix < N_DATA_QUADS; ix++) {
if (aryAngles[startingAngle + ix]) { // if we're supposed to display that angle
if (aryInvalidDataFlag[ix] & BAD_DATA_MASK) { // if LIDAR reported a data error...
if (bShowErrors) { // if we're supposed to show data errors...
Serial.print(F("A,"));
Serial.print(startingAngle + ix);
Serial.print(F(","));
getSurreal
committed
if (aryInvalidDataFlag[ix] & INVALID_DATA_FLAG)
Serial.println(F("-2"));
if (aryInvalidDataFlag[ix] & STRENGTH_WARNING_FLAG)
Serial.println(F("-3"));
}
}
else { // show clean data
Serial.print(F("A,"));
Serial.print(startingAngle + ix);
Serial.print(F(","));
Serial.print(int(aryDist[ix]));
Serial.print(F(","));
Serial.println(aryQuality[ix]);
}
} // if (aryAngles[startingAngle + ix])
} // for (int ix = 0; ix < N_DATA_QUADS; ix++)
getSurreal
committed
} // if (xv_config.show_dist)
} // if (eValidatePacket() == 0
else if (bShowErrors) { // we have encountered a CRC error
Serial.println(F("C,-1"));
getSurreal
committed
// initialize a bunch of stuff before we switch back to State 1
for (int ix = 0; ix < N_DATA_QUADS; ix++) {
aryDist[ix] = 0;
aryQuality[ix] = 0;
aryInvalidDataFlag[ix] = 0;
getSurreal
committed
}
for (ixPacket = 0; ixPacket < PACKET_LENGTH; ixPacket++) // clear out this packet
Packet[ixPacket] = 0;
ixPacket = 0;
eState = eState_Find_COMMAND; // This packet is done -- look for next COMMAND byte
} // if (ixPacket == PACKET_LENGTH)
} // if (eState == eState_Find_COMMAND)
} // if (Serial1.available() > 0)
getSurreal
committed
if (xv_config.motor_enable) {
if (pwm_val != pwm_last) {
Timer3.pwm(xv_config.motor_pwm_pin, pwm_val); // replacement for analogWrite()
pwm_last = pwm_val;
}
getSurreal
committed
} // if (xv_config.motor_enable)
} // loop
/*
* processIndex - Process the packet element 'index'
* index is the index byte in the 90 packets, going from A0 (packet 0, readings 0 to 3) to F9
* (packet 89, readings 356 to 359).
getSurreal
committed
* Uses: Packet
* ledState gets toggled if angle = 0
* ledPin = which pin the LED is connected to
* ledState = LED on or off
* xv_config.show_dist = true if we're supposed to show distance
* curMillis = milliseconds, now
* lastMillis = milliseconds, last time through this subroutine
* bShowInterval = true ==> display time interval once per revolution, at angle 0
getSurreal
committed
* Calls: digitalWrite() - used to toggle LED pin
* Serial.print
* Returns: The first angle (of 4) in the current 'index' group
*/
uint16_t processIndex() {
getSurreal
committed
uint16_t angle = 0;
uint16_t data_4deg_index = Packet[OFFSET_TO_INDEX] - INDEX_LO;
angle = data_4deg_index * N_DATA_QUADS; // 1st angle in the set of 4
if (angle == 0) {
if (ledState) {
ledState = LOW;
}
else {
ledState = HIGH;
}
digitalWrite(ledPin, ledState);
if (xv_config.show_dist) {
curMillis = millis();
if(bShowInterval) {
Serial.print(F("T,")); // Time Interval in ms since last complete revolution
Serial.println(curMillis - lastMillis);
lastMillis = curMillis;
}
} // if (angle == 0)
getSurreal
committed
return angle;
}
/*
* processSpeed- Process the packet element 'speed'
* speed is two-bytes of information, little-endian. It represents the speed, in 64th of RPM (aka value
* in RPM represented in fixed point, with 6 bits used for the decimal part).
getSurreal
committed
* Uses: Packet
* angle = if 0 then enable display of RPM and PWM
* xv_config.show_rpm = true if we're supposed to display RPM and PWM
* Calls: Serial.print
*/
void processSpeed() {
motor_rph_low_byte = Packet[OFFSET_TO_SPEED_LSB];
motor_rph_high_byte = Packet[OFFSET_TO_SPEED_MSB];
motor_rph = (motor_rph_high_byte << 8) | motor_rph_low_byte;
motor_rpm = float( (motor_rph_high_byte << 8) | motor_rph_low_byte ) / 64.0;
if (xv_config.show_rpm and startingAngle == 0) {
Serial.print(F("RPM,"));
Serial.print(motor_rpm);
Serial.print(F(",PWM,"));
Serial.println(pwm_val);
}
getSurreal
committed
}
/*
* Data 0 to Data 3 are the 4 readings. Each one is 4 bytes long, and organized as follows :
* byte 0 : <distance 7:0>
* byte 1 : <"invalid data" flag> <"strength warning" flag> <distance 13:8>
* byte 2 : <signal strength 7:0>
* byte 3 : <signal strength 15:8>
*/
/*
* processDistance- Process the packet element 'distance'
* Enter with: iQuad = which one of the (4) readings to process, value = 0..3
getSurreal
committed
* Uses: Packet
* dist[] = sets distance to object in binary: ISbb bbbb bbbb bbbb
* so maximum distance is 0x3FFF (16383 decimal) millimeters (mm)
getSurreal
committed
* Calls: N/A
* Exits with: 0 = okay
* Error: 1 << 7 = INVALID_DATA_FLAG is set
* 1 << 6 = STRENGTH_WARNING_FLAG is set
getSurreal
committed
*/
byte processDistance(int iQuad) {
getSurreal
committed
uint8_t dataL, dataM;
aryDist[iQuad] = 0; // initialize
int iOffset = OFFSET_TO_4_DATA_READINGS + (iQuad * N_DATA_QUADS) + OFFSET_DATA_DISTANCE_LSB;
// byte 0 : <distance 7:0> (LSB)
// byte 1 : <"invalid data" flag> <"strength warning" flag> <distance 13:8> (MSB)
dataM = Packet[iOffset + 1]; // get MSB of distance data + flags
if (dataM & BAD_DATA_MASK) // if either INVALID_DATA_FLAG or STRENGTH_WARNING_FLAG is set...
return dataM & BAD_DATA_MASK; // ...then return non-zero
dataL = Packet[iOffset]; // LSB of distance data
aryDist[iQuad] = dataL | ((dataM & 0x3F) << 8);
return 0; // okay
getSurreal
committed
}
/*
* processSignalStrength- Process the packet element 'signal strength'
* Enter with: iQuad = which one of the (4) readings to process, value = 0..3
getSurreal
committed
* Uses: Packet
* quality[] = signal quality
* Calls: N/A
*/
void processSignalStrength(int iQuad) {
getSurreal
committed
uint8_t dataL, dataM;
aryQuality[iQuad] = 0; // initialize
int iOffset = OFFSET_TO_4_DATA_READINGS + (iQuad * N_DATA_QUADS) + OFFSET_DATA_SIGNAL_LSB;
dataL = Packet[iOffset]; // signal strength LSB
dataM = Packet[iOffset + 1];
aryQuality[iQuad] = dataL | (dataM << 8);
getSurreal
committed
}
getSurreal
committed
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
/*
* eValidatePacket - Validate 'Packet'
* Enter with: 'Packet' is ready to check
* Uses: CalcCRC
* Exits with: 0 = Packet is okay
* Error: non-zero = Packet is no good
*/
byte eValidatePacket() {
unsigned long chk32;
unsigned long checksum;
const int bytesToCheck = PACKET_LENGTH - 2;
const int CalcCRC_Len = bytesToCheck / 2;
unsigned int CalcCRC[CalcCRC_Len];
byte b1a, b1b, b2a, b2b;
int ix;
for (int ix = 0; ix < CalcCRC_Len; ix++) // initialize 'CalcCRC' array
CalcCRC[ix] = 0;
// Perform checksum validity test
for (ix = 0; ix < bytesToCheck; ix += 2) // build 'CalcCRC' array
CalcCRC[ix / 2] = Packet[ix] + ((Packet[ix + 1]) << 8);
chk32 = 0;
for (ix = 0; ix < CalcCRC_Len; ix++)
chk32 = (chk32 << 1) + CalcCRC[ix];
checksum = (chk32 & 0x7FFF) + (chk32 >> 15);
checksum &= 0x7FFF;
b1a = checksum & 0xFF;
b1b = Packet[OFFSET_TO_CRC_L];
b2a = checksum >> 8;
b2b = Packet[OFFSET_TO_CRC_M];
if ((b1a == b1b) && (b2a == b2b))
return VALID_PACKET; // okay
getSurreal
committed
else
return INVALID_PACKET; // non-zero = bad CRC
getSurreal
committed
/*
* initEEPROM
*/
xv_config.id = 0x05;
strcpy(xv_config.version, "1.2.2");
xv_config.motor_pwm_pin = 9; // pin connected N-Channel Mosfet
xv_config.rpm_setpoint = 300; // desired RPM
xv_config.rpm_min = 200;
xv_config.rpm_max = 300;
xv_config.sample_time = 20;
xv_config.Kp = 2.0;
xv_config.Ki = 1.0;
xv_config.Kd = 0.0;
getSurreal
committed
xv_config.motor_enable = true;
xv_config.raw_data = true;
James LeRoy
committed
xv_config.show_dist = false;
xv_config.show_rpm = false;
getSurreal
committed
xv_config.show_angle = SHOW_ALL_ANGLES;
James LeRoy
committed
getSurreal
committed
/*
* initSerialCommands
*/
sCmd.addCommand("help", help);
sCmd.addCommand("Help", help);
sCmd.addCommand("ShowConfig", showConfig);
sCmd.addCommand("SaveConfig", saveConfig);
sCmd.addCommand("ResetConfig",initEEPROM);
sCmd.addCommand("SetRPM", setRPM);
sCmd.addCommand("SetKp", setKp);
sCmd.addCommand("SetKi", setKi);
sCmd.addCommand("SetKd", setKd);
sCmd.addCommand("SetSampleTime", setSampleTime);
sCmd.addCommand("ShowRPM", showRPM);
sCmd.addCommand("HideRPM", hideRPM);
sCmd.addCommand("ShowDist", showDist);
sCmd.addCommand("HideDist", hideDist);
sCmd.addCommand("ShowAngle", showAngle);
getSurreal
committed
sCmd.addCommand("HideAngle", hideDist);
sCmd.addCommand("MotorOff", motorOff);
sCmd.addCommand("MotorOn", motorOn);
sCmd.addCommand("ShowRaw", showRaw);
sCmd.addCommand("HideRaw", hideRaw);
getSurreal
committed
sCmd.addCommand("ShowErrors", showErrors);
sCmd.addCommand("HideErrors", hideErrors);
sCmd.addCommand("ShowInterval", showInterval);
sCmd.addCommand("HideInterval", hideInterval);
}
/*
* showInterval - enable display of Time interval (which happens once per revolution, at angle 0
*/
void showInterval() {
bShowInterval = true;
if (xv_config.show_dist == false) { // suppress activity message if we're executing 'show distance'
Serial.println(F(" "));
Serial.println(F("Showing time interval (ms per revolution)"));
}
}
/*
* hideInterval - suppress display of Time interval
*/
void hideInterval() {
bShowInterval = false;
if (xv_config.show_dist == false) { // suppress activity message if we're executing 'show distance'
Serial.println(F(" "));
Serial.println(F("Hiding time interval"));
}
getSurreal
committed
}
/*
* showErrors
*/
void showErrors() {
bShowErrors = true; // enable error display
if (xv_config.show_dist == false) { // suppress activity message if we're executing 'show distance'
Serial.println(F(" "));
Serial.println(F("Showing errors"));
}
getSurreal
committed
}
/*
* hideErrors
*/
void hideErrors() { // disable error display
bShowErrors = false;
if (xv_config.show_dist == false) { // suppress activity message if we're executing 'show distance'
Serial.println(F(" "));
Serial.println(F("Hiding errors"));
}
getSurreal
committed
/*
* showRPM
*/
James LeRoy
committed
xv_config.show_rpm = true;
if (xv_config.raw_data == true) {
hideRaw();
}
if (xv_config.show_dist == false) { // suppress activity message if we're executing 'show distance'
Serial.println(F(" "));
Serial.println(F("Showing RPM data"));
}
getSurreal
committed
/*
* hideRPM
*/
James LeRoy
committed
xv_config.show_rpm = false;
if (xv_config.show_dist == false) { // suppress activity message if we're executing 'show distance'
Serial.println(F(" "));
Serial.println(F("Hiding RPM data"));
}
void showDist() {
showDist2();
for (int ix = 0; ix < N_ANGLES; ix++)
aryAngles[ix] = true;
}
void showDist2() {
hideRaw();
xv_config.show_angle = SHOW_ALL_ANGLES;
if (xv_config.show_dist == false) { // suppress activity message if we're executing 'show distance'
Serial.println(F(" "));
Serial.println(F("Code,Angle,Distance(mm),Signal strength"));
xv_config.show_dist = true;
}
void hideDist() {
James LeRoy
committed
xv_config.show_dist = false;
for (int ix = 0; ix < N_ANGLES; ix++)
aryAngles[ix] = false;
xv_config.show_angle = 0;
if (xv_config.show_dist == false) { // suppress activity message if we're executing 'show distance'
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Hiding Distance data"));
getSurreal
committed
/*
* doShowAngle - Multi-angle range(s) implementation - DSH
getSurreal
committed
* Command: ShowAngles ddd, ddd-ddd, etc.
* Enter with: N/A
* Uses: aryAngles (an array of 360 booleans) is set to appropriate values
* Calls: showDist
* Exits with: N/A
* TEST THIS STRING: ShowAngles 16-20, 300-305, 123-124, 10
*/
void showAngle() {
getSurreal
committed
double sVal = 0.0;
char c, *arg;
boolean syntax_error = false;
int doing_from_to, from, to, ix, lToken, n_groups = 0;
for (ix = 0; ix < N_ANGLES; ix++) // initialize
aryAngles[ix] = false;
doing_from_to = 0; // state = doing 'from'
// Make sure that there is at least 1 angle or group of angles present
do {
arg = sCmd.next(); // get the next token
if (arg == NULL) { // it's empty -- just exit
sCmd.readSerial();
arg = sCmd.next();
break;
}
// see if the token has an embedded "-", meaning from - to
lToken = strlen(arg); // get the length of the current token
for (ix = 0; ix < lToken; ix++) {
c = arg[ix];
if (c == ',') { // optional trailing comma
getSurreal
committed
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
doing_from_to = 0;
break;
}
else if (c == '-') { // optional '-' means "from - to"
to = 0;
doing_from_to = 1; // from now on, we're doing 'to'
}
else if (c == ' ') { // ignore blanks
Serial.println(F("{ }"));
}
else if ((c >= '0') && (c <= '9')) {
if (doing_from_to == 0) {
from *= 10;
from += c - '0';
to = from; // default to = from
n_groups++; // count the number of active groups (s/b >= 1)
}
else {
to *= 10;
to += c - '0';
}
}
else {
syntax_error = true;
n_groups = 0;
break;
}
} // for (ix = 0; ix < lToken; ix++)
// validate 'from' and 'to' and set 'aryAngles' with correct values
if ((from >= 0) && (from < N_ANGLES) && (to >= 0) && (to < N_ANGLES)) {
if (to >= from) {
for (ix = from; ix <= to; ix++) {
aryAngles[ix] = true;
}
}
else {
syntax_error = true;
break;
}
}
else {
syntax_error = true;
break;
}
from = 0;
to = 0;
doing_from_to = 0;
} // do
while (arg != NULL);
if (n_groups == 0)
syntax_error = true;
// Handle syntax errors
if (syntax_error) {
Serial.println(F(" "));
Serial.println(F("Incorrect syntax"));
Serial.println(F(" Example: ShowAngle 0, 15-30, 45-50, 10"));
Serial.println(F(" Example: ShowAngle 0-359 to show all angles."));
Serial.println(F("Notes: Use a space after each comma"));
Serial.println(F(" No particular order is required"));
Serial.println(F(" In a from-to pair, the 1st value must be lowest. From-to pairs can overlap ranges."));
getSurreal
committed
}
else { // no errors detected, display the angles and start
// We're ready to process multiple angles
Serial.println(F(""));
Serial.print(F("Angles:"));
xv_config.show_angle == SHOW_ALL_ANGLES; // start by assuming 'all angles'
getSurreal
committed
for (int ix = 0; ix < N_ANGLES; ix++) { // display the angle array
if(aryAngles[ix]) {
Serial.print(ix, DEC);
Serial.print(F(","));
}
else
xv_config.show_angle == 0; // disable 'display all angles'
getSurreal
committed
}
Serial.println(F(""));
showDist2();
} // if not (syntax_error)
getSurreal
committed
}
getSurreal
committed
xv_config.motor_enable = false;
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Motor off"));
}
void motorOn() {
getSurreal
committed
xv_config.motor_enable = true;
Timer3.pwm(xv_config.motor_pwm_pin, pwm_val);
rpm_err = 0; // reset rpm error
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Motor on"));
void motorCheck() { // Make sure the motor RPMs are good else shut it down
now = millis();
getSurreal
committed
if (now - motor_check_timer > motor_check_interval){
if ((motor_rpm < xv_config.rpm_min or motor_rpm > xv_config.rpm_max) and pwm_val > 1000) {
rpm_err++;
}
else {
rpm_err = 0;
}
if (rpm_err > rpm_err_thresh) {
getSurreal
committed
motorOff();
ledState = LOW;
digitalWrite(ledPin, ledState);
}
motor_check_timer = millis();
}
}
void hideRaw() {
xv_config.raw_data = false;
//Serial.println(F(" "));
//Serial.println(F("Raw lidar data disabled"));
void showRaw() {
xv_config.raw_data = true;
hideDist();
hideRPM();
//Serial.println(F(" "));
//Serial.println(F("Lidar data enabled"));
void setLowRPM(double sVal) {
xv_config.rpm_setpoint = sVal;
}
char *arg;
boolean syntax_error = false;
arg = sCmd.next();
if (arg != NULL) {
if (sVal < xv_config.rpm_min) {
sVal = xv_config.rpm_min;
James LeRoy
committed
Serial.println(F(" "));
Serial.println(xv_config.rpm_min);
if (sVal > xv_config.rpm_max) {
sVal = xv_config.rpm_max;
James LeRoy
committed
Serial.println(F(" "));
Serial.println(xv_config.rpm_max);
}
else {
syntax_error = true;
}
arg = sCmd.next();
if (arg != NULL) {
syntax_error = true;
}
if (syntax_error) {
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Incorrect syntax. Example: SetRPM 200"));
getSurreal
committed
Serial.print(F("Old RPM setpoint:"));
Serial.println(xv_config.rpm_setpoint);
getSurreal
committed
//Serial.println(F(" "));
James LeRoy
committed
Serial.print(F("New RPM setpoint: "));
Serial.println(sVal);
}
}
void setKp() {
char *arg;
boolean syntax_error = false;
arg = sCmd.next();
if (arg != NULL) {
}
else {
syntax_error = true;
}
arg = sCmd.next();
if (arg != NULL) {
syntax_error = true;
}
if (syntax_error) {
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Incorrect syntax. Example: SetKp 1.0"));
James LeRoy
committed
Serial.println(F(" "));
Serial.print(F("Setting Kp to: "));
Serial.println(sVal);
xv_config.Kp = sVal;
rpmPID.SetTunings(xv_config.Kp, xv_config.Ki, xv_config.Kd);
void setKi() {
char *arg;
boolean syntax_error = false;
arg = sCmd.next();
if (arg != NULL) {
}
else {
syntax_error = true;
}
arg = sCmd.next();
if (arg != NULL) {
syntax_error = true;
}
if (syntax_error) {
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Incorrect syntax. Example: SetKi 0.5"));
James LeRoy
committed
Serial.println(F(" "));
Serial.print(F("Setting Ki to: "));
xv_config.Ki = sVal;
rpmPID.SetTunings(xv_config.Kp, xv_config.Ki, xv_config.Kd);
}
}
void setKd() {
char *arg;
boolean syntax_error = false;
arg = sCmd.next();
if (arg != NULL) {
}
else {
syntax_error = true;
}
arg = sCmd.next();
if (arg != NULL) {
syntax_error = true;
}
if (syntax_error) {
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Incorrect syntax. Example: SetKd 0.001"));
James LeRoy
committed
Serial.println(F(" "));
Serial.print(F("Setting Kd to: "));
xv_config.Kd = sVal;
rpmPID.SetTunings(xv_config.Kp, xv_config.Ki, xv_config.Kd);
void setSampleTime() {
double sVal = 0.0;
char *arg;
boolean syntax_error = false;
arg = sCmd.next();
if (arg != NULL) {
sVal = atoi(arg); // Converts a char string to an integer
}
else {
syntax_error = true;
}
arg = sCmd.next();
if (arg != NULL) {
syntax_error = true;
}
if (syntax_error) {
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Incorrect syntax. Example: SetSampleTime 20"));
}
else {
James LeRoy
committed
Serial.println(F(" "));
Serial.print(F("Setting Sample time to: "));
xv_config.sample_time = sVal;
rpmPID.SetSampleTime(xv_config.sample_time);
}
}
if (xv_config.raw_data == true) {
hideRaw();
}
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F(" "));
Serial.print(F("XV Lidar Controller Firmware Version "));
Serial.println(xv_config.version);
getSurreal
committed
Serial.print(F("GetSurreal.com *"));
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F(" "));
Serial.println(F("List of available commands (case sensitive)"));
Serial.println(F(" HideRaw - Stop outputting the raw data from the lidar"));
Serial.println(F(" ShowRaw - Enable the output of the raw lidar data (default)"));
Serial.println(F(" ShowDist - Show the distance data"));
Serial.println(F(" HideDist - Hide the distance data"));
Serial.println(F(" ShowAngle - Show distance data for a multiple angles (Ex: ShowAngle 0, 15-30, 45-50, 10)"));
Serial.println(F(" HideAngle - Hide distance data for all angles"));
Serial.println(F(" ShowErrors - Show all error messages"));
Serial.println(F(" HideErrors - Hide error messages"));
James LeRoy
committed
Serial.println(F(" ShowConfig - Show the running configuration"));
Serial.println(F(" SaveConfig - Save the running configuration to EEPROM"));
Serial.println(F(" ResetConfig - Restore the original configuration"));
Serial.println(F(" SetRPM - Set the desired rotation speed (min: 180, max: 349)"));
James LeRoy
committed
Serial.println(F(" SetKp - Set the proportional gain"));
Serial.println(F(" SetKi - Set the integral gain"));
Serial.println(F(" SetKd - Set the derivative gain"));
Serial.println(F(" SetSampleTime - Set the frequency the PID is calculated (ms)"));
Serial.println(F(" ShowRPM - Show the rotation speed"));
Serial.println(F(" HideRPM - Hide the rotation speed"));
Serial.println(F(" ShowInterval - Show time interval per revolution in ms, at angle=0"));
Serial.println(F(" HideInterval - Hide time interval"));
James LeRoy
committed
Serial.println(F(" MotorOff - Stop spinning the lidar"));
Serial.println(F(" MotorOn - Enable spinning of the lidar"));
Serial.println(F(" "));
Serial.println(F("Output comma-separated format:"));
Serial.println(F(" A,<Angle>,<Distance in mm>,<Strength>"));
Serial.println(F(" C,-1 means CRC error was generated by LIDAR"));
Serial.println(F(" T,<Time interval in milliseconds>"));
Serial.println(F(" "));
Serial.println(F("Errors:"));
Serial.println(F(" -1 = CRC Error"));
Serial.println(F(" -2 = LIDAR reports Invalid data for this angle"));
Serial.println(F(" -3 = LIDAR reports Poor signal strength for this angle"));
James LeRoy
committed
Serial.println(F(" "));
void showConfig() {
if (xv_config.raw_data == true) {
hideRaw();
}
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F(" "));
Serial.print(F("XV Lidar Controller Firmware Version "));
Serial.println(xv_config.version);
Serial.print(F("GetSurreal.com"));
Serial.println(F(" "));
Serial.println(F(" "));
Serial.print(F("PWM pin: "));
Serial.println(xv_config.motor_pwm_pin);
James LeRoy
committed
Serial.print(F("Target RPM: "));
Serial.println(xv_config.rpm_setpoint);
James LeRoy
committed
Serial.print(F("Max PWM: "));
Serial.println(xv_config.pwm_max);
James LeRoy
committed
Serial.print(F("Min PWM: "));
Serial.println(xv_config.pwm_min);
James LeRoy
committed
Serial.print(F("PID Kp: "));
Serial.println(xv_config.Kp);
James LeRoy
committed
Serial.print(F("PID Ki: "));
Serial.println(xv_config.Ki);
James LeRoy
committed
Serial.print(F("PID Kd: "));
Serial.println(xv_config.Kd);
James LeRoy
committed
Serial.print(F("SampleTime: "));
getSurreal
committed
James LeRoy
committed
Serial.print(F("Motor Enable: "));
getSurreal
committed
Serial.println(xv_config.motor_enable);
James LeRoy
committed
Serial.print(F("Show Raw Data: "));
Serial.println(xv_config.raw_data);
James LeRoy
committed
Serial.print(F("Show Dist Data: "));
Serial.println(xv_config.show_dist);
Serial.print(F("Show RPM Data: "));
Serial.println(xv_config.show_rpm);
//Serial.print(F("Show Angle: "));
//Serial.println(xv_config.show_angle);
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F(" "));
void saveConfig() {
EEPROM_writeAnything(0, xv_config);
Serial.println(F("Config Saved."));