316 lines
9.4 KiB
C++
316 lines
9.4 KiB
C++
#include <Arduino.h>
|
|
#include "xtensa/core-macros.h"
|
|
#include <ESP32-HUB75-MatrixPanel-I2S-DMA.h>
|
|
|
|
#include "main.h"
|
|
#include "overlay.h"
|
|
|
|
#define R1 42
|
|
#define G1 40
|
|
#define BL1 41
|
|
#define R2 38
|
|
#define G2 37
|
|
#define BL2 39
|
|
#define CH_A 45
|
|
#define CH_B 36
|
|
#define CH_C 48
|
|
#define CH_D 35
|
|
#define CH_E 21
|
|
#define CLK 2
|
|
#define LAT 47
|
|
#define OE 14
|
|
|
|
#define PIN_E 21
|
|
#define PANEL_WIDTH 64
|
|
#define PANEL_HEIGHT 32 // Panel height of 64 will required PIN_E to be defined.
|
|
|
|
#define PANELS_NUMBER 1
|
|
|
|
#define PANE_WIDTH PANEL_WIDTH * PANELS_NUMBER
|
|
#define PANE_HEIGHT PANEL_HEIGHT
|
|
#define NUM_LEDS PANE_WIDTH*PANE_HEIGHT
|
|
|
|
#define ONBOARD_LED 13
|
|
|
|
MatrixPanel_I2S_DMA *matrix = nullptr;
|
|
|
|
#define NUM_AGENTS 128
|
|
#define AGENT_MOVE_DISTANCE 1
|
|
#define AGENT_DROP_AMOUNT 1
|
|
#define AGENT_ANGLE 0.175
|
|
#define NUM_ITERATIONS 1000
|
|
typedef struct {
|
|
int x_position;
|
|
int y_position;
|
|
float heading;
|
|
} SlimeAgent;
|
|
|
|
float attractant[PANEL_WIDTH][PANEL_HEIGHT] = {};
|
|
int iterations = 0;
|
|
SlimeAgent agents[NUM_AGENTS];
|
|
|
|
void init_attractant() {
|
|
memset(attractant, 0.0, sizeof attractant);
|
|
// draw a circle out of attractant
|
|
//for (int p = 0; p < 64; p++) {
|
|
// float angle = ((PI * 2) / 64) * p;
|
|
// int x = 31 + round(10 * sin(angle));
|
|
// int y = 15 + round(10 * cos(angle));
|
|
// attractant[x][y] = 15.0;
|
|
//}
|
|
|
|
// draw a bitmap
|
|
//for(int x = 0; x < PANEL_WIDTH; x++) {
|
|
// for(int y = 0; y < PANEL_HEIGHT; y++) {
|
|
// int pixel_num = (y * PANEL_WIDTH) + x;
|
|
// if(overlay[ pixel_num / 8 ] & (1 << (7 - (pixel_num % 8)) ) ){
|
|
// attractant[x][y] = 15.0;
|
|
// }
|
|
// }
|
|
//}
|
|
}
|
|
|
|
void init_agents() {
|
|
for(int a = 0; a < NUM_AGENTS; a++) {
|
|
float angle = ((PI * 2) / NUM_AGENTS) * a;
|
|
//agents[a].x_position = 31 + round(10*sin(angle));
|
|
//agents[a].y_position = 15 + round(10*cos(angle));
|
|
// agents[a].heading = ((PI * 2) / NUM_AGENTS) * );
|
|
|
|
//agents[a].x_position = random(12, 52);
|
|
//agents[a].y_position = random(12, 20);
|
|
agents[a].x_position = random(0, 64);
|
|
agents[a].y_position = random(0, 32);
|
|
agents[a].heading = (random(0, 100) / 100.0) * (PI*2);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void setup(){
|
|
Serial.begin(BAUD_RATE);
|
|
//while (!Serial) {
|
|
// ; // wait for serial port to connect. Needed for native USB
|
|
//}
|
|
pinMode(ONBOARD_LED, OUTPUT);
|
|
|
|
// redefine pins if required
|
|
HUB75_I2S_CFG::i2s_pins _pins={R1, G1, BL1, R2, G2, BL2, CH_A, CH_B, CH_C, CH_D, CH_E, LAT, OE, CLK};
|
|
HUB75_I2S_CFG mxconfig(PANEL_WIDTH, PANEL_HEIGHT, PANELS_NUMBER, _pins);
|
|
|
|
mxconfig.gpio.e = PIN_E;
|
|
mxconfig.driver = HUB75_I2S_CFG::FM6126A; // for panels using FM6126A chips
|
|
|
|
mxconfig.latch_blanking = 4;
|
|
mxconfig.i2sspeed = HUB75_I2S_CFG::HZ_10M;
|
|
mxconfig.clkphase = false;
|
|
|
|
mxconfig.double_buff = true;
|
|
|
|
matrix = new MatrixPanel_I2S_DMA(mxconfig);
|
|
matrix->begin();
|
|
matrix->setBrightness8(64);
|
|
matrix->fillScreenRGB888(0, 0, 0);
|
|
|
|
init_agents();
|
|
init_attractant();
|
|
}
|
|
|
|
bool is_in_bounds(int x, int y) {
|
|
return (x < PANEL_WIDTH) and (y < PANEL_HEIGHT) and (x >= 0) and (y >= 0);
|
|
}
|
|
|
|
void draw_bg() {
|
|
int h_lines = round(PANEL_HEIGHT / 7);
|
|
int v_lines = round(PANEL_WIDTH / 7);
|
|
for(int v = 0; v <= v_lines; v++) {
|
|
matrix->drawFastVLine(v*7+2, 0, PANEL_HEIGHT, matrix->color565(41, 17, 76));
|
|
}
|
|
for(int h = 0; h <= h_lines; h++) {
|
|
matrix->drawFastHLine(0, 7*h + 1, PANEL_WIDTH, matrix->color565(41, 17, 76));
|
|
}
|
|
}
|
|
|
|
void draw_attractant() {
|
|
for(int x = 0; x<PANEL_WIDTH; x++) {
|
|
for(int y = 0; y<PANEL_HEIGHT; y++){
|
|
if (attractant[x][y] > 20.0) {
|
|
matrix->drawPixel(x, y, matrix->color565(
|
|
int((255.0 / 255.0) * round(attractant[x][y])),
|
|
int((80.0 / 255.0) * round(attractant[x][y])),
|
|
int((83.0 / 255.0) * round(attractant[x][y]))
|
|
)
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void setupGaussianKernel(float* kernel, int width, float sigma) {
|
|
float sum = 0.0;
|
|
for(int i = 0; i < width; i++) {
|
|
//kernel[width+i] = exp( -(i*i) / (2 * sigma * sigma)) / (PI * 2 * sigma * sigma);
|
|
kernel[i] = exp(-0.5f * (i * i) / (sigma * sigma));
|
|
sum += kernel[width];
|
|
}
|
|
for(int i; i < width; i++) {
|
|
kernel[i] = sum/width;
|
|
}
|
|
Serial.printf("%f %f %f\n", kernel[0], kernel[1], kernel[2]);
|
|
}
|
|
|
|
#define GAUSS_WIDTH 5
|
|
#define GAUSS_SIGMA 1.0
|
|
#define DECAY_FACTOR 0.9
|
|
float first_pass[PANEL_WIDTH][PANEL_HEIGHT];
|
|
void gaussian_blur() {
|
|
float kernel[GAUSS_WIDTH];
|
|
// first_pass[PANEL_WIDTH][PANEL_HEIGHT];
|
|
memset(first_pass, 0.0, sizeof first_pass);
|
|
setupGaussianKernel(kernel, GAUSS_WIDTH, GAUSS_SIGMA);
|
|
// horizontal pass
|
|
for (int x = 0; x < PANEL_WIDTH; x++) {
|
|
for(int y = 0; y < PANEL_HEIGHT; y++) {
|
|
float sum = attractant[x][y] * DECAY_FACTOR * kernel[0]; //0.0;
|
|
int additions = 1;
|
|
for (int x_offset = 1; x_offset < GAUSS_WIDTH; x_offset++) {
|
|
if (is_in_bounds(x+x_offset, y)) {
|
|
sum += attractant[x + x_offset][y] * kernel[x_offset];
|
|
additions++;
|
|
}
|
|
if (is_in_bounds(x-x_offset, y)) {
|
|
sum += attractant[x - x_offset][y] * kernel[x_offset];
|
|
additions++;
|
|
}
|
|
}
|
|
first_pass[x][y] = sum/GAUSS_WIDTH;
|
|
}
|
|
}
|
|
// vertical pass
|
|
for (int x = 0; x < PANEL_WIDTH; x++) {
|
|
for(int y = 0; y < PANEL_HEIGHT; y++) {
|
|
float sum = first_pass[x][y] * kernel[0]; //0.0;
|
|
int additions = 1;
|
|
for (int y_offset = 1; y_offset < GAUSS_WIDTH; y_offset++) {
|
|
if (is_in_bounds(x, y + y_offset)) {
|
|
sum += first_pass[x][y + y_offset] * kernel[y_offset];
|
|
additions++;
|
|
}
|
|
if (is_in_bounds(x, y - y_offset)) {
|
|
sum += first_pass[x][y - y_offset] * kernel[y_offset];
|
|
additions++;
|
|
}
|
|
}
|
|
attractant[x][y] = sum/GAUSS_WIDTH;
|
|
}
|
|
}
|
|
}
|
|
|
|
#define BLUR_KERNEL_SIZE 3
|
|
void box_blur() {
|
|
//Serial.println("memset: starting");
|
|
memset(first_pass, 0.0, sizeof first_pass);
|
|
//Serial.println("memset: passed");
|
|
// horizontal pass
|
|
for (int x = 0; x < PANEL_WIDTH; x++) {
|
|
for(int y = 0; y < PANEL_HEIGHT; y++) {
|
|
float sum;
|
|
sum = attractant[x][y] * DECAY_FACTOR; //0.0;
|
|
int additions = 1;
|
|
for (int x_offset = 1; x_offset < BLUR_KERNEL_SIZE; x_offset++) {
|
|
if (is_in_bounds(x+x_offset, y)) {
|
|
sum += attractant[x + x_offset][y];
|
|
additions++;
|
|
}
|
|
if (is_in_bounds(x-x_offset, y)) {
|
|
sum += attractant[x - x_offset][y];
|
|
additions++;
|
|
}
|
|
}
|
|
first_pass[x][y] = (sum/additions);
|
|
}
|
|
}
|
|
// vertical pass
|
|
for (int x = 0; x < PANEL_WIDTH; x++) {
|
|
for(int y = 0; y < PANEL_HEIGHT; y++) {
|
|
float sum = first_pass[x][y]; //0.0;
|
|
int additions = 1;
|
|
for (int y_offset = 1; y_offset < BLUR_KERNEL_SIZE; y_offset++) {
|
|
if (is_in_bounds(x, y + y_offset)) {
|
|
sum += first_pass[x][y + y_offset];
|
|
additions++;
|
|
}
|
|
if (is_in_bounds(x, y - y_offset)) {
|
|
sum += first_pass[x][y - y_offset];
|
|
additions++;
|
|
}
|
|
}
|
|
|
|
float result = (sum/additions); // * DECAY_FACTOR;
|
|
//Serial.printf("result: %f", result); //additions: %d\n", sum, additions);
|
|
attractant[x][y] = (((8.0*attractant[x][y]) + result) / 9.0);
|
|
}
|
|
}
|
|
}
|
|
|
|
void loop() {
|
|
matrix ->flipDMABuffer();
|
|
delay(10);
|
|
//matrix->clearScreen();
|
|
matrix->fillScreenRGB888(15, 0, 10);
|
|
|
|
// draw background and organism
|
|
draw_bg();
|
|
draw_attractant();
|
|
|
|
for(int a = 0; a < NUM_AGENTS; a++) {
|
|
//matrix->drawPixel(agents[a].x_position, agents[a].y_position, matrix->color565(0, 0, attractant[agents[a].x_position][agents[a].y_position]));
|
|
// motor step
|
|
// check if agent can move forward
|
|
float target_x = agents[a].x_position + cos(agents[a].heading) * AGENT_MOVE_DISTANCE;
|
|
float target_y = agents[a].y_position + sin(agents[a].heading) * AGENT_MOVE_DISTANCE;
|
|
int move_x = round(target_x);
|
|
int move_y = round(target_y);
|
|
if (is_in_bounds(move_x, move_y)) {
|
|
agents[a].x_position = move_x;
|
|
agents[a].y_position = move_y;
|
|
if (attractant[move_x][move_y] < 254.0) {
|
|
attractant[move_x][move_y] += AGENT_DROP_AMOUNT;
|
|
}
|
|
} else {
|
|
agents[a].heading += (random(-1,2) *2 - 1 ) * 30 / 100.0 * PI*2;
|
|
}
|
|
// sample step
|
|
int front_x = round(agents[a].x_position + cos(agents[a].heading) * AGENT_MOVE_DISTANCE);
|
|
int front_y = round(agents[a].y_position + sin(agents[a].heading) * AGENT_MOVE_DISTANCE);
|
|
int left_x = round(agents[a].x_position + cos(agents[a].heading - (AGENT_ANGLE * (PI * 2))));
|
|
int left_y = round(agents[a].y_position + sin(agents[a].heading - (AGENT_ANGLE * (PI * 2))));
|
|
int right_x = round(agents[a].x_position + cos(agents[a].heading + (AGENT_ANGLE * (PI * 2))));
|
|
int right_y = round(agents[a].y_position + sin(agents[a].heading + (AGENT_ANGLE * (PI * 2))));
|
|
float front_value = attractant[front_x][front_y];
|
|
float left_value = attractant[left_x][left_y];
|
|
float right_value = attractant[right_x][right_y];
|
|
if (front_value > right_value and front_value > left_value) {
|
|
;
|
|
} else if (front_value < right_value and front_value < left_value) {
|
|
agents[a].heading += (random(0, 2) * 2 - 1) * (AGENT_ANGLE / 100.0) * (PI*2) ;
|
|
} else if (left_value > right_value) {
|
|
agents[a].heading -= AGENT_ANGLE * (PI * 2);
|
|
} else {
|
|
agents[a].heading += AGENT_ANGLE * (PI * 2);
|
|
}
|
|
}
|
|
iterations++;
|
|
if (iterations > 64 && iterations % 64 == 0) { // (( == 0) {
|
|
//gaussian_blur();
|
|
//box_blur();
|
|
;
|
|
}
|
|
if (iterations >= NUM_ITERATIONS) {
|
|
init_attractant();
|
|
init_agents();
|
|
iterations = 0;
|
|
}
|
|
}
|
|
|