|
|
|
#include <avr/io.h> // register definitions
|
|
|
|
#include <avr/pgmspace.h> // storing data in program memory
|
|
|
|
#include <avr/interrupt.h> // interrupt vectors
|
|
|
|
#include <util/delay.h> // delay functions
|
|
|
|
|
|
|
|
#include <stdint.h>
|
|
|
|
#include <stdbool.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
|
|
|
|
// Include stuff from the library
|
|
|
|
#include "lib/iopins.h"
|
|
|
|
#include "lib/usart.h"
|
|
|
|
#include "lib/nsdelay.h"
|
|
|
|
|
|
|
|
#define WS_PIN 7
|
|
|
|
|
|
|
|
#define TRIG1_PIN 3
|
|
|
|
#define TRIG2_PIN 9
|
|
|
|
#define TRIG3_PIN 11
|
|
|
|
|
|
|
|
#define ECHO1_PIN 2
|
|
|
|
#define ECHO2_PIN 8
|
|
|
|
#define ECHO3_PIN 10
|
|
|
|
|
|
|
|
|
|
|
|
/** Wait long enough for the colors to show */
|
|
|
|
static inline __attribute__((always_inline))
|
|
|
|
void ws_show(void)
|
|
|
|
{
|
|
|
|
_delay_us(10);
|
|
|
|
}
|
|
|
|
|
|
|
|
/** Send one byte to the RGB strip */
|
|
|
|
static inline __attribute__((always_inline))
|
|
|
|
void ws_send_byte(uint8_t bb)
|
|
|
|
{
|
|
|
|
for (int8_t i = 8; i > 0; i--) {
|
|
|
|
pin_up(WS_PIN);
|
|
|
|
if (bb & 0x80) {
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
pin_down(WS_PIN);
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
} else {
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
pin_down(WS_PIN);
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
__asm__ volatile("nop");
|
|
|
|
}
|
|
|
|
|
|
|
|
bb = (uint8_t)(bb << 1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void ws_send_rgb(uint8_t r, uint8_t g, uint8_t b)
|
|
|
|
{
|
|
|
|
ws_send_byte(g);
|
|
|
|
ws_send_byte(r);
|
|
|
|
ws_send_byte(b);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void hw_init(void)
|
|
|
|
{
|
|
|
|
usart_init(BAUD_115200);
|
|
|
|
|
|
|
|
as_output(WS_PIN);
|
|
|
|
|
|
|
|
as_input_pu(ECHO1_PIN);
|
|
|
|
as_input_pu(ECHO2_PIN);
|
|
|
|
as_input_pu(ECHO3_PIN);
|
|
|
|
|
|
|
|
as_output(TRIG1_PIN);
|
|
|
|
as_output(TRIG2_PIN);
|
|
|
|
as_output(TRIG3_PIN);
|
|
|
|
|
|
|
|
as_output(13);
|
|
|
|
}
|
|
|
|
|
|
|
|
typedef enum {
|
|
|
|
MEAS_WAIT_1,
|
|
|
|
MEAS_WAIT_0,
|
|
|
|
MEAS_DONE
|
|
|
|
} MeasPhase;
|
|
|
|
|
|
|
|
|
|
|
|
#define MBUF_LEN 16
|
|
|
|
typedef struct {
|
|
|
|
float data[MBUF_LEN];
|
|
|
|
} MBuf;
|
|
|
|
|
|
|
|
static float mbuf_add(MBuf *buf, float value)
|
|
|
|
{
|
|
|
|
float aggr = value;
|
|
|
|
for (int i = MBUF_LEN - 1; i > 0; i--) {
|
|
|
|
float m = buf->data[i - 1];
|
|
|
|
aggr += m;
|
|
|
|
buf->data[i] = m;
|
|
|
|
}
|
|
|
|
|
|
|
|
buf->data[0] = value;
|
|
|
|
|
|
|
|
return aggr / (float)MBUF_LEN;
|
|
|
|
}
|
|
|
|
|
|
|
|
static MBuf mb_offs1;
|
|
|
|
static MBuf mb_offs2;
|
|
|
|
static MBuf mb_offs3;
|
|
|
|
|
|
|
|
typedef struct {
|
|
|
|
uint8_t r;
|
|
|
|
uint8_t g;
|
|
|
|
uint8_t b;
|
|
|
|
} RGB;
|
|
|
|
|
|
|
|
static RGB history[30];
|
|
|
|
|
|
|
|
|
|
|
|
static uint16_t meas(MBuf *mbuf, uint8_t TRIG, uint8_t ECHO)
|
|
|
|
{
|
|
|
|
_delay_ms(6);
|
|
|
|
pin_up_n(TRIG);
|
|
|
|
_delay_ms(1);
|
|
|
|
pin_down_n(TRIG);
|
|
|
|
|
|
|
|
MeasPhase meas_phase = MEAS_WAIT_1;
|
|
|
|
|
|
|
|
uint32_t echo = 0;
|
|
|
|
|
|
|
|
TCNT1 = 0;
|
|
|
|
TCCR1B = (0b010 << CS10);
|
|
|
|
|
|
|
|
while (true) {
|
|
|
|
if (meas_phase == MEAS_WAIT_1) {
|
|
|
|
if (pin_is_high_n(ECHO)) {
|
|
|
|
echo = TCNT1;
|
|
|
|
meas_phase = MEAS_WAIT_0;
|
|
|
|
}
|
|
|
|
} else if (meas_phase == MEAS_WAIT_0) {
|
|
|
|
if (pin_is_low_n(ECHO)) {
|
|
|
|
echo = TCNT1 - echo;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// timeout
|
|
|
|
if (TCNT1 >= 15000) {
|
|
|
|
echo = 15000;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
TCCR1B = 0; // stop
|
|
|
|
|
|
|
|
// Both pulses measured with 0.5us accuracy
|
|
|
|
|
|
|
|
// Convert to mm
|
|
|
|
|
|
|
|
float offset = 255 - echo / (1.25f * 25.0f); //25
|
|
|
|
|
|
|
|
if (offset > 255) {
|
|
|
|
offset = 255;
|
|
|
|
} else if (offset < 0) {
|
|
|
|
offset = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
offset = mbuf_add(mbuf, offset);
|
|
|
|
|
|
|
|
return (uint16_t) roundf(offset); // converted to RGB-level
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void sonar_measure(void)
|
|
|
|
{
|
|
|
|
uint16_t c1 = meas(&mb_offs1, TRIG1_PIN, ECHO1_PIN);
|
|
|
|
uint16_t c2 = meas(&mb_offs2, TRIG2_PIN, ECHO2_PIN);
|
|
|
|
uint16_t c3 = meas(&mb_offs3, TRIG3_PIN, ECHO3_PIN);
|
|
|
|
|
|
|
|
for (int i = 30 - 1; i > 0; i--) {
|
|
|
|
history[i].r = history[i - 1].r;
|
|
|
|
history[i].g = history[i - 1].g;
|
|
|
|
history[i].b = history[i - 1].b;
|
|
|
|
}
|
|
|
|
|
|
|
|
history[0].r = (uint8_t)c1;
|
|
|
|
history[0].g = (uint8_t)c2;
|
|
|
|
history[0].b = (uint8_t)c3;
|
|
|
|
|
|
|
|
for (int i = 0; i < 30; i++) {
|
|
|
|
ws_send_rgb(history[i].r, history[i].g, history[i].b);
|
|
|
|
}
|
|
|
|
|
|
|
|
ws_show();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int main(void)
|
|
|
|
{
|
|
|
|
hw_init();
|
|
|
|
|
|
|
|
usart_puts_P(PSTR("===========================\r\n"));
|
|
|
|
usart_puts_P(PSTR("\r\n"));
|
|
|
|
usart_puts_P(PSTR("RGB SONAR DEMO\r\n"));
|
|
|
|
usart_puts_P(PSTR("FEL CVUT K338\r\n"));
|
|
|
|
usart_puts_P(PSTR("\r\n"));
|
|
|
|
usart_puts_P(PSTR("(c) Ondrej Hruska 2016\r\n"));
|
|
|
|
usart_puts_P(PSTR("\r\n"));
|
|
|
|
usart_puts_P(PSTR("WS2812B LED strip - DIN: D7\r\n"));
|
|
|
|
usart_puts_P(PSTR("Sonars (HC-SR04) - trig/echo:\r\n"));
|
|
|
|
usart_puts_P(PSTR(" \"red\" D3/D2\r\n"));
|
|
|
|
usart_puts_P(PSTR(" \"green\" D9/D8\r\n"));
|
|
|
|
usart_puts_P(PSTR(" \"blue\" D11/D10\r\n"));
|
|
|
|
usart_puts_P(PSTR("\r\n"));
|
|
|
|
usart_puts_P(PSTR("===========================\r\n"));
|
|
|
|
|
|
|
|
int cnt = 0;
|
|
|
|
|
|
|
|
while (1) {
|
|
|
|
sonar_measure();
|
|
|
|
|
|
|
|
if (++cnt == 20) {
|
|
|
|
cnt = 0;
|
|
|
|
pin_toggle(13);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|