electro
/
arduino-spatial-rgb
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

sdfsdf

Browse Source
master
Ondřej Hruška 8 years ago
parent ac0bc06e29
commit b7b82a5422
1 changed files with 85 additions and 70 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 155
      main.c

155
main.c
Unescape View File

@ -6,6 +6,7 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <stdlib.h> #include <stdlib.h>
#include <stdio.h>
// Include stuff from the library // Include stuff from the library
#include "lib/iopins.h" #include "lib/iopins.h"
@ -14,11 +15,13 @@
static void sonar_start(void); static void sonar_start(void);
#define WS_PIN 5 #define WS_PIN 6
#define TRIG_PIN 2 #define TRIG_PIN 2
#define ECHO1_PIN 3 #define ECHO1_PIN 3
#define ECHO2_PIN 4 #define ECHO2_PIN 4
#define ECHO3_PIN 5
/** Wait long enough for the colors to show */ /** Wait long enough for the colors to show */
@ -27,18 +30,6 @@ static void ws_show(void)
_delay_us(10); _delay_us(10);
} }
static inline __attribute__((always_inline))
void delay_cyc(uint8_t __count)
{
__asm__ volatile (
"1: dec %0" "\n\t"
"brne 1b"
: "=r" (__count)
: "0" (__count)
);
}
/** Send one byte to the RGB strip */ /** Send one byte to the RGB strip */
static inline __attribute__((always_inline)) static inline __attribute__((always_inline))
void ws_send_byte(uint8_t bb) void ws_send_byte(uint8_t bb)
@ -46,13 +37,21 @@ void ws_send_byte(uint8_t bb)
for (int8_t i = 8; i > 0; i--) { for (int8_t i = 8; i > 0; i--) {
pin_up(WS_PIN); pin_up(WS_PIN);
if (bb & 0x80) { if (bb & 0x80) {
delay_cyc(4); __asm__ volatile ("nop");
__asm__ volatile ("nop");
__asm__ volatile ("nop");
__asm__ volatile ("nop");
__asm__ volatile ("nop");
pin_down(WS_PIN); pin_down(WS_PIN);
delay_cyc(1); __asm__ volatile ("nop");
} else { } else {
delay_cyc(1); __asm__ volatile ("nop");
pin_down(WS_PIN); pin_down(WS_PIN);
delay_cyc(4); __asm__ volatile ("nop");
__asm__ volatile ("nop");
__asm__ volatile ("nop");
__asm__ volatile ("nop");
__asm__ volatile ("nop");
} }
bb = (uint8_t)(bb << 1); bb = (uint8_t)(bb << 1);
@ -73,9 +72,12 @@ static void hw_init(void)
usart_init(BAUD_115200); usart_init(BAUD_115200);
as_output(2); as_output(2);
as_input_pu(3); as_input_pu(3);
as_input_pu(4); as_input_pu(4);
as_output(5); // WS as_input_pu(5);
as_output(6); // WS
} }
typedef enum { typedef enum {
@ -85,44 +87,50 @@ typedef enum {
} MeasPhase; } MeasPhase;
#define PULSE_LEN 9 #define MBUF_LEN 16
const uint8_t pulse[PULSE_LEN] = {79, 150, 206, 243, 255, 243, 206, 150, 79};
#define MBUF_LEN 64
typedef struct { typedef struct {
float data[MBUF_LEN]; float data[MBUF_LEN];
} MBuf; } MBuf;
float mbuf_add(MBuf *buf, float value) static float mbuf_add(MBuf *buf, float value)
{ {
float aggr = value; float aggr = value;
for (int i = 1; i < MBUF_LEN; i++) { for (int i = MBUF_LEN - 1; i > 0; i--) {
aggr += buf->data[i]; float m = buf->data[i-1];
buf->data[i] = buf->data[i-1]; aggr += m;
buf->data[i] = m;
} }
buf->data[0] = value; buf->data[0] = value;
return aggr / (float)MBUF_LEN; return aggr / (float)MBUF_LEN;
} }
MBuf mb_offs1; static MBuf mb_offs1;
MBuf mb_offs2; 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) static void sonar_measure(void)
{ {
usart_puts("MEASURE!\n");
pin_up(TRIG_PIN); pin_up(TRIG_PIN);
_delay_ms(10); _delay_ms(10);
pin_down(TRIG_PIN); pin_down(TRIG_PIN);
MeasPhase e1_ph = MEAS_WAIT_1; MeasPhase e1_ph = MEAS_WAIT_1;
MeasPhase e2_ph = MEAS_WAIT_1; MeasPhase e2_ph = MEAS_WAIT_1;
MeasPhase e3_ph = MEAS_WAIT_1;
uint32_t echo1 = 0; uint32_t echo1 = 0;
uint32_t echo2 = 0; uint32_t echo2 = 0;
uint32_t echo3 = 0;
TCNT1 = 0; TCNT1 = 0;
TCCR1B = (0b010 << CS10); TCCR1B = (0b010 << CS10);
@ -159,7 +167,23 @@ static void sonar_measure(void)
break; break;
} }
if (e1_ph == MEAS_DONE && e2_ph == MEAS_DONE) { 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 break; // done
} }
} }
@ -176,54 +200,45 @@ static void sonar_measure(void)
echo2 *= 8000; echo2 *= 8000;
echo2 /= 10000; echo2 /= 10000;
char buf[10]; echo3 *= 8000;
echo3 /= 10000;
ltoa(echo1, buf, 10); float offset1 = 255 - echo1 / 25.0f;
usart_puts("echo1: "); float offset2 = 255 - echo2 / 25.0f;
usart_puts(buf); float offset3 = 255 - echo3 / 25.0f;
usart_puts(" -> ");
float offset1 = echo1 / 100.0f; if (offset1 > 255) offset1 = 255;
float offset2 = echo2 / 100.0f; 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); offset1 = mbuf_add(&mb_offs1, offset1);
offset2 = mbuf_add(&mb_offs2, offset2); offset2 = mbuf_add(&mb_offs2, offset2);
offset3 = mbuf_add(&mb_offs3, offset3);
int of1i = (int) roundf(offset1); int c1 = (int) roundf(offset1);
int of2i = (int) roundf(offset2); int c2 = (int) roundf(offset2);
int c3 = (int) roundf(offset3);
of1i = 15 - of1i; // char buf[30];
of2i = 15 - of2i;
int gi = -15 + PULSE_LEN/2 - of1i - 15; for (int i = 30 - 1; i > 0; i--) {
int bi = -15 + PULSE_LEN/2 + of2i + 15; history[i].r = history[i-1].r;
int ri = -15 + PULSE_LEN/2 + 0; history[i].g = history[i-1].g;
history[i].b = history[i-1].b;
uint8_t r, g, b; // sprintf(buf, "%d %d %d\n", history[i].r, history[i].g, history[i].b);
for (int i = 0; i < 32; i++) { // usart_puts(buf);
if (ri >= 0 && ri < PULSE_LEN) { }
r = pulse[ri];
} else {
r = 0;
}
if (gi >= 0 && gi < PULSE_LEN) {
g = pulse[gi];
} else {
g = 0;
}
if (bi >= 0 && bi < PULSE_LEN) {
b = pulse[bi];
} else {
b = 0;
}
ws_send_rgb(r,g,b); history[0].r = (uint8_t)c1;
history[0].g = (uint8_t)c2;
history[0].b = (uint8_t)c3;
ri++; for (int i = 0; i < 30; i++) {
gi++; ws_send_rgb(history[i].r, history[i].g, history[i].b);
bi++;
} }
ws_show(); ws_show();
@ -236,6 +251,6 @@ int main(void)
while (1) { while (1) {
sonar_measure(); sonar_measure();
_delay_ms(20); _delay_ms(10);
} }
} }

Write Preview
Loading…
Cancel
Save