diff, but shitty

project/display.c

@@ -17,6 +17,7 @@ static float wavegrid[WAVEGRID_LEN];
 static ws2812_rgb_t pixels[PIXEL_COUNT] = {};
 
 static MeanBuf *mb;
+static float last_dist = 0;
 
 void display_show(void)
 {
@@ -34,10 +35,37 @@ void display_show(void)
 				wavegrid[PIXEL_COUNT*4-i-1] +
 				wavegrid[PIXEL_COUNT*4+i];
 
+		// clamp
 		if (x > 255) x = 255;
+		if (x < -255) x = -255;
 
-		pixels[i].r = (x);
-		pixels[i].b = (255.0f-x);
+		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);
@@ -50,10 +78,14 @@ static void handle_sonar_value(float mm)
 		wavegrid[i] = wavegrid[i-1] * (1.0f - WAVE_DISSIPATION);
 	}
 
-	float x = mm/5.0f;
-	if (x>255) x = 255;
+	const float max_level = 255.0f;
+	const float scale = 1.0f;
 
-	wavegrid[0] = 255 - x;
+	float x = mm * scale;
+	if (x > max_level) x = max_level;
+	if (x < -max_level) x = -max_level;
+
+	wavegrid[0] = x;
 
 	display_show();
 }
@@ -63,22 +95,23 @@ static void show(void*arg)
 {
 	(void)arg;
 
-	handle_sonar_value(meanbuf_current(mb));
+	float now = meanbuf_current(mb);
+	float diff = (last_dist - now);
+	last_dist = now;
+
+	handle_sonar_value(diff);
 }
 
 
-static void sonar(void* arg)
+static void sonar_poll(void* arg)
 {
 	(void)arg;
 
-	//info("Sonar");
-
 	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) {
@@ -114,7 +147,6 @@ void display_init(void)
 
 	display_show();
 
-	add_periodic_task(sonar, NULL, 50, true);
-
-	add_periodic_task(show, NULL, 75, true);
+	add_periodic_task(sonar_poll, NULL, 50, true);
+	add_periodic_task(show, NULL, 50, true);
 }