XV_Lidar_Controller.ino

/*
  XV Lidar Controller v1.2.2

 Copyright 2014 James LeRoy getSurreal
 https://github.com/getSurreal/XV_Lidar_Controller
 http://www.getsurreal.com/products/xv-lidar-controller
<<<<<<< HEAD

 Modified to add CRC checking - Doug Hilton, WD0UG November, 2015 mailto: six.speed (at) yahoo (dot) com

 See README for additional information

=======
 
 See README for additional information 
 
>>>>>>> 9423c3c07ba8665de06a1a0f3cdad7d726b30174
 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
#include <PID.h>
#include <EEPROM.h>
#include "EEPROMAnything.h"
#include <SerialCommand.h>

<<<<<<< HEAD
const int SHOW_ALL_ANGLES = 360;     // value means 'display all angle data, 0..359'

=======
>>>>>>> 9423c3c07ba8665de06a1a0f3cdad7d726b30174
struct EEPROM_Config {
  byte id;
  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

  // PID tuning values
  double Kp;
  double Ki;
  double Kd;

  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
  unsigned int show_angle;  // controlled by ShowAngle (0 - 359, 360 shows all)
}
xv_config;

/*
 * EEPROM_ID
 * Used to validate that the EEPROM is initialized
 * Important to increment the number anytime variables are added or removed
 *   or when the default values are changed
 */
const byte EEPROM_ID = 0x05;

double pwm_val = 500;  // start with ~50% power
double pwm_last;
double motor_rpm;
unsigned long now;
unsigned long motor_check_timer = millis();
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;
unsigned long curMillis;
unsigned long lastMillis = millis();

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
<<<<<<< HEAD
uint8_t motor_rph_high_byte = 0;
uint8_t motor_rph_low_byte = 0;
uint16_t dist[N_DATA_QUADS] = {0, 0, 0, 0};   // thre are (4) distances, one for each data quad
uint16_t quality[N_DATA_QUADS] = {0, 0, 0, 0}; // same with 'quality'
=======
uint16_t data_status = 0;
uint16_t data_4deg_index = 0;
uint16_t data_loop_index = 0;
uint8_t motor_rph_high_byte = 0; 
uint8_t motor_rph_low_byte = 0;
uint8_t data0, data2;
uint16_t dist, quality;
>>>>>>> 9423c3c07ba8665de06a1a0f3cdad7d726b30174
uint16_t motor_rph = 0;
uint16_t angle;

SerialCommand sCmd;


  #if defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY)  // if Teensy 2.0
  const int ledPin = 11;
  #endif
  
  #if defined(__MK20DX256__)  // if Teensy 3.1
  const int ledPin = 13;
  #endif
boolean ledState = LOW;

void setup() {
  EEPROM_readAnything(0, xv_config);
  if ( xv_config.id != EEPROM_ID) { // verify EEPROM values have been initialized
    initEEPROM();
  }
<<<<<<< HEAD
  pinMode(xv_config.motor_pwm_pin, OUTPUT);
  Serial.begin(115200);                      // USB serial
  Serial1.begin(115200);                     // XV LDS data

  /*
   *   Timer3.initialize(30)
   *   sets PWM frequency to 32.768kHz for quite ultrasonic motor control
   *   PWM duty cycle is 0-1023
   */
  Timer3.initialize(30);
=======
  pinMode(xv_config.motor_pwm_pin, OUTPUT); 
  Serial.begin(115200);  // USB serial
  #if defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY) // if Teensy 2.0
  Serial1.begin(115200);  // XV LDS data 
  #endif
  
  #if defined(__MK20DX256__) // if Teensy 3.1
  Serial1.begin(115200);  // XV LDS data 
  #endif

  Timer3.initialize(30); // set PWM frequency to 32.768kHz  
>>>>>>> 9423c3c07ba8665de06a1a0f3cdad7d726b30174

  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);

  initSerialCommands();
  pinMode(ledPin, OUTPUT);
<<<<<<< HEAD

  eState = eState_Find_COMMAND;
  for (ixPacket = 0; ixPacket < PACKET_LENGTH; ixPacket++)  // Initialize
    Packet[ixPacket] = 0;
  ixPacket = 0;
=======
>>>>>>> 9423c3c07ba8665de06a1a0f3cdad7d726b30174
}

void loop() {
<<<<<<< HEAD
  boolean bPacketOkay;                            // true = packet is okay (CRC valid)
  sCmd.readSerial();  // check for incoming serial commands
  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
    // 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
        bPacketOkay = (eValidatePacket() == 0);   // Check packet CRC
        startingAngle = processIndex(bPacketOkay);   // get the starting angle of this group (of 4)
        processSpeed(bPacketOkay);                // 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
          processDistance(bPacketOkay, ix);
        for (int ix = 0; ix < N_DATA_QUADS; ix++) // process the signal strength (quality)
          processSignalStrength(bPacketOkay, ix);
        if (bPacketOkay) {
          if (xv_config.show_dist) {              // show distance command is active
            if (xv_config.show_angle == SHOW_ALL_ANGLES
                || (xv_config.show_angle >= startingAngle && xv_config.show_angle < startingAngle + N_DATA_QUADS)) {
              for (int ix = 0; ix < N_DATA_QUADS; ix++) {
                if ((xv_config.show_angle == SHOW_ALL_ANGLES)
                    || (xv_config.show_angle == startingAngle + ix)) {
                  Serial.print(startingAngle + ix);
                  Serial.print(F(": "));
                  Serial.print(int(dist[ix]));
                  Serial.print(F(" ("));
                  Serial.print(quality[ix]);
                  Serial.println(F(")"));
                }
              }  // or (int ix = 0; ix < N_DATA_QUADS; ix++)
            }  // if (xv_config.show_angle == SHOW_ALL_ANGLES ...
          }  // if (xv_config.show_dist)
        }  // if (bPacketOkay)
        // initialize a bunch of stuff before we switch back to State 1