419 lines
15 KiB
C++
419 lines
15 KiB
C++
// Sample sketch for the SonarMote - Standalone parking assist with RGB LED indicator
|
|
// This example uses the NewPing library from https://code.google.com/p/arduino-new-ping/
|
|
// but that could be replaced by raw reading of the sonar sensor as seen in other SonarMote examples
|
|
// More info/photos at: http://lowpowerlab.com/sonar
|
|
// Ultrasonic sensor (HC-SR04) connected to D6 (Trig), D7 (Echo), and power enabled through D5
|
|
// This sketch sleeps the Moteino and sensor most of the time. It wakes up every few seconds to take
|
|
// a distance reading. If it detects an approaching object (car) it increases the sampling rate
|
|
// and starts lighting up the LED (from green to yellow to red to blinking red). Once there is no more
|
|
// motion the LED is turned off and the cycle is back to a few seconds in between sensor reads.
|
|
// Button is connected on D3. Holding the button for a few seconds enters the "red zone adjust" mode (RZA).
|
|
// By default the red zone limit is at 25cm (LED turns RED below this and starts blinking faster and faster).
|
|
// In RZA, readings are taken for 5 seconds. In this time you have the chance to set a new red zone limit.
|
|
// Valid new red zone readings are between the RED__LIMIT_UPPER (default 25cm) and MAX_ADJUST_DISTANCE (cm).
|
|
// In RZA mode the BLU Led blinks fast to indicate new red limit distance. It blinks slow if the readings are invalid
|
|
// If desired this value could be saved to EEPROM to persist if unit is turned off
|
|
// Make sure you adjust the settings in the configuration section below !!!
|
|
|
|
// **********************************************************************************
|
|
// Copyright Felix Rusu, LowPowerLab.com
|
|
// Library and code by Felix Rusu - felix@lowpowerlab.com
|
|
// **********************************************************************************
|
|
// License
|
|
// **********************************************************************************
|
|
// This program is free software; you can redistribute it
|
|
// and/or modify it under the terms of the GNU General
|
|
// Public License as published by the Free Software
|
|
// Foundation; either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// This program is distributed in the hope that it will
|
|
// be useful, but WITHOUT ANY WARRANTY; without even the
|
|
// implied warranty of MERCHANTABILITY or FITNESS FOR A
|
|
// PARTICULAR PURPOSE. See the GNU General Public
|
|
// License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General
|
|
// Public License along with this program.
|
|
// If not, see <http://www.gnu.org/licenses/>.
|
|
//
|
|
// Licence can be viewed at
|
|
// http://www.gnu.org/licenses/gpl-3.0.txt
|
|
//
|
|
// Please maintain this license information along with authorship
|
|
// and copyright notices in any redistribution of this code
|
|
// **********************************************************************************
|
|
#include <NewPing.h> // get this library at: https://code.google.com/p/arduino-new-ping/
|
|
#include <LowPower.h> //get library from: https://github.com/lowpowerlab/lowpower
|
|
//writeup here: http://www.rocketscream.com/blog/2011/07/04/lightweight-low-power-arduino-library/
|
|
|
|
#define TRIG 6 // digital pin wired to TRIG pin of ultrasonic sensor
|
|
#define ECHO 7 // digital pin wired to ECHO pin of ultrasonic sensor
|
|
#define SENSOR_EN 5 // digital pin that enables power to ultrasonic sensor
|
|
#define RED A0 // pin connected to red LED
|
|
#define GRN A1 // pin connected to green LED
|
|
#define BLU A2 // pin connected to blue LED
|
|
#define BUTTON_INT 1 // user button on interrupt 1 (D3)
|
|
#define BUTTON_PIN 3 // user button on interrupt 1 (D3)
|
|
#define BUTTON_HOLD_MS 3000 // hold button this many ms before entering red zone adjust
|
|
#define MOTEINOLED 9 // moteino onboard LED
|
|
#define MAX_DISTANCE 220 // maximum valid distance
|
|
#define MIN_DISTANCE 2 // minimum valid distance
|
|
|
|
#define GRN_LIMIT_UPPER 180+redZoneAdjust // upper limit distance for GREEN
|
|
#define YLW_LIMIT_UPPER 40+redZoneAdjust // upper limit distance for YELLOW
|
|
#define RED_LIMIT_UPPER 25+redZoneAdjust // upper limit distance for RED
|
|
|
|
#define MAX_ADJUST_DISTANCE (MAX_DISTANCE-GRN_LIMIT_UPPER) //this is the amount by which the RED_LIMIT_UPPER can by increased
|
|
|
|
//possible states of LED status
|
|
#define STATE_SOLID 0
|
|
#define STATE_BLINK 1
|
|
|
|
//possible states for LED color
|
|
#define STATE_OFF 0
|
|
#define STATE_GRN 1
|
|
#define STATE_YLW 2
|
|
#define STATE_RED 3
|
|
|
|
byte state_LED = STATE_SOLID;
|
|
byte state_LEDCOLOR = STATE_OFF;
|
|
byte state_LEDONOFF = LOW;
|
|
byte redZoneAdjust = 0; //this is adjustable via the button (press button for a few seconds, then take a reading)
|
|
|
|
#define LED_RED {digitalWrite(RED,HIGH);digitalWrite(GRN,LOW);}
|
|
#define LED_GRN {digitalWrite(RED,LOW);digitalWrite(GRN,HIGH);}
|
|
#define LED_YLW {digitalWrite(RED,HIGH);digitalWrite(GRN,HIGH);}
|
|
#define LED_OFF {digitalWrite(RED,LOW);digitalWrite(GRN,LOW);}
|
|
|
|
#define SERIAL_EN //uncomment if you want serial debugging output
|
|
#ifdef SERIAL_EN
|
|
#define SERIAL_BAUD 115200
|
|
#define DEBUG(input) {Serial.print(input);}
|
|
#define DEBUGln(input) {Serial.println(input);}
|
|
#define SERIALFLUSH() {Serial.flush();}
|
|
#else
|
|
#define DEBUG(input);
|
|
#define DEBUGln(input);
|
|
#define SERIALFLUSH();
|
|
#endif
|
|
|
|
#define SLEEP_MINILOOP SLEEP_15MS
|
|
#define SLEEP_LOOP SLEEP_250MS
|
|
#define SLEEP_LONG SLEEP_2S
|
|
#define SLEEP_LOBATT SLEEP_4S
|
|
#define SLEEP_HIBERNATE SLEEP_8S
|
|
|
|
#define BATT_MONITOR A7 // Sense VBAT_COND signal (when powered externally should read ~3.25v/3.3v (1000-1023), when external power is cutoff it should start reading around 2.85v/3.3v * 1023 ~= 883 (ratio given by 10k+4.7K divider from VBAT_COND = 1.47 multiplier)
|
|
#define BATT_CYCLES 120 // read and report battery voltage every this many sleep cycles (ex 30cycles * 8sec sleep = 240sec/4min). For 450 cyclesyou would get ~1 hour intervals
|
|
#define BATT_FORMULA(reading) reading * 0.00322 * 1.475 // >>> fine tune this parameter to match your voltage when fully charged
|
|
#define BATT_LOW 3.35
|
|
|
|
NewPing sensor(TRIG, ECHO, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
|
|
float readDistance(byte samples=3); //take 3 samples by default
|
|
void checkBattery(byte samples=10); //take 10 samples by default
|
|
float batteryVolts = 5;
|
|
|
|
void setup() {
|
|
#ifdef SERIAL_EN
|
|
Serial.begin(SERIAL_BAUD); // Open serial monitor at 115200 baud to see ping results.
|
|
#endif
|
|
pinMode(TRIG, OUTPUT);
|
|
pinMode(ECHO, INPUT);
|
|
pinMode(RED, OUTPUT);
|
|
pinMode(GRN, OUTPUT);
|
|
pinMode(BLU, OUTPUT);
|
|
pinMode(SENSOR_EN, OUTPUT);
|
|
digitalWrite(SENSOR_EN, LOW);
|
|
pinMode(BUTTON_PIN, INPUT_PULLUP);
|
|
attachInterrupt(BUTTON_INT, buttonInterrupt, FALLING);
|
|
}
|
|
|
|
#define FLAG_INTERRUPT 0x01
|
|
volatile int mainEventFlags = 0;
|
|
boolean buttonPressed = false;
|
|
void buttonInterrupt()
|
|
{
|
|
mainEventFlags |= FLAG_INTERRUPT;
|
|
}
|
|
|
|
long distance=0;
|
|
long lastDistance=0;
|
|
long lastSigDistance=0;
|
|
byte loops=0;
|
|
byte miniLoops=0;
|
|
byte skipBlinkingLoops=5;
|
|
unsigned long now=0;
|
|
period_t sleepTime = SLEEP_LONG; //period_t is an enum type defined in the LowPower library (LowPower.h)
|
|
|
|
void loop() {
|
|
if (mainEventFlags & FLAG_INTERRUPT)
|
|
{
|
|
LowPower.powerDown(SLEEP_30MS, ADC_OFF, BOD_ON);
|
|
mainEventFlags &= ~FLAG_INTERRUPT;
|
|
if (!digitalRead(BUTTON_PIN)) {
|
|
buttonPressed=true;
|
|
}
|
|
}
|
|
|
|
if (buttonPressed)
|
|
{
|
|
detachInterrupt(BUTTON_INT);
|
|
DEBUGln("BUTTON PRESS!");
|
|
unsigned long timestamp = millis();
|
|
while (millis() - timestamp < BUTTON_HOLD_MS)
|
|
{
|
|
if (digitalRead(BUTTON_PIN))
|
|
{
|
|
buttonPressed = false;
|
|
break;
|
|
}
|
|
DEBUG('.');SERIALFLUSH();
|
|
LowPower.powerDown(SLEEP_30MS, ADC_OFF, BOD_ON);
|
|
timestamp-=40;
|
|
}
|
|
|
|
//if it's still pressed after BUTTON_HOLD_MS then enter red zone adjust mode
|
|
if (buttonPressed)
|
|
{
|
|
DEBUG("STILL_PRESSED");SERIALFLUSH();
|
|
handleRedZoneAdjust();
|
|
}
|
|
else
|
|
{
|
|
DEBUG("ABORTED");SERIALFLUSH();
|
|
}
|
|
attachInterrupt(BUTTON_INT, buttonInterrupt, FALLING);
|
|
}
|
|
|
|
|
|
if (miniLoops>0)
|
|
{
|
|
miniLoops--;
|
|
sleepTime = SLEEP_MINILOOP;
|
|
//when looping fast we need to wake the sensor about 60-75ms before doing a reading (about 5 miniloops assuming 1 miniloop=15ms)
|
|
//otherwise there will be a visible delay in the LED blinking
|
|
if (miniLoops == 5) digitalWrite(SENSOR_EN, HIGH);
|
|
}
|
|
else if (loops > 0)
|
|
{
|
|
loops--;
|
|
miniLoops=16; //16 mini loops translate
|
|
sleepTime = SLEEP_MINILOOP;
|
|
}
|
|
else
|
|
//sleep longer when no significant state changes happened
|
|
//if battery is low, sleep even longer to try to squeeze more life
|
|
sleepTime = (batteryVolts > BATT_LOW ? SLEEP_LONG : SLEEP_LOBATT);
|
|
|
|
if ((loops == 0 && miniLoops == 0) || ((miniLoops % skipBlinkingLoops) == 0))
|
|
{
|
|
DEBUG('*');
|
|
handleLEDState();
|
|
}
|
|
|
|
SERIALFLUSH(); //flush any characters in the serial buffer before sleeping otherwise they get lost or garbled
|
|
LowPower.powerDown(sleepTime, ADC_OFF, BOD_OFF); //put microcontroller to sleep to save battery life
|
|
if (miniLoops > 0) { DEBUG('.');return; } //as long as we still have miniloops we skip readings
|
|
//only proceed to a reading every loop
|
|
now = millis();
|
|
distance = readDistance();
|
|
|
|
DEBUGln();
|
|
DEBUG("Read: ");
|
|
DEBUG(distance); // Convert ping time to distance in cm and print result (0 = outside set distance range)
|
|
DEBUG("cm");
|
|
DEBUG(" [");
|
|
DEBUG(millis()-now);
|
|
DEBUGln("]ms");
|
|
|
|
if (distance > MAX_DISTANCE || distance < MIN_DISTANCE)
|
|
{
|
|
DEBUGln("Out of range");
|
|
loops=0;
|
|
lastDistance = distance;
|
|
return;
|
|
}
|
|
|
|
if (distance < GRN_LIMIT_UPPER && distance > YLW_LIMIT_UPPER && abs(lastSigDistance-distance)>20)
|
|
{
|
|
if (distance < lastSigDistance)
|
|
loops=12; //begin looping fast only when object is approaching
|
|
lastSigDistance = distance;
|
|
}
|
|
else if (distance < YLW_LIMIT_UPPER && abs(lastSigDistance-distance)>5)
|
|
{
|
|
if (distance < lastSigDistance)
|
|
loops=12; //begin looping fast only when object is approaching
|
|
lastSigDistance = distance;
|
|
}
|
|
|
|
//if the looping was started, determine the state we're in
|
|
if (loops > 0)
|
|
{
|
|
if (distance >= YLW_LIMIT_UPPER) { state_LEDCOLOR = STATE_GRN; state_LED = STATE_SOLID; }
|
|
else if (distance >= RED_LIMIT_UPPER) { state_LEDCOLOR=STATE_YLW; state_LED = STATE_SOLID; }
|
|
else { state_LEDCOLOR=STATE_RED; state_LED = STATE_BLINK; }
|
|
}
|
|
else { state_LEDCOLOR=STATE_OFF; state_LED = STATE_SOLID; }
|
|
|
|
//adjust the blinking rate based on the distance to the object
|
|
if (state_LEDCOLOR==STATE_RED)
|
|
{
|
|
if (distance > RED_LIMIT_UPPER-2)
|
|
skipBlinkingLoops = 8;
|
|
else if (distance > RED_LIMIT_UPPER-6)
|
|
skipBlinkingLoops = 6;
|
|
else if (lastDistance > RED_LIMIT_UPPER-10)
|
|
skipBlinkingLoops = 4;
|
|
else if (lastDistance > RED_LIMIT_UPPER-14)
|
|
skipBlinkingLoops = 2;
|
|
else
|
|
{
|
|
skipBlinkingLoops = 1;
|
|
state_LED = STATE_SOLID;
|
|
}
|
|
}
|
|
else skipBlinkingLoops = 1;
|
|
lastDistance = distance; //remember the last reading
|
|
checkBattery();
|
|
if (batteryVolts < BATT_LOW)
|
|
Blink(BLU);
|
|
else Blink(MOTEINOLED);
|
|
DEBUG("Batt: ");
|
|
DEBUG(batteryVolts);
|
|
DEBUGln("v");
|
|
}
|
|
|
|
//reads the ultrasonic sensor, takes 3 samples by default
|
|
float uS;
|
|
float readDistance(byte samples)
|
|
{
|
|
uS = 0;
|
|
if (loops == 0 && miniLoops == 0)
|
|
{
|
|
digitalWrite(SENSOR_EN, HIGH);
|
|
//need about 60-75ms after power up before HC-SR04 will be usable, so just sleep in the meantime
|
|
LowPower.powerDown(SLEEP_60MS, ADC_OFF, BOD_OFF);
|
|
LowPower.powerDown(SLEEP_15MS, ADC_OFF, BOD_OFF);
|
|
}
|
|
|
|
sensor.ping();
|
|
for (byte i=0; i<samples; i++)
|
|
{
|
|
uS += sensor.ping(); // Send ping, get ping time in microseconds (uS).
|
|
if (samples >1) delay(4); //need a short delay between samples
|
|
}
|
|
digitalWrite(SENSOR_EN, LOW);
|
|
return (uS / samples) / US_ROUNDTRIP_CM;
|
|
}
|
|
|
|
|
|
////reads the ultrasonic sensor, takes 3 samples by default
|
|
//float readDistance(byte samples)
|
|
//{
|
|
// long duration, distance;
|
|
// digitalWrite(SENSOR_EN, HIGH);
|
|
// delay(75);
|
|
//
|
|
// digitalWrite(TRIG, LOW); // Added this line
|
|
// delayMicroseconds(2); // Added this line
|
|
// digitalWrite(TRIG, HIGH);
|
|
// delayMicroseconds(10); // Added this line
|
|
// digitalWrite(TRIG, LOW);
|
|
// pulseIn(ECHO, HIGH);
|
|
//
|
|
// digitalWrite(TRIG, LOW); // Added this line
|
|
// delayMicroseconds(2); // Added this line
|
|
// digitalWrite(TRIG, HIGH);
|
|
// delayMicroseconds(10); // Added this line
|
|
// digitalWrite(TRIG, LOW);
|
|
// duration = pulseIn(ECHO, HIGH);
|
|
// distance = (duration/2) / 29.1;
|
|
// digitalWrite(SENSOR_EN, LOW);
|
|
// return distance;
|
|
//}
|
|
|
|
//handles the status and color of the LED depending what state we are in
|
|
void handleLEDState()
|
|
{
|
|
switch(state_LEDCOLOR)
|
|
{
|
|
case STATE_OFF: LED_OFF; break;
|
|
case STATE_GRN: LED_GRN; break;
|
|
case STATE_YLW: LED_YLW; break;
|
|
case STATE_RED:
|
|
if (state_LED == STATE_BLINK)
|
|
{
|
|
if (state_LEDONOFF == HIGH)
|
|
{
|
|
LED_OFF;
|
|
state_LEDONOFF = LOW;
|
|
}
|
|
else
|
|
{
|
|
LED_RED;
|
|
state_LEDONOFF = HIGH;
|
|
}
|
|
}
|
|
else LED_RED;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Blink(byte pin)
|
|
{
|
|
pinMode(pin, OUTPUT);
|
|
digitalWrite(pin, HIGH);
|
|
delay(2);
|
|
digitalWrite(pin, LOW);
|
|
}
|
|
|
|
byte cycleCount=BATT_CYCLES;
|
|
void checkBattery(byte samples)
|
|
{
|
|
if (cycleCount++ == BATT_CYCLES) //only read battery every BATT_CYCLES sleep cycles
|
|
{
|
|
unsigned int readings=0;
|
|
for (byte i=0; i<samples; i++) //take 10 samples, and average
|
|
readings+=analogRead(BATT_MONITOR);
|
|
batteryVolts = BATT_FORMULA((float)readings / samples);
|
|
cycleCount = 0;
|
|
}
|
|
}
|
|
|
|
//enter red zone adjust mode
|
|
void handleRedZoneAdjust()
|
|
{
|
|
DEBUGln("\nRED_ZONE_ADJUST");SERIALFLUSH();
|
|
LED_OFF; //turn off all other LEDs
|
|
|
|
unsigned long startTimestamp=millis();
|
|
float distance;
|
|
int adjustTime = 5000;
|
|
byte state = HIGH;
|
|
|
|
while (millis()-startTimestamp < adjustTime)
|
|
{
|
|
digitalWrite(BLU, state);
|
|
state = state ? LOW : HIGH; //flip state for next loop
|
|
distance = readDistance();
|
|
if (distance > MAX_ADJUST_DISTANCE + RED_LIMIT_UPPER-redZoneAdjust)
|
|
delay(300);
|
|
else if (distance <= RED_LIMIT_UPPER-redZoneAdjust)
|
|
state = HIGH; //keep LED on
|
|
else
|
|
delay (distance);
|
|
|
|
DEBUG(distance);DEBUGln("cm");SERIALFLUSH();
|
|
Blink(MOTEINOLED);
|
|
}
|
|
|
|
digitalWrite(BLU, LOW); //turn LED off
|
|
if (distance > RED_LIMIT_UPPER-redZoneAdjust && distance <= MAX_ADJUST_DISTANCE + RED_LIMIT_UPPER-redZoneAdjust)
|
|
{
|
|
redZoneAdjust = distance - RED_LIMIT_UPPER - redZoneAdjust;
|
|
DEBUG("New RED_ZONE_SHIFT = "); DEBUGln(redZoneAdjust);SERIALFLUSH();
|
|
}
|
|
}
|