Newer
Older
getSurreal
committed
XV Lidar Controller v1.3.0
Copyright 2014 James LeRoy getSurreal
https://github.com/getSurreal/XV_Lidar_Controller
getSurreal
committed
NOTE: USES THE ARDUINO 1.6.6 DEVELOPMENT PLATFORM
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
getSurreal
committed
Modified to add show multiple angles - 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
getSurreal
committed
#define SERIALCOMMAND_MAXCOMMANDLENGTH 20 // ORIGINAL:
****************************************************************************************
NOTE: DON'T FORGET TO COPY THE NEW SerialCommand.h TO YOUR ARDUINO LIBRARY DIRECTORY!!!
****************************************************************************************
getSurreal
committed
Modified to add ShowCSV and HideCSV command - DSH
Modified EEPROM configuration to add new values and delete old ones - DSH
Modified to add ShowAll/HideAll command - DSH
Modified to let ShowInterval and ShowRPM run without ShowAngle
Modified to check that xv_config is not too big for EEPROM
Modified to delete ShowDist/HideDist commands, since that functionality is subsumed by ShowAngle(s)
getSurreal
committed
James LeRoy
committed
The F() macro in the Serial statements tells the compiler to keep your strings in PROGMEM
getSurreal
committed
The Teensy 20. board (REF: http://www.pjrc.com/teensy/index.html)
Processor: ATMEGA32U4 8-bit AVR 16MHz
Flash memory: 32256
RAM memory: 2560
EEPROM: 1024
I/O: 25, 5 Volt
Analog In: 12
PWM: 7
UART,I2C,SPI: 1,1,1
#include <TimerThree.h> // used for ultrasonic PWM motor control
getSurreal
committed
// ******************************************************************
// WARNING: If you change 'xv_config', you must change '_EEPROM_ID' *
// When it is changed, the EEPROM data will be initialized *
// ******************************************************************
const byte _EEPROM_ID = 0x07; // used to validate EEPROM initialized (Current as of 26NOV2015 - DSH)
const byte _EEPROM_SAFETY_CHECK = (-1 - _EEPROM_ID); // used to validate that we haven't written past the end of the EEPROM
// You may change 'szCURRENT_EEPROM_VERSION' to indicate a new software version, but it is not checked, just displayed
const char szCURRENT_EEPROM_VERSION[6] = "1.3.0"; // as of 24NOV2015 - DSH
const int _N_ANGLES = 360; // # of angles in a circle (0..359)
getSurreal
committed
// These constants will let us read and write individual variables to the EEPROM - DSH
const byte _BITS_PER_BYTE = 8;
const byte _MASK_BYTE = (1 << _BITS_PER_BYTE) - 1; // (value: 0xFF)
const unsigned int _MASK_WORD_LSB = _MASK_BYTE; // Mask off bottom 8 bits of INTEGER (value:0xFF)
const unsigned int _MASK_WORD_MSB = _MASK_BYTE << _BITS_PER_BYTE; // Mask off top 8 bits of INTEGER (value:0xFF00)
const int _LEN_BYTE = 1;
const int _LEN_BOOLEAN = 1;
const int _LEN_INTEGER = 2;
const int _LEN_DOUBLE = 4;
/*
* An enumerated list of "What we're displaying" commands
* WARNING: IF YOU ADD A VALUE, FIX VARIOUS swich (eOUT) STATEMENTS IN THE CODE BELOW!!!
* WARNING: MUST BE LESS THAN 256 VALUES (masked by 0xFF)
*/
const byte _eOUT_UNKNOWN = 0; // Unknown or uninitialized (value: 0)
const byte _eOUT_RAW = _eOUT_UNKNOWN + 1; // ShowRaw = retransmit the seiral data to the USB port (value: 1)
const byte _eOUT_ANGLE = _eOUT_RAW + 1; // ShowAngle = display angle data (value: 2)
const byte _eOUT_MAX = _eOUT_ANGLE + 1; // WARNING: This value must fit in 8 bits (range: 0-255) (value: 3)
getSurreal
committed
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
/*
* An enumerated list of special output requirements, e.g., Show Errors, Show RPM, Show Interval, Show CSV format
* NOTE: These are bit-values, so they can be individually set/cleared individually
*/
const unsigned int _eSHOW_RPM = 0; // Show R.P.M.
const unsigned int _eSHOW_CSV = _eSHOW_RPM + 1; // Show in .csv format
const unsigned int _eSHOW_ERRORS = _eSHOW_CSV + 1; // Show Errors
const unsigned int _eSHOW_INTERVAL = _eSHOW_ERRORS + 1; // Show Interval
const unsigned int _eSHOW_MAX = _eSHOW_INTERVAL + 1; // WARNING: This value is a bit shift number, so it must be 0-7 (fits into top byte of 'eOUT')
/*
* The top 8-bits of eOUT indicate special output requirements
*/
const unsigned int _OUT_MOD_BASE = 1 << _BITS_PER_BYTE; // Start the display modifiers in the top 8-bits (value: 0x100)
/* MSB LSB
7 6 5 4 3 2 1 0 7 6 5 4 3 2 1 0
x x x x x x x x n n n n n n n n
| | | | | | | + RPM
| | | | | | + CSV
| | | | } + ERROR
| | | | + Interval
| | | + RFU 1
| | + RFU 2
| + RFU 3
+ RFU 4
*/
// NOTE: if you add or change these, don't forget to fix up 'showRaw'
const unsigned int _OUT_MOD_SHOW_RPM = _OUT_MOD_BASE; // 1 = show RPM (value: 0x100) manipulated by showRPM/hideRPM
const unsigned int _OUT_MOD_SHOW_CSV = _OUT_MOD_SHOW_RPM * 2; // 1 = show CSV format (value: 0x200)
const unsigned int _OUT_MOD_SHOW_ERRORS = _OUT_MOD_SHOW_CSV * 2; // 1 = show errors (value: 0x400)
const unsigned int _OUT_MOD_SHOW_INTERVAL = _OUT_MOD_SHOW_ERRORS * 2; // 1 = show interval (value: 0x800)
const unsigned int _OUT_MOD_SHOW_RFU_1 = _OUT_MOD_SHOW_INTERVAL * 2; // 1 = reserved future use 1 (value: 0x1000)
const unsigned int _OUT_MOD_SHOW_RFU_2 = _OUT_MOD_SHOW_RFU_1 * 2; // 1 = reserved future use 2 (value: 0x2000)
const unsigned int _OUT_MOD_SHOW_RFU_3 = _OUT_MOD_SHOW_RFU_2 * 2; // 1 = reserved future use 3 (value: 0x4000)
const unsigned int _OUT_MOD_SHOW_RFU_4 = _OUT_MOD_SHOW_RFU_3 * 2; // 1 = reserved future use 4 (value: 0x8000)
#define _MASK_eOUT_SHOW _MASK_WORD_LSB // mask 'eOUT' for what we're showing (value:0xFF)
#define _MASK_eOUT_MODIFIER _MASK_WORD_MSB // mask 'eOUT' for display modifiers (value:0xFF00)
const int _EE_LEN_ID = _LEN_BYTE; // value:1
const int _EE_LEN_VERSION = 6; // value:6 (arbitrary, but currently contains, e.g., string "1.3.0" (plus 0-terminator)
const int _EE_LEN_MOTOR_PWM_PIN = _LEN_BYTE; // value:1
const int _EE_LEN_RPM_SETPOINT = _LEN_DOUBLE; // value:4
const int _EE_LEN_RPM_MIN = _LEN_DOUBLE; // value:4
const int _EE_LEN_RPM_MAX = _LEN_DOUBLE; // value:4
const int _EE_LEN_PWM_MAX = _LEN_DOUBLE; // value:4
const int _EE_LEN_PWM_MIN = _LEN_DOUBLE; // value:4
const int _EE_LEN_SAMPLE_TIME = _LEN_INTEGER; // value:2
const int _EE_LEN_Kp = _LEN_DOUBLE; // value:4
const int _EE_LEN_Ki = _LEN_DOUBLE; // value:4
const int _EE_LEN_Kd = _LEN_DOUBLE; // value:4
const int _EE_LEN_MOTOR_ENABLE = _LEN_BOOLEAN; // value:1
const int _EE_LEN_SHOW_ALL_ANGLES = _LEN_BOOLEAN; // value:1
const int _EE_LEN_eOUT = _LEN_INTEGER; // value:2
const int _EE_LEN_aryAngles = _N_ANGLES * _LEN_BOOLEAN; // value:360
const int _EE_LEN_SAFETY = _LEN_BYTE; // value:1
const int _EE_OFFSET_ID = 0; // value:0
const int _EE_OFFSET_VERSION = _EE_OFFSET_ID + _EE_LEN_ID; // value:1
const int _EE_OFFSET_MOTOR_PWM_PIN = _EE_OFFSET_VERSION + _EE_LEN_VERSION ; // value:7
const int _EE_OFFSET_RPM_SETPOINT = _EE_OFFSET_MOTOR_PWM_PIN + _EE_LEN_MOTOR_PWM_PIN ; // value:8
const int _EE_OFFSET_RPM_MIN = _EE_OFFSET_RPM_SETPOINT + _EE_LEN_RPM_SETPOINT ; // value:12
const int _EE_OFFSET_RPM_MAX = _EE_OFFSET_RPM_MIN + _EE_LEN_RPM_MIN ; // value:16
const int _EE_OFFSET_PWM_MAX = _EE_OFFSET_RPM_MAX + _EE_LEN_RPM_MAX ; // value:20
const int _EE_OFFSET_PWM_MIN = _EE_OFFSET_PWM_MAX + _EE_LEN_PWM_MAX ; // value:24
const int _EE_OFFSET_SAMPLE_TIME = _EE_OFFSET_PWM_MIN + _EE_LEN_PWM_MIN ; // value:28
const int _EE_OFFSET_Kp = _EE_OFFSET_SAMPLE_TIME + _EE_LEN_SAMPLE_TIME ; // value:30
const int _EE_OFFSET_Ki = _EE_OFFSET_Kp + _EE_LEN_Kp ; // value:34
const int _EE_OFFSET_Kd = _EE_OFFSET_Ki + _EE_LEN_Ki ; // value:38
const int _EE_OFFSET_MOTOR_ENABLE = _EE_OFFSET_Kd + _EE_LEN_Kd ; // value:42
const int _EE_OFFSET_SHOW_ALL_ANGLES = _EE_OFFSET_MOTOR_ENABLE + _EE_LEN_MOTOR_ENABLE ; // value:43
const int _EE_OFFSET_eOUT = _EE_OFFSET_SHOW_ALL_ANGLES + _EE_LEN_SHOW_ALL_ANGLES ; // value:44
const int _EE_OFFSET_aryAngles = _EE_OFFSET_eOUT + _EE_LEN_eOUT ; // value:46
const int _EE_OFFSET_SAFETY = _EE_OFFSET_aryAngles + _EE_LEN_aryAngles ; // value:406
const int _EEPROM_Config_LEN = _EE_OFFSET_SAFETY + _EE_LEN_SAFETY ; // value:407
/*
* Default values for EEPROM
*/
const byte _DEFAULT_motor_pwm_pin = 9; // pin connected N-Channel Mosfet
const double _DEFAULT_rpm_setpoint = 200; // desired RPM (arrived at this value by testing)
const double _DEFAULT_rpm_min = 200;
const double _DEFAULT_rpm_max = 300;
const double _DEFAULT_pwm_min = 100;
const int _DEFAULT_pwm_max = 1023;
const int _DEFAULT_sample_time = 20;
const double _DEFAULT_Kp = 2.0; // PID tuning values
const double _DEFAULT_Ki = 1.0;
const double _DEFAULT_Kd = 0.0;
const boolean _DEFAULT_bMotorEnable = true; // default is to turn the motor ON
const boolean _DEFAULT_bShowAllAngles = false; // default
const byte _DEFAULT_eOUT = _eOUT_RAW; // default to "RAW" display
const boolean _DEFAULT_bANGLE = false; // default for entries in 'aryAngle'
const double _DEFAULT_pwm_val = 500; // 50% duty cycle
const unsigned long _DEFAULT_motor_check_interval = 200;
const unsigned int _DEFAULT_rpm_err_thresh = 10;
/*
* Define the structure of the record that gets written to the EEPROM
*/
getSurreal
committed
byte id; // value == _EEPROM_ID
char szVersion[_EE_LEN_VERSION]; // e.g., "1.3.0" plus 0-terminator
byte motor_pwm_pin; // pin connected to mosfet for motor speed control, e.g., 9
double rpm_setpoint; // desired RPM (uses double to be compatible with PID library)
getSurreal
committed
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
getSurreal
committed
boolean bMotorEnable; // true = spin the motor. NOTE: No data is available when not spinning
boolean bShowAllAngles; // true = shows all angles
unsigned int eOUT; // What we're displaying now (2 8-bit nibbles)
// Bottom 8-bits = a value indicating what we're supposed to print, i.e., { unknown; raw; distance; angle }
// Top 8-bits = a bit-mask with display modifiers, e.g., {show RPM; show CSV format; show errors; show interval}
boolean aryAngles[_N_ANGLES]; // each entry = true if we're supposed to display the corresponding angle
byte safety; // a value to make sure that EEPROM data hasn't been overwritten
getSurreal
committed
double pwm_val = _DEFAULT_pwm_val; // start with ~50% power
unsigned long now;
unsigned long motor_check_timer = millis();
getSurreal
committed
unsigned long motor_check_interval = _DEFAULT_motor_check_interval;
unsigned int rpm_err_thresh = _DEFAULT_rpm_err_thresh; // 2 seconds (10 * 200ms) to shutdown motor with improper RPM and high voltage
James LeRoy
committed
unsigned long curMillis;
unsigned long lastMillis = millis();
getSurreal
committed
// Some Packet-related constants (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'
getSurreal
committed
const int _OFFSET_TO_START = 0;
const int _OFFSET_TO_INDEX = _OFFSET_TO_START + 1;
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
getSurreal
committed
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;
getSurreal
committed
getSurreal
committed
int Packet[_PACKET_LENGTH]; // an input packet
int ixPacket = 0; // index into 'Packet' array
const int _VALID_PACKET = 0;
const int _IN_VALID_PACKET = _VALID_PACKET + 1;
const byte _INVALID_DATA_FLAG = (1 << 7); // Mask for byte 1 of each data quad "Invalid data"
getSurreal
committed
getSurreal
committed
boolean bEEPROM_Initialized = false; // true if EEPROM was upgraded to latest software version
getSurreal
committed
/* 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) .
*/
getSurreal
committed
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... ).
*/
getSurreal
committed
const byte _BAD_DATA_MASK = (_INVALID_DATA_FLAG | _STRENGTH_WARNING_FLAG);
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;
getSurreal
committed
uint16_t aryDist[_N_DATA_QUADS] = {0,0,0,0}; // thre are (4) distances, one for each data quad
// NOTE: 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)
getSurreal
committed
int iCurrentAngle = 0;
SerialCommand sCmd; // create an instance of the serial device handler object
getSurreal
committed
const int ledPin = 11; // blink the LED
boolean ledState = LOW; // Toggle LOW/HIGH
byte aryInvalidDataFlag[_N_DATA_QUADS] = {0,0,0,0}; // non-zero = _INVALID_DATA_FLAG or _STRENGTH_WARNING_FLAG is set
int eShow; // What to display {UNKNOWN; RAW; DISTANCE; ANGLE}
int oldAngle = 0;
James LeRoy
committed
getSurreal
committed
// initialization (before 'loop')
getSurreal
committed
Serial.begin(115200); // USB serial
// Software consistency checks (catchable programmer mistakes)
if ((_eOUT_MAX != 3) || (_eOUT_MAX > _MASK_BYTE)) { // limit decimal value to what will fit in 1 byte
getSurreal
committed
Serial.println(F("SOFTWARE ERROR: _eOUT_MAX invalid. HALT!"));
while (1); // HANG!!!!!!
getSurreal
committed
if (_eSHOW_MAX >= _BITS_PER_BYTE) { // limit binary value to what can be expressed in 8-bits
Serial.println(F("SOFTWARE ERROR: _eSHOW_MAX invalid. HALT!"));
while (1); // HANG!!!!
}
if (_EEPROM_Config_LEN >= _EEPROM_SIZE) {
Serial.println(F("SOFTWARE ERROR: _EEPROM_Config_LEN >= _EEPROM_SIZE. HALT!"));
while (1); // HANG!!!!
}
getSurreal
committed
EEPROM_readAnything(_EE_OFFSET_ID, xv_config); // read the saved EEPROM configuration
if(xv_config.id != _EEPROM_ID) { // If EEPROM ID has changed...
Serial.println(F("EEPROM version is different from last time. Initializing EEPROM"));
initEEPROM(); // ...initialize EEPROM to new layout
//showConfig(); // for DEBUGGING: display EEPROM configuration data
}
pinMode(xv_config.motor_pwm_pin, OUTPUT); // set the pin that drives the LIDAR motor
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; // Initialize the 'state' for the main loop
for (ixPacket = 0; ixPacket < _PACKET_LENGTH; ixPacket++) // Initialize
getSurreal
committed
Packet[ixPacket] = 0;
getSurreal
committed
ixPacket = 0;
if (xv_config.bMotorEnable == false) // Turn on the motor
forceMotorOn(); // enable the motor
forceRaw(); // Default mode = 'show raw data'
getSurreal
committed
// Main loop (forever)
void loop() {
sCmd.readSerial(); // check for incoming serial commands
if (Serial1.available() > 0) { // If a byte is available...
inByte = Serial1.read(); // ...fetch incoming byte
eShow = (xv_config.eOUT & _MASK_eOUT_SHOW); // what are we supposed to display?
if (eShow == _eOUT_RAW) // if we're doing 'RAW' data...
Serial.print(inByte, BYTE); // ... echo input to output
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
getSurreal
committed
if (eState == _eState_Find_COMMAND) { // flush input until we receive a COMMAND (value: 0xFA)
if(inByte == _COMMAND) {
eState++; // switch to 'build a packet' state
Packet[ixPacket++] = inByte; // store 1st byte of data into 'Packet'
getSurreal
committed
}
getSurreal
committed
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
getSurreal
committed
for (int ix = 0; ix < _N_DATA_QUADS; ix++) // process the distance
aryInvalidDataFlag[ix] = processDistance(ix);
getSurreal
committed
for (int ix = 0; ix < _N_DATA_QUADS; ix++) { // process the signal strength (quality)
aryQuality[ix] = 0;
getSurreal
committed
if (aryInvalidDataFlag[ix] == 0) {
processSignalStrength(ix);
getSurreal
committed
} // if (aryInvalidDataFlag[ix] == 0)
} // for (int ix = 0; ix < _N_DATA_QUADS; ix++)
352
353
354
355
356
357
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
if ((eShow == _eOUT_UNKNOWN) || (eShow == _eOUT_ANGLE)) {
for (int ix = 0; ix < _N_DATA_QUADS; ix++) {
// We can't count on being able to display speed data at angle = 0
// so we look for the angle to go from a high value to a lower value
// which is safe to assume that it's either 0 or the closest value that is valid
iCurrentAngle = startingAngle + ix; // convert unsigned to signed integer for compare
if (iCurrentAngle < oldAngle) // the angle has gone from, e.g., 359 to 0
if (xv_config.eOUT & _OUT_MOD_SHOW_RPM) // if we're supposed to be showing RPM...
if (xv_config.bMotorEnable) // if the motor is spinning...
showSpeed(); // ...display RPM once per rev, at angle 0 (or as close as possible)
oldAngle = iCurrentAngle;
if (xv_config.aryAngles[startingAngle + ix]) { // if we're supposed to display that angle
if (aryInvalidDataFlag[ix] == 0) { // make sure that the 'Invalid Data' flag is clear
if (eShow == _eOUT_ANGLE) { // only display this stuff if we're specifically displaying angles
if (xv_config.eOUT & _OUT_MOD_SHOW_CSV) { // Display in CSV format?
Serial.print(startingAngle + ix);
Serial.print(F(","));
Serial.print(int(aryDist[ix]));
Serial.print(F(","));
Serial.println(aryQuality[ix]);
}
else {
Serial.print(startingAngle + ix);
Serial.print(F(": "));
Serial.print(int(aryDist[ix]));
Serial.print(F(" ("));
Serial.print(aryQuality[ix]);
Serial.println(F(")"));
}
} // if (eShow == _eOUT_ANGLE)
} // if (aryInvalidDataFlag[ix] == 0)
else if (xv_config.eOUT & _OUT_MOD_SHOW_ERRORS) { // If we're supposed to show errors...
if (xv_config.bMotorEnable) { // and if the motor is spinning...
if (eShow == _eOUT_ANGLE) { // only display this stuff if we're specifically displaying angles
if (xv_config.eOUT & _OUT_MOD_SHOW_CSV) { // Display in CSV format?
getSurreal
committed
Serial.print(startingAngle + ix);
Serial.print(F(","));
if (aryInvalidDataFlag[ix] & _INVALID_DATA_FLAG)
Serial.println(F("I,"));
if (aryInvalidDataFlag[ix] & _STRENGTH_WARNING_FLAG)
Serial.println(F("S,"));
getSurreal
committed
}
getSurreal
committed
Serial.print(startingAngle + ix);
Serial.print(F(": "));
if (aryInvalidDataFlag[ix] & _INVALID_DATA_FLAG)
Serial.println(F("{I}"));
if (aryInvalidDataFlag[ix] & _STRENGTH_WARNING_FLAG)
Serial.println(F("{S}"));
}
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
} // if (xv_config.bMotorEnable)
} // else if (xv_config.eOUT & _OUT_MOD_SHOW_ERRORS)
} // if (xv_config.aryAngles[startingAngle + ix])
} // for (int ix = 0; ix < _N_DATA_QUADS; ix++)
} // if ((eShow == _eOUT_UNKNOWN) || (eSHow == _eOUT_ANGLE))
} // if (eValidatePacket() == 0
else { // Packet did not pass CRC check!
if (xv_config.eOUT & _OUT_MOD_SHOW_ERRORS) { // Show errors?
/* DEBUGGING ONLY: Dump the contents of 'Packet'
for (int ix = 0; ix < _PACKET_LENGTH; ix++) { // a full error display (dump Packet to screen)
if (Packet[ix] < 0x10)
Serial.print(F("0"));
Serial.print(Packet[ix], HEX);
Serial.print(F(" "));
}
*/
Serial.println(F("CRC")); // an abbreviated error display
} // if (xv_config.eOUT & _OUT_MOD_SHOW_ERRORS)
} // if not (eValidatePacket() == 0
// 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;
}
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
getSurreal
committed
} // if (ixPacket == _PACKET_LENGTH)
} // if not (eState == _eState_Find_COMMAND)
getSurreal
committed
} // if (Serial1.available() > 0)
getSurreal
committed
if (xv_config.bMotorEnable) {
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.bMotorEnable)
getSurreal
committed
} // 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
getSurreal
committed
* xv_config.eOUT = output type
* xv_config.bShowInterval
getSurreal
committed
* curMillis = milliseconds, now
* lastMillis = milliseconds, last time through this subroutine
* 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;
getSurreal
committed
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);
getSurreal
committed
|| ((xv_config.eOUT & _MASK_eOUT_SHOW) == _eOUT_ANGLE)) {
curMillis = millis();
getSurreal
committed
if (xv_config.eOUT & _OUT_MOD_SHOW_INTERVAL) {
if (xv_config.bMotorEnable) { // if the motor is spinning...
if (xv_config.eOUT & _OUT_MOD_SHOW_CSV) { // is display format "CSV"?
Serial.print(F("Interval:"));
Serial.println(curMillis - lastMillis);
}
else {
Serial.print(F("Time interval ms:"));
Serial.println(curMillis - lastMillis);
}
} // if (xv_config.bMotorEnable)
getSurreal
committed
} // if (xv_config.eOUT & _OUT_MOD_SHOW_INTERVAL)
lastMillis = curMillis;
getSurreal
committed
} // if (((xv_config.eOUT & _MASK_eOUT_SHOW) == _eOUT_DIST)...
} // if (angle == 0)
getSurreal
committed
return angle;
}
/*
getSurreal
committed
* processSpeed- Keep the motor spinning
getSurreal
committed
* 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
getSurreal
committed
* motor_rph_low_byte, motor_rph_high_byte
* motor_rph, motor_rpm
getSurreal
committed
* Calls: Serial.print
*/
void processSpeed() {
getSurreal
committed
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;
getSurreal
committed
}
/*
* showSpeed - Display the motor RPM once per revolution (at angle 0, or as close to it as possible)
* Enter with: N/A
* Uses: xv_config.eOUT
* motor_rpm
* pwm_val
* Calls: processSpeed
*/
void showSpeed() {
if (xv_config.eOUT & _OUT_MOD_SHOW_CSV) { // is display format "CSV"?
getSurreal
committed
Serial.print(F("RPM:"));
Serial.print(motor_rpm);
Serial.print(F(",PWM:"));
Serial.println(pwm_val);
}
else { // normal display format
Serial.print(F("RPM: "));
Serial.print(motor_rpm);
getSurreal
committed
Serial.print(F(" PWM: "));
Serial.println(pwm_val);
getSurreal
committed
processSpeed(); // keep the motor spinning
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
getSurreal
committed
* 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
getSurreal
committed
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
getSurreal
committed
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
getSurreal
committed
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
/*
* 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;
getSurreal
committed
const int bytesToCheck = _PACKET_LENGTH - 2;
getSurreal
committed
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;
getSurreal
committed
b1b = Packet[_OFFSET_TO_CRC_L];
getSurreal
committed
b2a = checksum >> 8;
getSurreal
committed
b2b = Packet[_OFFSET_TO_CRC_M];
getSurreal
committed
if ((b1a == b1b) && (b2a == b2b))
getSurreal
committed
return _VALID_PACKET; // okay
getSurreal
committed
else
getSurreal
committed
return _IN_VALID_PACKET; // non-zero = bad CRC
getSurreal
committed
/*
getSurreal
committed
* initEEPROM - Initialize the EEPROM to known values
getSurreal
committed
*/
getSurreal
committed
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
int ix;
/*
byte *ptr_xv_config = &xv_config.id;
for (ix = 0; ix < _EEPROM_Config_LEN; ix++) // erase the EEPROM
EEPROM_writeAnything(ix, dummy);
for (int ix = 0; ix < _EEPROM_Config_LEN; ix++) {
if (*ptr_xv_config < 0x10)
Serial.print(F("0"));
Serial.print(*ptr_xv_config++, HEX);
Serial.print(F(" "));
if ((ix & 0x0F) == 0x0F) // CRLF after 16 bytes displayed
Serial.println(F(" "));
}
Serial.println(F(" "));
*/
xv_config.id = _EEPROM_ID; // set it to current value
xv_config.safety = _EEPROM_SAFETY_CHECK; // set "overwrite check" safety byte to known value
strcpy(xv_config.szVersion, szCURRENT_EEPROM_VERSION); // Store the current software version
xv_config.motor_pwm_pin = _DEFAULT_motor_pwm_pin; // pin connected N-Channel Mosfet
xv_config.rpm_setpoint = _DEFAULT_rpm_setpoint; // desired RPM (arrived at this value by testing)
xv_config.rpm_min = _DEFAULT_rpm_min; // set the rest of the default values...
xv_config.rpm_max = _DEFAULT_rpm_max;
xv_config.pwm_min = _DEFAULT_pwm_min;
xv_config.pwm_max = _DEFAULT_pwm_max;
xv_config.sample_time = _DEFAULT_sample_time;
// PID tuning values
xv_config.Kp = _DEFAULT_Kp;
xv_config.Ki = _DEFAULT_Ki;
xv_config.Kd = _DEFAULT_Kd;
xv_config.bMotorEnable = _DEFAULT_bMotorEnable;
xv_config.bShowAllAngles = _DEFAULT_bShowAllAngles;
xv_config.eOUT = _DEFAULT_eOUT;
for (ix = 0; ix < _N_ANGLES; ix++) // initialize the array of displayable angles
xv_config.aryAngles[ix] = _DEFAULT_bANGLE;
EEPROM_writeAnything(0, xv_config); // write the entire configuration to the EEPROM
bEEPROM_Initialized = true; // remember that we updated EEPROM (used by 'showConfig')
getSurreal
committed
/*
getSurreal
committed
* initSerialCommands - add operator commands to the 'sCmd' object
getSurreal
committed
*/
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);
getSurreal
committed
sCmd.addCommand("ShowAngles", showAngles);
sCmd.addCommand("HideAngles", hideDist);
sCmd.addCommand("ShowAngle", showAngles);
getSurreal
committed
sCmd.addCommand("HideAngle", hideDist);
sCmd.addCommand("MotorOff", motorOff);
sCmd.addCommand("MotorOn", motorOn);
sCmd.addCommand("HideRaw", hideRaw);
getSurreal
committed
sCmd.addCommand("ShowRaw", showRaw);
getSurreal
committed
sCmd.addCommand("ShowErrors", showErrors);
sCmd.addCommand("HideErrors", hideErrors);
getSurreal
committed
sCmd.addCommand("ShowError", showErrors);
sCmd.addCommand("HideError", hideErrors);
sCmd.addCommand("ShowCSV", showCSV);
sCmd.addCommand("HideCSV", hideCSV);
sCmd.addCommand("ShowInterval", showInterval);
sCmd.addCommand("HideInterval", hideInterval);
sCmd.addCommand("ShowAll", showAll);
sCmd.addCommand("HideAll", hideAll);
}
/*
* showAll - enable all the display modifier commands, i.e., RPM, Errors, Interval
getSurreal
committed
*/
void showAll() {
showRPM(); // enable showRPM
showErrors(); // enable showErrors
showInterval(); // enable showInterval
}
/*
* hideAll - disable all the display modifier commands, i.e., RPM, Errors, Interval
*/
void hideAll() {
hideRPM(); // disable showRPM
hideErrors(); // disable showErrors
hideInterval(); // disable showInterval
getSurreal
committed
/*
getSurreal
committed
* hideDist - Hide distance data from LIDAR
getSurreal
committed
*/
void hideDist() {
getSurreal
committed
int eShow = (xv_config.eOUT & _MASK_eOUT_SHOW);
if (eShow == _eOUT_ANGLE) { // you can only hide distance if it is currently active
getSurreal
committed
xv_config.eOUT &= -1 - _MASK_eOUT_SHOW; // keep top 8-bits and turn off bottom 8 bits before setting command
xv_config.eOUT |= _eOUT_UNKNOWN; // set "What we're displaying" to "unknown"
EEPROM_writeAnything(_EE_OFFSET_eOUT, xv_config.eOUT);
xv_config.bShowAllAngles = true; // set default back to 'show all angles'
EEPROM_writeAnything(_EE_OFFSET_SHOW_ALL_ANGLES, xv_config.bShowAllAngles);
James LeRoy
committed
Serial.println(F(" "));
Serial.println(F("Hiding Distance data"));
getSurreal
committed
/*
getSurreal
committed
* showAngles - Multi-angle range(s) implementation - DSH
* Command: ShowAngle(s) ddd, ddd-ddd, etc. | ALL | all | 360
getSurreal
committed
* Enter with: N/A
getSurreal
committed
* Uses: aryAngles (an array of 360 booleans) gets set to appropriate values
* xv_config.bShowAllAngles gets set to true if all angles (0-359) are requested
getSurreal
committed
* Calls: showDist
* Exits with: N/A
* TEST THIS STRING: ShowAngles 16-20, 300-305, 123-124, 10
*/
getSurreal
committed
void showAngles() {
getSurreal
committed
char c, *arg;
getSurreal
committed
boolean bOneshot = false, bFoundAngle = false, syntax_error = false, bDoAll = false;
getSurreal
committed
int doing_from_to, from, to, ix, lToken, n_groups = 0;
getSurreal
committed
unsigned int current_eOUT = xv_config.eOUT; // save the current value so we can restore the top 8-bits (modifiers)
//unsigned int currentCommand = current_eOUT & _MASK_eOUT_SHOW; // isolate the command (bottom 8-bits)
const int lenAll = 4; // length of szAll, including terminating 0
const char szAll[lenAll] = "ALL"; // the word "ALL" (in upper-case) is used to match "ShowAngle ALL"
const char sz360[lenAll] = "360"; // the value "360" is used to match "ShowAngle 360"
doing_from_to = 0; // state = doing 'from'
// Make sure that there is at least 1 angle or group of angles present, else try to use angles from EEPROM
getSurreal
committed
do {
getSurreal
committed
arg = sCmd.next(); // get the next token
if (arg == NULL) { // it's empty -- just exit
getSurreal
committed
sCmd.readSerial();
arg = sCmd.next();
break;
}
getSurreal
committed
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
lToken = strlen(arg); // get the length of the current token
// is 'arg' = the string "all" ?
if (lToken == lenAll - 1) { // we have a candidate for match, now check it in detail
bDoAll = true;
for (ix = 0; ix < lenAll; ix++) { // eschew 'strncmp' because it's stupid about "all" vs. "allxxx"
if ((arg[ix] & (-1 - ' ')) != szAll[ix]) { // force to upper-case for match
bDoAll = false;
break;
}
} // for (ix = 0; ix < lenAll; ix++)
if (bDoAll == false) { // it's not "all" -- check for "360" which means the same thing
bDoAll = true;
for (ix = 0; ix < lenAll; ix++) { // eschew 'strncmp' because it's stupid about "360" vs. "3600"
if (arg[ix] != sz360[ix]) {
bDoAll = false;
break;
}
} // for (ix = 0; ix < lenAll; ix++)
} // if (bDoAll == false)
} // if (lToken == lenAll - 1)
else
bDoAll = false;
// 'bDoAll' = true means we're supposed to display all angles, else, continue parsing input string
if (bDoAll)
break; // break out of the do loop
getSurreal
committed
// see if the token has an embedded "-", meaning from - to
for (ix = 0; ix < lToken; ix++) {
c = arg[ix];
if (c == ',') { // optional trailing comma
getSurreal
committed
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'
}
getSurreal
committed
else if (c == ' ') { // ignore blanks
getSurreal
committed
}
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
getSurreal
committed
if ((from >= 0) && (from < _N_ANGLES)
&& (to >= 0) && (to < _N_ANGLES)) {
getSurreal
committed
if (to >= from) {
getSurreal
committed
if (bOneshot == false) {
bOneshot = true;
for (ix = 0; ix < _N_ANGLES; ix++) // initialize aryAngles (just once)
xv_config.aryAngles[ix] = false;
}
getSurreal
committed
for (ix = from; ix <= to; ix++) {
getSurreal
committed
xv_config.aryAngles[ix] = true;
getSurreal
committed
}
}
else {
syntax_error = true;
break;
}
}
else {
syntax_error = true;
break;
}
from = 0;
to = 0;
doing_from_to = 0;
} // do
while (arg != NULL);
getSurreal
committed
// Either we have 1 or more angles, else we got "ALL" or "360" or we have a syntax error
if (bDoAll) {
if (xv_config.bShowAllAngles == false) { // only rewrite EEPROM if it needs it
xv_config.bShowAllAngles = true; // value if we're showing all angles
for (ix = 0; ix < _N_ANGLES; ix++)
xv_config.aryAngles[ix] = true;
EEPROM_writeAnything(_EE_OFFSET_aryAngles, xv_config.aryAngles);
EEPROM_writeAnything(_EE_OFFSET_SHOW_ALL_ANGLES, xv_config.bShowAllAngles);
}
} // if (bDoAll)
else { // if no angles specified, try to use EEPROM values
bFoundAngle = false;
if (n_groups == 0) { // no angle groups were specified...
for (ix = 0; ix < _N_ANGLES; ix++) { // ...are any angles stored in EEPROM?
if (xv_config.aryAngles[ix]) {
bFoundAngle = true; // we found at least 1 angle in EEPROM
break;
}
} // for (ix = 0; ix < _N_ANGLES; ix++)
if (bFoundAngle == false) // no angles found in EEPROM...error
syntax_error = true;
} // if (n_groups == 0)
} // if (bDoAll == false)
getSurreal
committed
// Handle syntax errors
if (syntax_error) {
getSurreal
committed
xv_config.eOUT = (current_eOUT & _MASK_eOUT_MODIFIER); // restore any modifier bits
xv_config.eOUT |= _eOUT_UNKNOWN; // set "What we're displaying" to "unknown"
EEPROM_writeAnything(_EE_OFFSET_eOUT, xv_config.eOUT);
// NOTE: We're leaving any previously-set angles and 'bShowAllAngles' alone
// Maybe the operator just keyed in a bad value, but no need to punish...
getSurreal
committed
Serial.println(F(" "));
getSurreal
committed
Serial.println(F("Incorrect syntax. Examples:"));
Serial.println(F(" ShowAngle(s) 0, 15-30, 45-50, 10"));
Serial.println(F(" ShowAngle(s) all or 360"));
Serial.println(F("Syntax rules:"));
Serial.println(F(" Use a space after each comma. No particular order (of angles) is required."));
Serial.println(F(" In a from-to pair, the 1st value must be lowest."));
}
else { // We're ready to process multiple angles
xv_config.eOUT = (current_eOUT & _MASK_eOUT_MODIFIER); // restore any modifier bits
xv_config.eOUT |= _eOUT_ANGLE; // set "What we're displaying" to "Angle"
EEPROM_writeAnything(_EE_OFFSET_eOUT, xv_config.eOUT);
if (bDoAll == false) { // if we haven't already written 'bShowAllAngles' and 'aryAngles'...
xv_config.bShowAllAngles = true; // count the angles and set 'bShowAllAngles'
for (ix = 0; ix < _N_ANGLES; ix++) {
if (xv_config.aryAngles[ix] == false) {
xv_config.bShowAllAngles = false;
break;
}
getSurreal
committed
}
getSurreal
committed
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
EEPROM_writeAnything(_EE_OFFSET_SHOW_ALL_ANGLES, xv_config.bShowAllAngles);
if (n_groups != 0)
EEPROM_writeAnything(_EE_OFFSET_aryAngles, xv_config.aryAngles);
} // if (bDoAll == false)
Serial.println(F(" "));
Serial.print(F("Angles:"));
if (xv_config.bShowAllAngles)
Serial.print(F("all"));
else {
for (int ix = 0; ix < _N_ANGLES; ix++) { // display the angle array
if(xv_config.aryAngles[ix]) {
Serial.print(ix, DEC);
Serial.print(F(","));
}
else
xv_config.bShowAllAngles = false;
} // for (int ix = 0; ix < _N_ANGLES; ix++)
Serial.println(F(" "));
} // if not (xv_config.bShowAllAngles)
Serial.println(F(" "));
Serial.println(F("Showing Distance data <Angle>: <dist mm> (signal strength)}"));
if (!(xv_config.bMotorEnable)) // Turn the motor ON if it's OFF
forceMotorOn();
} // if not (syntax_error)
getSurreal
committed
}
getSurreal
committed
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
/*
* forceRaw
* Update 'eOUT' word as follows:
* Top 8-bits: Preserve modifiers {RPM; CSV; ERRORS; INTERVAL, and maybe RFU_1; RFU_2; RFU_3; RFU_4}
* Bottom 8-bits (the command): Set it to '_eOUT_RAW'
*/
void forceRaw() {
xv_config.eOUT &= _MASK_eOUT_MODIFIER; // Preserve the MOD bits (top 8-bits)
xv_config.eOUT |= _eOUT_RAW; // Set "What we're displaying" to display "raw" data
EEPROM_writeAnything(_EE_OFFSET_eOUT, xv_config.eOUT);
}
/*
* showRaw - Show "raw" LIDAR data (feed data directly to user)
*/
void showRaw() {
if ((xv_config.eOUT & _MASK_eOUT_SHOW) != _eOUT_RAW) { // Only do this if it's not already active
forceRaw(); // enable "RAW" mode
if (!(xv_config.bMotorEnable)) // Turn the motor ON if it's OFF
forceMotorOn(); // start up the motor
}
}
/*
* hideRaw - Hide "raw" LIDAR data
*/
void hideRaw() {
if ((xv_config.eOUT & _MASK_eOUT_SHOW) == _eOUT_RAW) { // Only do this if we're displaying RAW data
xv_config.eOUT &= _MASK_eOUT_MODIFIER; // Preserve the MOD bits (top 8-bits)
xv_config.eOUT |= _eOUT_UNKNOWN; // Set "What we're displaying" to "unknown"
EEPROM_writeAnything(_EE_OFFSET_eOUT, xv_config.eOUT); // write 'eOUT' to EEPROM
Serial.println(F(" "));
Serial.println(F("Raw LIDAR data disabled"));
}
}
/*
* motorOff - turn the motor OFF
*/
getSurreal
committed
if (xv_config.bMotorEnable) { // you can only turn the motor OFF if it's ON
xv_config.bMotorEnable = false;
EEPROM_writeAnything(_EE_OFFSET_MOTOR_ENABLE, xv_config.bMotorEnable);
Timer3.pwm(xv_config.motor_pwm_pin, 0);
if ((xv_config.eOUT & _MASK_eOUT_SHOW) != _eOUT_RAW) { // can't display messages if we're in RAW mode
Serial.println(F(" "));
Serial.println(F("Motor off"));
}
} // if (xv_config.bMotorEnable)
getSurreal
committed
/*
* forceMotorOn - turn the motor on silently
*/
void forceMotorOn() {
xv_config.bMotorEnable = true;
EEPROM_writeAnything(_EE_OFFSET_MOTOR_ENABLE, xv_config.bMotorEnable);
getSurreal
committed
rpm_err = 0; // reset rpm error
getSurreal
committed
/*
* motorOn - turn the motor ON
*/
void motorOn() {
if (xv_config.bMotorEnable == false) { // you can only turn the motor ON if it's OFF
forceMotorOn();
if ((xv_config.eOUT & _MASK_eOUT_SHOW) != _eOUT_RAW) { // can't display messages if we're in RAW mode
Serial.println(F(" "));
Serial.println(F("Motor on"));
}
}
}
/*
* motorCheck - Make sure the motor RPMs are good else shut it down
*/
void motorCheck() {
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();