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.
152 lines
2.3 KiB
152 lines
2.3 KiB
#include "display.h"
|
|
#include "com/debug.h"
|
|
#include "utils/timebase.h"
|
|
#include "utils/meanbuf.h"
|
|
|
|
#include <math.h>
|
|
|
|
#define PIXEL_COUNT 30
|
|
|
|
#define WAVEGRID_DEPTH 5
|
|
#define WAVEGRID_LEN PIXEL_COUNT*WAVEGRID_DEPTH
|
|
static float wavegrid[WAVEGRID_LEN];
|
|
|
|
#define WAVE_DISSIPATION 0.011f
|
|
|
|
|
|
static ws2812_rgb_t pixels[PIXEL_COUNT] = {};
|
|
|
|
static MeanBuf *mb;
|
|
static float last_dist = 0;
|
|
|
|
void display_show(void)
|
|
{
|
|
for (int i = 0; i < PIXEL_COUNT; i++) {
|
|
|
|
// 0 1 2 3 #0+i
|
|
// 7 6 5 4 #2-i-1
|
|
// 8 9 A B #2+i
|
|
// F E D C #
|
|
// G I J K
|
|
|
|
float x = wavegrid[i] +
|
|
wavegrid[PIXEL_COUNT*2-i-1] +
|
|
wavegrid[PIXEL_COUNT*2+i] +
|
|
wavegrid[PIXEL_COUNT*4-i-1] +
|
|
wavegrid[PIXEL_COUNT*4+i];
|
|
|
|
// clamp
|
|
if (x > 255) x = 255;
|
|
if (x < -255) x = -255;
|
|
|
|
ws2812_rgb_t *p = &pixels[i];
|
|
|
|
p->num = 0;
|
|
|
|
p->b = 128 + x/2.0f;
|
|
|
|
if (x > 128) {
|
|
p->r = x-128;
|
|
p->b -= p->r;
|
|
}
|
|
|
|
//p->g = 128;
|
|
|
|
//if (x > 0) p->r = x;
|
|
//else p->b = -x;
|
|
|
|
|
|
// p->g = 0;
|
|
// p->r = x > 0 ? x: 0;//x;
|
|
// p->b = 128 - x/2.0f;//255-x;
|
|
|
|
// if (x > 0) {
|
|
// } else {
|
|
// p->r = 0;
|
|
// p->g = -x;
|
|
// p->b = 255 + x;
|
|
// }
|
|
}
|
|
|
|
colorled_set_many((uint32_t*) pixels, PIXEL_COUNT);
|
|
}
|
|
|
|
|
|
static void handle_sonar_value(float mm)
|
|
{
|
|
for (int i = WAVEGRID_LEN-1; i > 0; i--) {
|
|
wavegrid[i] = wavegrid[i-1] * (1.0f - WAVE_DISSIPATION);
|
|
}
|
|
|
|
const float max_level = 255.0f;
|
|
const float scale = 1.0f;
|
|
|
|
float x = mm * scale;
|
|
if (x > max_level) x = max_level;
|
|
if (x < -max_level) x = -max_level;
|
|
|
|
wavegrid[0] = x;
|
|
|
|
display_show();
|
|
}
|
|
|
|
|
|
static void show(void*arg)
|
|
{
|
|
(void)arg;
|
|
|
|
float now = meanbuf_current(mb);
|
|
float diff = (last_dist - now);
|
|
last_dist = now;
|
|
|
|
handle_sonar_value(diff);
|
|
}
|
|
|
|
|
|
static void sonar_poll(void* arg)
|
|
{
|
|
(void)arg;
|
|
|
|
GPIOB->BSRR = GPIO_Pin_13;
|
|
delay_us(10);
|
|
GPIOB->BRR = GPIO_Pin_13;
|
|
|
|
// wait for response
|
|
bool suc = false;
|
|
until_timeout(50) {
|
|
if((GPIOB->IDR & (1 << 14)) != 0) {
|
|
suc = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!suc) {
|
|
dbg("Not suc");
|
|
return;
|
|
}
|
|
|
|
uint32_t cnt = 0;
|
|
until_timeout(50) {
|
|
if((GPIOB->IDR & (1 << 14)) == 0) break;
|
|
cnt++;
|
|
}
|
|
|
|
float t = cnt / 11.2f;
|
|
|
|
meanbuf_add(mb, t);
|
|
}
|
|
|
|
|
|
void display_init(void)
|
|
{
|
|
mb = meanbuf_create(10);
|
|
|
|
for (int i = 0; i < WAVEGRID_LEN; i++) {
|
|
wavegrid[i] = 0;
|
|
}
|
|
|
|
display_show();
|
|
|
|
add_periodic_task(sonar_poll, NULL, 50, true);
|
|
add_periodic_task(show, NULL, 50, true);
|
|
}
|
|
|