You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
256 lines
4.4 KiB
256 lines
4.4 KiB
#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"
|
|
|
|
static void sonar_start(void);
|
|
|
|
#define WS_PIN 6
|
|
|
|
#define TRIG_PIN 2
|
|
|
|
#define ECHO1_PIN 3
|
|
#define ECHO2_PIN 4
|
|
#define ECHO3_PIN 5
|
|
|
|
|
|
/** Wait long enough for the colors to show */
|
|
static 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(2);
|
|
|
|
as_input_pu(3);
|
|
as_input_pu(4);
|
|
as_input_pu(5);
|
|
|
|
as_output(6); // WS
|
|
}
|
|
|
|
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 void sonar_measure(void)
|
|
{
|
|
pin_up(TRIG_PIN);
|
|
_delay_ms(10);
|
|
pin_down(TRIG_PIN);
|
|
|
|
MeasPhase e1_ph = MEAS_WAIT_1;
|
|
MeasPhase e2_ph = MEAS_WAIT_1;
|
|
MeasPhase e3_ph = MEAS_WAIT_1;
|
|
|
|
uint32_t echo1 = 0;
|
|
uint32_t echo2 = 0;
|
|
uint32_t echo3 = 0;
|
|
|
|
TCNT1 = 0;
|
|
TCCR1B = (0b010 << CS10);
|
|
while (true) {
|
|
switch (e1_ph) {
|
|
case MEAS_WAIT_1:
|
|
if (pin_is_high(ECHO1_PIN)) {
|
|
echo1 = TCNT1;
|
|
e1_ph = MEAS_WAIT_0;
|
|
}
|
|
break;
|
|
|
|
case MEAS_WAIT_0:
|
|
if (pin_is_low(ECHO1_PIN)) {
|
|
echo1 = TCNT1 - echo1;
|
|
e1_ph = MEAS_DONE;
|
|
}
|
|
break;
|
|
}
|
|
|
|
switch (e2_ph) {
|
|
case MEAS_WAIT_1:
|
|
if (pin_is_high(ECHO2_PIN)) {
|
|
echo2 = TCNT1;
|
|
e2_ph = MEAS_WAIT_0;
|
|
}
|
|
break;
|
|
|
|
case MEAS_WAIT_0:
|
|
if (pin_is_low(ECHO2_PIN)) {
|
|
echo2 = TCNT1 - echo2;
|
|
e2_ph = MEAS_DONE;
|
|
}
|
|
break;
|
|
}
|
|
|
|
switch (e3_ph) {
|
|
case MEAS_WAIT_1:
|
|
if (pin_is_high(ECHO3_PIN)) {
|
|
echo3 = TCNT1;
|
|
e3_ph = MEAS_WAIT_0;
|
|
}
|
|
break;
|
|
|
|
case MEAS_WAIT_0:
|
|
if (pin_is_low(ECHO3_PIN)) {
|
|
echo3 = TCNT1 - echo3;
|
|
e3_ph = MEAS_DONE;
|
|
}
|
|
break;
|
|
}
|
|
|
|
if (e1_ph == MEAS_DONE && e2_ph == MEAS_DONE && e3_ph == MEAS_DONE) {
|
|
break; // done
|
|
}
|
|
}
|
|
|
|
TCCR1B = 0; // stop
|
|
|
|
// Both pulses measured with 0.5us accuracy
|
|
|
|
// Convert to mm
|
|
|
|
echo1 *= 8000;
|
|
echo1 /= 10000;
|
|
|
|
echo2 *= 8000;
|
|
echo2 /= 10000;
|
|
|
|
echo3 *= 8000;
|
|
echo3 /= 10000;
|
|
|
|
float offset1 = 255 - echo1 / 25.0f;
|
|
float offset2 = 255 - echo2 / 25.0f;
|
|
float offset3 = 255 - echo3 / 25.0f;
|
|
|
|
if (offset1 > 255) offset1 = 255;
|
|
if (offset2 > 255) offset2 = 255;
|
|
if (offset3 > 255) offset3 = 255;
|
|
if (offset1 < 0) offset1 = 0;
|
|
if (offset2 < 0) offset2 = 0;
|
|
if (offset3 < 0) offset3 = 0;
|
|
|
|
offset1 = mbuf_add(&mb_offs1, offset1);
|
|
offset2 = mbuf_add(&mb_offs2, offset2);
|
|
offset3 = mbuf_add(&mb_offs3, offset3);
|
|
|
|
int c1 = (int) roundf(offset1);
|
|
int c2 = (int) roundf(offset2);
|
|
int c3 = (int) roundf(offset3);
|
|
|
|
// char buf[30];
|
|
|
|
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;
|
|
|
|
// sprintf(buf, "%d %d %d\n", history[i].r, history[i].g, history[i].b);
|
|
// usart_puts(buf);
|
|
}
|
|
|
|
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();
|
|
|
|
while (1) {
|
|
sonar_measure();
|
|
_delay_ms(10);
|
|
}
|
|
}
|
|
|