Saving settings to EEPROM and other IMO sensible changes
This commit is contained in:
parent
b35e11696e
commit
6b9ea8ef06
9 changed files with 436 additions and 255 deletions
|
|
@ -12,4 +12,15 @@
|
||||||
platform = atmelavr
|
platform = atmelavr
|
||||||
board = ATmega88P
|
board = ATmega88P
|
||||||
framework = arduino
|
framework = arduino
|
||||||
|
upload_protocol = stk500v1
|
||||||
|
; each flag in a new line
|
||||||
|
upload_flags =
|
||||||
|
-P$UPLOAD_PORT
|
||||||
|
-b$UPLOAD_SPEED
|
||||||
|
|
||||||
|
; edit these lines
|
||||||
|
upload_port = COM4
|
||||||
|
upload_speed = 19200
|
||||||
|
|
||||||
|
board_build.f_cpu = 1000000L
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -34,7 +34,7 @@
|
||||||
To change trigger active duration: TRG_D [integer for milliseconds]
|
To change trigger active duration: TRG_D [integer for milliseconds]
|
||||||
To change gain factor: GAIN_F [integer for gain state - see note*]
|
To change gain factor: GAIN_F [integer for gain state - see note*]
|
||||||
To change ADC hysteresis value: HYST [integer]
|
To change ADC hysteresis value: HYST [integer]
|
||||||
To change sensor input pullup vRef low threshold: VADJ [float value]
|
To change sensor input pullup vRef low threshold: VFOL [float value]
|
||||||
To change comparator trigger high threshold: VCOMP [float value]
|
To change comparator trigger high threshold: VCOMP [float value]
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -43,7 +43,7 @@ These commands should be wrapped in this format:
|
||||||
|
|
||||||
Examples:
|
Examples:
|
||||||
<GAIN_F, 3> <~ set gain factor to index 3 (6x)
|
<GAIN_F, 3> <~ set gain factor to index 3 (6x)
|
||||||
<VADJ, 2350> <~ set the vref floor to 2.35V
|
<VFOL, 2350> <~ set the vref floor to 2.35V
|
||||||
|
|
||||||
*Note for Gain Factor:
|
*Note for Gain Factor:
|
||||||
The gain STATE is representative of these values:
|
The gain STATE is representative of these values:
|
||||||
|
|
@ -54,14 +54,7 @@ The gain STATE is representative of these values:
|
||||||
4 = 11x
|
4 = 11x
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/*------------------------------------------------------------*/
|
#include <Arduino.h>
|
||||||
|
|
||||||
// Debug output toggle. Uncomment to enable
|
|
||||||
#define DEBUG true
|
|
||||||
|
|
||||||
/* Debug output verbose mode will continuously output sensor readings
|
|
||||||
rather than waiting for user input */
|
|
||||||
//#define VERBOSE true
|
|
||||||
|
|
||||||
// Headers, variables, and functions
|
// Headers, variables, and functions
|
||||||
#include "LightChrono.h"
|
#include "LightChrono.h"
|
||||||
|
|
@ -74,68 +67,82 @@ The gain STATE is representative of these values:
|
||||||
// i2c input toggle. Uncomment to enable
|
// i2c input toggle. Uncomment to enable
|
||||||
//#define I2C_INPUT true
|
//#define I2C_INPUT true
|
||||||
|
|
||||||
void setup() {
|
void setup()
|
||||||
pinMode(TRG_OUT, OUTPUT); // declare the Trigger as as OUTPUT
|
{
|
||||||
|
pinMode(TRG_OUT, OUTPUT); // declare the Trigger as as OUTPUT
|
||||||
pinMode(ERR_LED, OUTPUT);
|
pinMode(ERR_LED, OUTPUT);
|
||||||
pinMode(Z_TRG, INPUT_PULLUP); // declare z-sense input with pullup
|
pinMode(Z_TRG, INPUT_PULLUP); // declare z-sense input with pullup
|
||||||
pinMode(V_FOLLOW_PIN, INPUT);
|
pinMode(V_FOLLOW_PIN, INPUT);
|
||||||
pinMode(VCOMP_SENSE_PIN, INPUT);
|
pinMode(VCOMP_SENSE_PIN, INPUT);
|
||||||
pinMode(GADJ_R0, INPUT); // declare input to set high impedance
|
pinMode(GADJ_R0, INPUT); // declare input to set high impedance
|
||||||
pinMode(GADJ_R1, INPUT); // declare input to set high impedance
|
pinMode(GADJ_R1, INPUT); // declare input to set high impedance
|
||||||
pinMode(GADJ_R2, INPUT); // declare input to set high impedance
|
pinMode(GADJ_R2, INPUT); // declare input to set high impedance
|
||||||
pinMode(GADJ_R3, INPUT); // declare input to set high impedance
|
pinMode(GADJ_R3, INPUT); // declare input to set high impedance
|
||||||
Serial.begin(9600);
|
Serial.begin(9600);
|
||||||
|
|
||||||
attachInterrupt(digitalPinToInterrupt(Z_TRG), pulse, FALLING);
|
attachInterrupt(digitalPinToInterrupt(Z_TRG), pulse, FALLING);
|
||||||
|
|
||||||
Serial.println("Initializing Pyr0-Piezo Sensor...");
|
Serial.println("Initializing Pyr0-Piezo Sensor...");
|
||||||
|
|
||||||
|
restoreConfig();
|
||||||
|
|
||||||
|
adjustGain();
|
||||||
}
|
}
|
||||||
|
|
||||||
/*------------------------------------------------*/
|
void loop()
|
||||||
|
{
|
||||||
void loop() {
|
if (mainLoop.hasPassed(LOOP_DUR))
|
||||||
if (mainLoop.hasPassed(LOOP_DUR)) {
|
{
|
||||||
mainLoop.restart();
|
mainLoop.restart();
|
||||||
// Blink LED's on init
|
|
||||||
if (BlinkCount > 0) {
|
|
||||||
BlinkState = !BlinkState;
|
|
||||||
digitalWrite(ERR_LED, BlinkState);
|
|
||||||
digitalWrite(TRG_OUT, BlinkState);
|
|
||||||
--BlinkCount;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Get Serial Input
|
// Get Serial Input
|
||||||
serialInput();
|
serialInput();
|
||||||
|
|
||||||
// Set any new parameters from serial input
|
// Set any new parameters from serial input
|
||||||
updateParams();
|
if (serialIncoming)
|
||||||
|
{
|
||||||
// Set the amplification gain factor
|
updateParams();
|
||||||
adjustGain();
|
}
|
||||||
|
|
||||||
// Check voltage of first and second stages and compare against thresholds
|
// Check voltage of first and second stages and compare against thresholds
|
||||||
adjustVin();
|
readVin();
|
||||||
VComp = analogRead(VCOMP_SENSE_PIN);
|
VComp = analogRead(VCOMP_SENSE_PIN);
|
||||||
VAdj = analogRead(V_FOLLOW_PIN);
|
VFol = analogRead(V_FOLLOW_PIN);
|
||||||
|
|
||||||
// Voltage Follower adjustment
|
VLast = VOld - Vin;
|
||||||
if (VLast > Hyst || VLast < -Hyst) {
|
if (VLast > Hyst || VLast < -Hyst)
|
||||||
|
{
|
||||||
|
// Voltage Follower adjustment
|
||||||
adjustFollow();
|
adjustFollow();
|
||||||
}
|
// Voltage Comparator adjustment
|
||||||
|
|
||||||
// Voltage Comparator adjustment
|
|
||||||
if (VLast > Hyst || VLast < -Hyst) {
|
|
||||||
adjustComp();
|
adjustComp();
|
||||||
|
// Alert the user that auto-calibration is ongoing
|
||||||
|
ERR_STATE = 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ERR_STATE = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Alert the user that auto-calibration is ongoing
|
// Blink LED's on init
|
||||||
calibrateAlert();
|
if (BlinkCount > 0)
|
||||||
|
{
|
||||||
|
BlinkState = !BlinkState;
|
||||||
|
digitalWrite(ERR_LED, BlinkState);
|
||||||
|
digitalWrite(TRG_OUT, BlinkState);
|
||||||
|
--BlinkCount;
|
||||||
|
}
|
||||||
|
else
|
||||||
// Check for error state
|
// Check for error state
|
||||||
checkError();
|
{
|
||||||
|
checkError();
|
||||||
|
}
|
||||||
|
|
||||||
// Reply with status
|
// Print state if debug is on
|
||||||
serialReply();
|
if (Debug > 0)
|
||||||
|
{
|
||||||
|
serialPrintState();
|
||||||
|
}
|
||||||
|
|
||||||
// Sets trigger output state to false after completing loop
|
// Sets trigger output state to false after completing loop
|
||||||
//digitalWrite(TRG_OUT, HIGH);
|
//digitalWrite(TRG_OUT, HIGH);
|
||||||
|
|
|
||||||
99
firmware/AVR-Source/Pyr0_Piezo_Sensor_v2/src/pP_config.cpp
Normal file
99
firmware/AVR-Source/Pyr0_Piezo_Sensor_v2/src/pP_config.cpp
Normal file
|
|
@ -0,0 +1,99 @@
|
||||||
|
#include "pP_config.h"
|
||||||
|
#include <EEPROM.h>
|
||||||
|
|
||||||
|
int GAIN_FACTOR = GAIN_FACTOR_DEFAULT; // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
|
||||||
|
int followerThrs = FOLLOWER_THRESHOLD_DEFAULT;
|
||||||
|
int compThrs = COMP_THRESHOLD_DEFAULT;
|
||||||
|
int LOOP_DUR = LOOP_DUR_DEFAULT; // duration of time between ADC checks and other loop functions
|
||||||
|
int TRG_DUR = TRG_DUR_DEFAULT; // duration of the Z-axis pulse sent, in ms
|
||||||
|
int Hyst = HYST_DEFAULT; // Hysteresis value for ADC measurements
|
||||||
|
int Debug = 0;
|
||||||
|
long voltMeterConstant = 1125300L; // For fine tuning input voltage sense
|
||||||
|
// byte pP_i2c_address = 0xa0; // I2C Bus Address
|
||||||
|
|
||||||
|
void resetEEPROM()
|
||||||
|
{
|
||||||
|
resetConfig();
|
||||||
|
EEPROM.put(GAIN_FACTOR_ADDRESS, GAIN_FACTOR);
|
||||||
|
EEPROM.put(FOLLOWER_THRESHOLD_ADDRESS, followerThrs);
|
||||||
|
EEPROM.put(COMP_THRESHOLD_ADDRESS, compThrs);
|
||||||
|
EEPROM.put(LOOP_DUR_ADDRESS, LOOP_DUR);
|
||||||
|
EEPROM.put(TRG_DUR_ADDRESS, TRG_DUR);
|
||||||
|
EEPROM.put(HYST_ADDRESS, Hyst);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Restore config from EEPROM, otherwise reset config and write to EEPROM
|
||||||
|
void restoreConfig()
|
||||||
|
{
|
||||||
|
int temp;
|
||||||
|
|
||||||
|
EEPROM.get(GAIN_FACTOR_ADDRESS, temp);
|
||||||
|
if (temp < 0 || temp > 4)
|
||||||
|
{
|
||||||
|
resetEEPROM();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
GAIN_FACTOR = temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
EEPROM.get(FOLLOWER_THRESHOLD_ADDRESS, temp);
|
||||||
|
if (temp < 0 || temp > 5000)
|
||||||
|
{
|
||||||
|
resetEEPROM();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
followerThrs = temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
EEPROM.get(COMP_THRESHOLD_ADDRESS, temp);
|
||||||
|
if (temp < 0 || temp > 5000)
|
||||||
|
{
|
||||||
|
resetEEPROM();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
compThrs = temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
EEPROM.get(LOOP_DUR_ADDRESS, temp);
|
||||||
|
if (temp < 0 && temp > 1000)
|
||||||
|
{
|
||||||
|
resetEEPROM();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
LOOP_DUR = temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
EEPROM.get(TRG_DUR_ADDRESS, temp);
|
||||||
|
if (temp < 0 || temp > 1000)
|
||||||
|
{
|
||||||
|
resetEEPROM();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
TRG_DUR = temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
EEPROM.get(HYST_ADDRESS, temp);
|
||||||
|
if (temp < 0 || temp > 1000)
|
||||||
|
{
|
||||||
|
resetEEPROM();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
Hyst = temp;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void resetConfig()
|
||||||
|
{
|
||||||
|
GAIN_FACTOR = GAIN_FACTOR_DEFAULT;
|
||||||
|
followerThrs = FOLLOWER_THRESHOLD_DEFAULT;
|
||||||
|
compThrs = COMP_THRESHOLD_DEFAULT;
|
||||||
|
LOOP_DUR = LOOP_DUR_DEFAULT;
|
||||||
|
TRG_DUR = TRG_DUR_DEFAULT;
|
||||||
|
Hyst = HYST_DEFAULT;
|
||||||
|
}
|
||||||
|
|
@ -1,41 +1,63 @@
|
||||||
|
#ifndef PP_CONFIG_H
|
||||||
|
#define PP_CONFIG_H
|
||||||
|
|
||||||
// Configurable settings:
|
// Configurable settings:
|
||||||
|
|
||||||
|
#define GAIN_FACTOR_DEFAULT 2
|
||||||
|
#define GAIN_FACTOR_ADDRESS 0
|
||||||
#if !(defined(GAIN_FACTOR))
|
#if !(defined(GAIN_FACTOR))
|
||||||
int GAIN_FACTOR = 2; // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
|
extern int GAIN_FACTOR; // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef senseThrs
|
#define FOLLOWER_THRESHOLD_DEFAULT 1450
|
||||||
#define senseThrs 1450
|
#define FOLLOWER_THRESHOLD_ADDRESS 4
|
||||||
|
#if !(defined(followerThrs))
|
||||||
|
extern int followerThrs;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef compThrs
|
#define COMP_THRESHOLD_DEFAULT 2850
|
||||||
#define compThrs 2850
|
#define COMP_THRESHOLD_ADDRESS 8
|
||||||
|
#if !(defined(compThrs))
|
||||||
|
extern int compThrs;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef InitCount
|
#ifndef InitCount
|
||||||
#define InitCount 6 // Number of times to blink the LED on start
|
#define InitCount 6 // Number of times to blink the LED on start
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define LOOP_DUR_DEFAULT 50
|
||||||
|
#define LOOP_DUR_ADDRESS 12
|
||||||
#if !(defined(LOOP_DUR))
|
#if !(defined(LOOP_DUR))
|
||||||
int LOOP_DUR = 50; // duration of time between ADC checks and other loop functions
|
extern int LOOP_DUR; // duration of time between ADC checks and other loop functions
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define TRG_DUR_DEFAULT 20
|
||||||
|
#define TRG_DUR_ADDRESS 16
|
||||||
#if !(defined(TRG_DUR))
|
#if !(defined(TRG_DUR))
|
||||||
int TRG_DUR = 20; // duration of the Z-axis pulse sent, in ms
|
extern int TRG_DUR; // duration of the Z-axis pulse sent, in ms
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define HYST_DEFAULT 20
|
||||||
|
#define HYST_ADDRESS 20
|
||||||
#if !(defined(Hyst))
|
#if !(defined(Hyst))
|
||||||
int Hyst = 20; // Hysteresis value for ADC measurements
|
extern int Hyst; // Hysteresis value for ADC measurements
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if !(defined(Debug))
|
||||||
|
extern int Debug;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if !(defined(voldMeterConstant))
|
#if !(defined(voldMeterConstant))
|
||||||
long voltMeterConstant = 1125300L; // For fine tuning input voltage sense
|
extern long voltMeterConstant; // For fine tuning input voltage sense
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef I2C_INPUT
|
// #ifdef I2C_INPUT
|
||||||
#if !(defined(pP_i2c_address))
|
// #if !(defined(pP_i2c_address))
|
||||||
byte pP_i2c_address = 0xa0; // I2C Bus Address
|
// extern byte pP_i2c_address = 0xa0; // I2C Bus Address
|
||||||
#endif
|
// #endif
|
||||||
#endif
|
// #endif
|
||||||
|
|
||||||
|
void restoreConfig();
|
||||||
|
void resetConfig();
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -54,14 +54,14 @@ long readVcc() {
|
||||||
|
|
||||||
/*------------------------------------------------*/
|
/*------------------------------------------------*/
|
||||||
|
|
||||||
void adjustVin() {
|
void readVin() {
|
||||||
VOld = Vin;
|
VOld = Vin;
|
||||||
Vin = readVcc(), DEC;
|
Vin = readVcc(), DEC;
|
||||||
senseLong = senseThrs * 1024L;
|
followerLong = followerThrs * 1023L;
|
||||||
compLong = compThrs * 1024L;
|
compLong = compThrs * 1023L;
|
||||||
senseInt = (long long) senseLong / Vin;
|
followerInt = (long long) followerLong / Vin;
|
||||||
compInt = (long long) compLong / Vin;
|
compInt = (long long) compLong / Vin;
|
||||||
senseInt = (int) senseInt;
|
followerInt = (int) followerInt;
|
||||||
compInt = (int) compInt;
|
compInt = (int) compInt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -71,7 +71,7 @@ long readVcc() {
|
||||||
/* Compares diffs of threshold vs read value
|
/* Compares diffs of threshold vs read value
|
||||||
if positive, adjusts the follower to within
|
if positive, adjusts the follower to within
|
||||||
the range set above*/
|
the range set above*/
|
||||||
ADJ_FOLLOW = (senseInt / 4);
|
ADJ_FOLLOW = (followerInt / 4);
|
||||||
|
|
||||||
// Analog output (PWM) of duty cycle
|
// Analog output (PWM) of duty cycle
|
||||||
analogWrite(V_FOL_PWM, ADJ_FOLLOW);
|
analogWrite(V_FOL_PWM, ADJ_FOLLOW);
|
||||||
|
|
@ -87,15 +87,6 @@ void adjustComp() {
|
||||||
|
|
||||||
/*------------------------------------------------*/
|
/*------------------------------------------------*/
|
||||||
|
|
||||||
void calibrateAlert() {
|
|
||||||
VLast = VOld - Vin;
|
|
||||||
if (VLast > Hyst || VLast < -Hyst ) {
|
|
||||||
ERR_STATE = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*------------------------------------------------*/
|
|
||||||
|
|
||||||
void adjustGain() {
|
void adjustGain() {
|
||||||
|
|
||||||
if (GAIN_FACTOR == 0) {
|
if (GAIN_FACTOR == 0) {
|
||||||
|
|
@ -103,7 +94,6 @@ void adjustGain() {
|
||||||
pinMode(GADJ_R2, INPUT);
|
pinMode(GADJ_R2, INPUT);
|
||||||
pinMode(GADJ_R1, INPUT);
|
pinMode(GADJ_R1, INPUT);
|
||||||
pinMode(GADJ_R0, INPUT);
|
pinMode(GADJ_R0, INPUT);
|
||||||
ERR_STATE = 0;
|
|
||||||
}
|
}
|
||||||
else if (GAIN_FACTOR > 0) {
|
else if (GAIN_FACTOR > 0) {
|
||||||
pinMode(GADJ_R3, OUTPUT);
|
pinMode(GADJ_R3, OUTPUT);
|
||||||
|
|
@ -111,25 +101,21 @@ void adjustGain() {
|
||||||
pinMode(GADJ_R2, INPUT);
|
pinMode(GADJ_R2, INPUT);
|
||||||
pinMode(GADJ_R1, INPUT);
|
pinMode(GADJ_R1, INPUT);
|
||||||
pinMode(GADJ_R0, INPUT);
|
pinMode(GADJ_R0, INPUT);
|
||||||
ERR_STATE = 0;
|
|
||||||
}
|
}
|
||||||
else if (GAIN_FACTOR > 1) {
|
else if (GAIN_FACTOR > 1) {
|
||||||
pinMode(GADJ_R2, OUTPUT);
|
pinMode(GADJ_R2, OUTPUT);
|
||||||
digitalWrite(GADJ_R2, LOW);
|
digitalWrite(GADJ_R2, LOW);
|
||||||
pinMode(GADJ_R1, INPUT);
|
pinMode(GADJ_R1, INPUT);
|
||||||
pinMode(GADJ_R0, INPUT);
|
pinMode(GADJ_R0, INPUT);
|
||||||
ERR_STATE = 0;
|
|
||||||
}
|
}
|
||||||
else if (GAIN_FACTOR > 2) {
|
else if (GAIN_FACTOR > 2) {
|
||||||
pinMode(GADJ_R1, OUTPUT);
|
pinMode(GADJ_R1, OUTPUT);
|
||||||
digitalWrite(GADJ_R1, LOW);
|
digitalWrite(GADJ_R1, LOW);
|
||||||
pinMode(GADJ_R0, INPUT);
|
pinMode(GADJ_R0, INPUT);
|
||||||
ERR_STATE = 0;
|
|
||||||
}
|
}
|
||||||
else if (GAIN_FACTOR > 3) {
|
else if (GAIN_FACTOR > 3) {
|
||||||
pinMode(GADJ_R0, OUTPUT);
|
pinMode(GADJ_R0, OUTPUT);
|
||||||
digitalWrite(GADJ_R0, LOW);
|
digitalWrite(GADJ_R0, LOW);
|
||||||
ERR_STATE = 0;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -31,7 +31,7 @@ void pP_i2c::i2cInput(int bytesReceived) {
|
||||||
}
|
}
|
||||||
switch (cmdRcvd[0]) {
|
switch (cmdRcvd[0]) {
|
||||||
case 0x00:
|
case 0x00:
|
||||||
senseInt = (long) cmdRcvd[1];
|
followerInt = (long) cmdRcvd[1];
|
||||||
return;
|
return;
|
||||||
break;
|
break;
|
||||||
case 0x01:
|
case 0x01:
|
||||||
|
|
|
||||||
|
|
@ -3,7 +3,7 @@
|
||||||
#ifdef I2C_INPUT
|
#ifdef I2C_INPUT
|
||||||
|
|
||||||
|
|
||||||
#define senseInt_Offset 0x00 // Integer of sense threshold in millivolts
|
#define followerInt_Offset 0x00 // Integer of sense threshold in millivolts
|
||||||
#define compInt_Offset 0x01 // Integer of comparator threshold in millivolts
|
#define compInt_Offset 0x01 // Integer of comparator threshold in millivolts
|
||||||
#define gainFactor_Offset 0x02 // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
|
#define gainFactor_Offset 0x02 // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
|
||||||
#define hysteresis_Offset 0x03 // Hysteresis value for ADC measurements
|
#define hysteresis_Offset 0x03 // Hysteresis value for ADC measurements
|
||||||
|
|
|
||||||
|
|
@ -1,211 +1,273 @@
|
||||||
|
#include <EEPROM.h>
|
||||||
|
|
||||||
/*------------------------------------------------*/
|
void parseData()
|
||||||
|
{
|
||||||
void parseData() {
|
|
||||||
|
|
||||||
// split the data into its parts
|
// split the data into its parts
|
||||||
|
|
||||||
char * strtokIndx; // this is used by strtok() as an index
|
|
||||||
|
|
||||||
strtokIndx = strtok(inputBuffer,","); // get the first part - the string
|
|
||||||
strcpy(serialMessageIn, strtokIndx); // copy it to serialMessageIn
|
|
||||||
|
|
||||||
strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
|
|
||||||
serialInt = atoi(strtokIndx); // convert this part to an integer
|
|
||||||
|
|
||||||
|
char *strtokIndx; // this is used by strtok() as an index
|
||||||
|
|
||||||
|
strtokIndx = strtok(inputBuffer, " "); // get the first part - the string
|
||||||
|
strcpy(serialMessageIn, strtokIndx); // copy it to serialMessageIn
|
||||||
|
|
||||||
|
strtokIndx = strtok(NULL, " "); // this continues where the previous call left off
|
||||||
|
serialInt = atoi(strtokIndx); // convert this part to an integer
|
||||||
}
|
}
|
||||||
/*------------------------------------------------*/
|
|
||||||
|
|
||||||
void identifyMarkers() {
|
void identifyMarkers()
|
||||||
|
{
|
||||||
char x = Serial.read();
|
char x = Serial.read();
|
||||||
// char y = Wire.read();
|
// char y = Wire.read();
|
||||||
|
|
||||||
if (x == endMarker) {
|
if (x == endMarker)
|
||||||
|
{
|
||||||
|
serialIncoming = true;
|
||||||
|
inputBuffer[bytesRecvd] = 0;
|
||||||
|
parseData();
|
||||||
|
bytesRecvd = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
inputBuffer[bytesRecvd] = x;
|
||||||
|
bytesRecvd++;
|
||||||
|
if (bytesRecvd == buffSize)
|
||||||
|
{
|
||||||
|
bytesRecvd = buffSize - 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef I2C
|
||||||
|
if (y == endMarker)
|
||||||
|
{
|
||||||
readInProgress = false;
|
readInProgress = false;
|
||||||
serialIncoming = true;
|
serialIncoming = true;
|
||||||
inputBuffer[bytesRecvd] = 0;
|
inputBuffer[bytesRecvd] = 0;
|
||||||
parseData();
|
parseData();
|
||||||
}
|
}
|
||||||
|
|
||||||
else if(readInProgress) {
|
if (readInProgress)
|
||||||
inputBuffer[bytesRecvd] = x;
|
{
|
||||||
bytesRecvd ++;
|
inputBuffer[bytesRecvd] = y;
|
||||||
if (bytesRecvd == buffSize) {
|
bytesRecvd++;
|
||||||
|
if (bytesRecvd == buffSize)
|
||||||
|
{
|
||||||
bytesRecvd = buffSize - 1;
|
bytesRecvd = buffSize - 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
else if (x == startMarker) {
|
if (y == startMarker)
|
||||||
bytesRecvd = 0;
|
{
|
||||||
|
bytesRecvd = 0;
|
||||||
readInProgress = true;
|
readInProgress = true;
|
||||||
}
|
}
|
||||||
#ifdef I2C
|
#endif
|
||||||
if (y == endMarker) {
|
|
||||||
readInProgress = false;
|
|
||||||
serialIncoming = true;
|
|
||||||
inputBuffer[bytesRecvd] = 0;
|
|
||||||
parseData();
|
|
||||||
}
|
|
||||||
|
|
||||||
if(readInProgress) {
|
|
||||||
inputBuffer[bytesRecvd] = y;
|
|
||||||
bytesRecvd ++;
|
|
||||||
if (bytesRecvd == buffSize) {
|
|
||||||
bytesRecvd = buffSize - 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (y == startMarker) {
|
|
||||||
bytesRecvd = 0;
|
|
||||||
readInProgress = true;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*------------------------------------------------*/
|
void updateGainFactor()
|
||||||
|
{
|
||||||
void updateTrigDuration() {
|
if (serialInt >= 0)
|
||||||
if (serialInt >= 0) {
|
{
|
||||||
TRG_DUR = serialInt;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
/*------------------------------------------------*/
|
|
||||||
|
|
||||||
void updateGainFactor() {
|
|
||||||
if (serialInt >= 0) {
|
|
||||||
GAIN_FACTOR = serialInt;
|
GAIN_FACTOR = serialInt;
|
||||||
|
adjustGain();
|
||||||
|
EEPROM.put(GAIN_FACTOR_ADDRESS, GAIN_FACTOR);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
/*------------------------------------------------*/
|
|
||||||
|
|
||||||
void updateVComp() {
|
void updateVFol()
|
||||||
if (serialInt >= 0) {
|
{
|
||||||
compInt = serialInt;
|
if (serialInt >= 0)
|
||||||
//senseInt = compInt; // syncing these params til #24 is fixed
|
{
|
||||||
|
followerThrs = serialInt;
|
||||||
|
adjustFollow();
|
||||||
|
EEPROM.put(FOLLOWER_THRESHOLD_ADDRESS, followerThrs);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
/*------------------------------------------------*/
|
|
||||||
|
|
||||||
void updateVAdj() {
|
void updateVComp()
|
||||||
if (serialInt >= 0) {
|
{
|
||||||
senseInt = serialInt;
|
if (serialInt >= 0)
|
||||||
//compInt = senseInt; // syncing these params til #24 is fixed
|
{
|
||||||
|
compThrs = serialInt;
|
||||||
|
adjustComp();
|
||||||
|
EEPROM.put(COMP_THRESHOLD_ADDRESS, compThrs);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
/*------------------------------------------------*/
|
|
||||||
|
|
||||||
void updateHysteresis() {
|
void updateLoopDuration()
|
||||||
if (serialInt >= 0) {
|
{
|
||||||
|
if (serialInt >= 0)
|
||||||
|
{
|
||||||
|
LOOP_DUR = serialInt;
|
||||||
|
EEPROM.put(LOOP_DUR_ADDRESS, LOOP_DUR);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void updateTrigDuration()
|
||||||
|
{
|
||||||
|
if (serialInt >= 0)
|
||||||
|
{
|
||||||
|
TRG_DUR = serialInt;
|
||||||
|
EEPROM.put(TRG_DUR_ADDRESS, TRG_DUR);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void updateHysteresis()
|
||||||
|
{
|
||||||
|
if (serialInt >= 0)
|
||||||
|
{
|
||||||
Hyst = serialInt;
|
Hyst = serialInt;
|
||||||
|
EEPROM.put(HYST_ADDRESS, Hyst);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
/*------------------------------------------------*/
|
|
||||||
|
|
||||||
void updateParams() {
|
void updateDebug()
|
||||||
if (strcmp(serialMessageIn, "TRG_D") == 0) {
|
{
|
||||||
updateTrigDuration();
|
if (serialInt > 0)
|
||||||
|
{
|
||||||
|
Debug = 1;
|
||||||
}
|
}
|
||||||
else if (strcmp(serialMessageIn, "GAIN_F") == 0) {
|
else if (serialInt == 0)
|
||||||
|
{
|
||||||
|
Debug = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void serialPrintConfig()
|
||||||
|
{
|
||||||
|
Serial.print("GAIN_F ");
|
||||||
|
Serial.print(GAIN_FACTOR);
|
||||||
|
switch (GAIN_FACTOR)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
Serial.println(" 3x");
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
Serial.println(" 3.5x");
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
Serial.println(" 4.33x");
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
Serial.println(" 6x");
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
Serial.println(" 11x");
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
Serial.println(" INVALID");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
Serial.print("VFOL ");
|
||||||
|
Serial.println(followerThrs);
|
||||||
|
|
||||||
|
Serial.print("VCOMP ");
|
||||||
|
Serial.println(compThrs);
|
||||||
|
|
||||||
|
Serial.print("LOOP_D ");
|
||||||
|
Serial.println(LOOP_DUR);
|
||||||
|
|
||||||
|
Serial.print("TRG_D ");
|
||||||
|
Serial.println(TRG_DUR);
|
||||||
|
|
||||||
|
Serial.print("HYST ");
|
||||||
|
Serial.println(Hyst);
|
||||||
|
}
|
||||||
|
|
||||||
|
void serialPrintState()
|
||||||
|
{
|
||||||
|
Serial.print("{");
|
||||||
|
|
||||||
|
Serial.print("\"Vcc\":");
|
||||||
|
Serial.print(Vin);
|
||||||
|
Serial.print(",");
|
||||||
|
|
||||||
|
Serial.print("\"VComp\":");
|
||||||
|
Serial.print(VComp);
|
||||||
|
Serial.print(",");
|
||||||
|
|
||||||
|
Serial.print("\"VFol\":");
|
||||||
|
Serial.print(VFol);
|
||||||
|
Serial.print(",");
|
||||||
|
|
||||||
|
Serial.print("\"Err\":");
|
||||||
|
Serial.print(ERR_STATE);
|
||||||
|
|
||||||
|
Serial.println("}");
|
||||||
|
}
|
||||||
|
|
||||||
|
void updateParams()
|
||||||
|
{
|
||||||
|
serialIncoming = false;
|
||||||
|
if (strcmp(serialMessageIn, "GAIN_F") == 0)
|
||||||
|
{
|
||||||
updateGainFactor();
|
updateGainFactor();
|
||||||
}
|
}
|
||||||
else if (strcmp(serialMessageIn, "VCOMP") == 0) {
|
else if (strcmp(serialMessageIn, "VFOL") == 0)
|
||||||
|
{
|
||||||
|
updateVFol();
|
||||||
|
}
|
||||||
|
else if (strcmp(serialMessageIn, "VCOMP") == 0)
|
||||||
|
{
|
||||||
updateVComp();
|
updateVComp();
|
||||||
}
|
}
|
||||||
else if (strcmp(serialMessageIn, "VADJ") == 0) {
|
else if (strcmp(serialMessageIn, "LOOP_D") == 0)
|
||||||
updateVAdj();
|
{
|
||||||
|
updateTrigDuration();
|
||||||
}
|
}
|
||||||
else if (strcmp(serialMessageIn, "HYST") == 0) {
|
else if (strcmp(serialMessageIn, "TRG_D") == 0)
|
||||||
|
{
|
||||||
|
updateTrigDuration();
|
||||||
|
}
|
||||||
|
else if (strcmp(serialMessageIn, "HYST") == 0)
|
||||||
|
{
|
||||||
updateHysteresis();
|
updateHysteresis();
|
||||||
}
|
}
|
||||||
else if (strcmp(serialMessageIn, "HELP") == 0) {
|
else if (strcmp(serialMessageIn, "DEBUG") == 0)
|
||||||
Serial.println("To change trigger active duration: TRG_D [integer for milliseconds]");
|
{
|
||||||
Serial.println("To change gain factor: GAIN_F [integer for gain state - see note*]");
|
updateDebug();
|
||||||
Serial.println("To change ADC hysteresis value: HYST [integer]");
|
}
|
||||||
Serial.println("To change sensor input pullup vRef low threshold: VADJ [float value]");
|
else if (strcmp(serialMessageIn, "CONFIG") == 0)
|
||||||
Serial.println("To change comparator trigger high threshold: VCOMP [float value]");
|
{
|
||||||
Serial.println("");
|
serialPrintConfig();
|
||||||
Serial.println("These commands should be wrapped in this format:");
|
}
|
||||||
Serial.println("<CMD, INT>");
|
else if (strcmp(serialMessageIn, "RESET") == 0)
|
||||||
Serial.println("");
|
{
|
||||||
Serial.println("Examples:");
|
resetConfig();
|
||||||
Serial.println("<GAIN_F, 3> <~ set gain factor to index 3 (6x)");
|
serialPrintConfig();
|
||||||
Serial.println("<VADJ, 2350> <~ set the vref floor to 2.35V");
|
}
|
||||||
parseData();
|
else if (strcmp(serialMessageIn, "STATE") == 0)
|
||||||
|
{
|
||||||
|
serialPrintState();
|
||||||
|
}
|
||||||
|
else if (strcmp(serialMessageIn, "HELP") == 0)
|
||||||
|
{
|
||||||
|
// Serial.println("To change gain factor: GAIN_F [integer for gain state - see note*]");
|
||||||
|
// Serial.println("To change voltage follower voltage (low threshold): VFOL [float value]");
|
||||||
|
// Serial.println("To change comparator voltage (high threshold): VCOMP [float value]");
|
||||||
|
// Serial.println("To change main loop period: LOOP_D [integer for milliseconds]");
|
||||||
|
// Serial.println("To change trigger active duration: TRG_D [integer for milliseconds]");
|
||||||
|
// Serial.println("To change ADC hysteresis value: HYST [integer]");
|
||||||
|
// Serial.println("To enable or disable debug output: DEBUG [0|1]");
|
||||||
|
// Serial.println("To print current config: CONFIG");
|
||||||
|
// Serial.println("To reset config to defaults: RESET");
|
||||||
|
// Serial.println("To print current state: STATE");
|
||||||
|
// Serial.println("");
|
||||||
|
// Serial.println("Commands are entered in this format:");
|
||||||
|
// Serial.println("CMD VAL");
|
||||||
|
// Serial.println("Commands are confirmed with Enter key");
|
||||||
|
// Serial.println("");
|
||||||
|
// Serial.println("Examples:");
|
||||||
|
// Serial.println("GAIN_F 3 <~ set gain factor to index 3 (6x)");
|
||||||
|
// Serial.println("VFOL 2350 <~ set the vref floor to 2.35V");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*------------------------------------------------*/
|
void serialInput()
|
||||||
|
{
|
||||||
void serialInput() {
|
|
||||||
|
|
||||||
// receive data from Serial and save it into inputBuffer
|
// receive data from Serial and save it into inputBuffer
|
||||||
|
if (Serial.available() > 0)
|
||||||
if(Serial.available() > 0) {
|
{
|
||||||
|
|
||||||
// the order of these IF clauses is significant
|
// the order of these IF clauses is significant
|
||||||
identifyMarkers();
|
identifyMarkers();
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*------------------------------------------------*/
|
|
||||||
|
|
||||||
void serialReply() {
|
|
||||||
#ifndef VERBOSE
|
|
||||||
if (serialIncoming) {
|
|
||||||
serialIncoming = false;
|
|
||||||
#endif
|
|
||||||
#ifdef DEBUG
|
|
||||||
Serial.print("Vcc:");
|
|
||||||
Serial.println(Vin);
|
|
||||||
Serial.print("Comp Sense:");
|
|
||||||
Serial.print(VComp);
|
|
||||||
Serial.print(" ");
|
|
||||||
Serial.print("Comparator State:");
|
|
||||||
Serial.print(ADJ_COMP);
|
|
||||||
Serial.print(" ");
|
|
||||||
Serial.println(compInt);
|
|
||||||
|
|
||||||
Serial.print("Amp Sense:");
|
|
||||||
Serial.print(VAdj);
|
|
||||||
Serial.print(" ");
|
|
||||||
Serial.print("Follower State:");
|
|
||||||
Serial.print(ADJ_FOLLOW);
|
|
||||||
Serial.print(" ");
|
|
||||||
Serial.println(senseInt);
|
|
||||||
|
|
||||||
Serial.print("Gain Factor:");
|
|
||||||
Serial.print(GAIN_FACTOR);
|
|
||||||
switch (GAIN_FACTOR) {
|
|
||||||
case 0:
|
|
||||||
Serial.println(" 3x");
|
|
||||||
break;
|
|
||||||
case 1:
|
|
||||||
Serial.println(" 3.5x");
|
|
||||||
break;
|
|
||||||
case 2:
|
|
||||||
Serial.println(" 4.33x");
|
|
||||||
break;
|
|
||||||
case 3:
|
|
||||||
Serial.println(" 6x");
|
|
||||||
break;
|
|
||||||
case 4:
|
|
||||||
Serial.println(" 11x");
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
Serial.println(" INVALID");
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
Serial.print("Delay:");
|
|
||||||
Serial.println(TRG_DUR);
|
|
||||||
Serial.print("Error State:");
|
|
||||||
Serial.println(ERR_STATE);
|
|
||||||
Serial.println("------------------");
|
|
||||||
#ifndef VERBOSE
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
|
||||||
|
|
@ -10,20 +10,16 @@ int VLast = 0;
|
||||||
|
|
||||||
// Convert threshold values based on the input voltage
|
// Convert threshold values based on the input voltage
|
||||||
|
|
||||||
long senseLong = senseThrs * 1024L;
|
long followerLong = followerThrs * 1023L;
|
||||||
long compLong = compThrs * 1024L;
|
long compLong = compThrs * 1023L;
|
||||||
long senseInt;
|
long followerInt;
|
||||||
long compInt;
|
long compInt;
|
||||||
|
|
||||||
// Voltage Comparator Adjustment parameters
|
// Voltage Comparator Adjustment parameters
|
||||||
int VComp = 0;
|
int VComp = 0;
|
||||||
int diffCompL = VComp - compInt;
|
|
||||||
int diffCompH = compInt - VComp;
|
|
||||||
|
|
||||||
// Voltage Follower Adjustment parameters
|
// Voltage Follower Adjustment parameters
|
||||||
int VAdj = 0;
|
int VFol = 0;
|
||||||
int diffAdjL = VAdj - senseInt;
|
|
||||||
int diffAdjH = senseInt - VAdj;
|
|
||||||
|
|
||||||
// Error blink parameters
|
// Error blink parameters
|
||||||
|
|
||||||
|
|
@ -33,10 +29,8 @@ int BlinkCount = InitCount * 2; // Multiply Blink count by 2 to handle
|
||||||
// Serial Input Parsing Variables
|
// Serial Input Parsing Variables
|
||||||
#define buffSize 40
|
#define buffSize 40
|
||||||
char inputBuffer[buffSize];
|
char inputBuffer[buffSize];
|
||||||
#define startMarker '<'
|
#define endMarker '\n'
|
||||||
#define endMarker '>'
|
|
||||||
byte bytesRecvd = 0;
|
byte bytesRecvd = 0;
|
||||||
bool readInProgress = false;
|
|
||||||
bool serialIncoming = false;
|
bool serialIncoming = false;
|
||||||
char serialMessageIn[buffSize] = {0};
|
char serialMessageIn[buffSize] = {0};
|
||||||
int serialInt = 0;
|
int serialInt = 0;
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue