-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathTRM_UNO_R4_WIFI.ino
More file actions
146 lines (123 loc) · 3.67 KB
/
TRM_UNO_R4_WIFI.ino
File metadata and controls
146 lines (123 loc) · 3.67 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
#include <Arduino.h>
#include <math.h>
#include <Arduino_LED_Matrix.h>
ArduinoLEDMatrix matrix;
static const int N = 8;
static const int IN = 3;
float state[N] = {0};
const float Ws[N][N] = {
{0.55, 0.05, 0.00, 0.00, 0.10, 0.00, 0.00, 0.00},
{0.00, 0.50, 0.08, 0.00, 0.00, 0.10, 0.00, 0.00},
{0.00, 0.00, 0.52, 0.07, 0.00, 0.00, 0.10, 0.00},
{0.10, 0.00, 0.00, 0.48, 0.07, 0.00, 0.00, 0.10},
{0.00, 0.10, 0.00, 0.00, 0.50, 0.07, 0.00, 0.00},
{0.00, 0.00, 0.10, 0.00, 0.00, 0.50, 0.07, 0.00},
{0.00, 0.00, 0.00, 0.10, 0.00, 0.00, 0.52, 0.07},
{0.07, 0.00, 0.00, 0.00, 0.10, 0.00, 0.00, 0.50}
};
const float Wi[N][IN] = {
{ 0.60, 0.20, 0.00},
{-0.30, 0.25, 0.10},
{ 0.15,-0.35, 0.05},
{ 0.10, 0.10, 0.00},
{-0.20, 0.15, 0.05},
{ 0.05, 0.30, 0.00},
{ 0.25,-0.10, 0.05},
{-0.10, 0.05, 0.10}
};
const float b[N] = {0.00, 0.02, -0.01, 0.00, 0.01, 0.00, -0.02, 0.01};
const float Wo[N] = {0.30, -0.10, 0.25, 0.05, -0.20, 0.15, 0.10, 0.05};
float u = 0.0f;
static inline float clampf(float x, float lo, float hi){ return x<lo?lo:(x>hi?hi:x); }
static inline float tanh_safe(float x){ x=clampf(x,-8.0f,8.0f); return tanhf(x); }
float trm_step(float x0, float x1, float x2){
float ns[N];
for(int i=0;i<N;i++){
float acc=b[i];
for(int j=0;j<N;j++) acc += Ws[i][j]*state[j];
acc += Wi[i][0]*x0 + Wi[i][1]*x1 + Wi[i][2]*x2;
ns[i]=tanh_safe(acc);
}
for(int i=0;i<N;i++) state[i]=ns[i];
float y=0.0f; for(int i=0;i<N;i++) y += Wo[i]*state[i];
return y;
}
// Serial float line reader
bool readFloatLine(float &out){
static String buf;
while(Serial.available()){
char c=(char)Serial.read();
if(c=='\n' || c=='\r'){
if(buf.length()==0) continue;
out = buf.toFloat();
buf="";
return true;
} else {
buf += c;
if(buf.length()>32) buf.remove(0,16);
}
}
return false;
}
// LED Matrix: 12x8 frame (96 bits)
uint8_t frame[8][12];
void clearFrame(){
for(int r=0;r<8;r++) for(int c=0;c<12;c++) frame[r][c]=0;
}
// Draw a horizontal bar (0..12 columns) centered vertically
void drawBar(int cols){
clearFrame();
cols = clampf(cols, 0, 12);
int row = 3; // simple bar on row 3 and 4 for thickness
for(int c=0;c<cols;c++){
frame[row][c]=1;
frame[row+1][c]=1;
}
}
void pushFrame(){
// ArduinoLEDMatrix expects a 8x12 bitmap; we pack into bytes.
// Easiest: use matrix.renderBitmap() if available; else pack manually.
// Many installs support renderBitmap(frame, 8, 12).
matrix.renderBitmap(frame, 8, 12);
}
unsigned long lastTick=0;
void setup(){
Serial.begin(115200);
pinMode(LED_BUILTIN, OUTPUT);
matrix.begin();
clearFrame();
pushFrame();
Serial.println("\nTRM UNO R4 WiFi (Serial u + LED Matrix y)");
Serial.println("Entra u (float) i Enter. Ex: 0.8 / -0.8 / 0");
}
void loop(){
float val;
if(readFloatLine(val)){
u = clampf(val, -2.0f, 2.0f);
Serial.print("u=");
Serial.println(u, 3);
}
unsigned long now = millis();
if(now - lastTick >= 150){
lastTick = now;
float t = now/1000.0f;
float y = trm_step(u, sinf(t), 1.0f);
// policy LED built-in
if(y > 0.35f) digitalWrite(LED_BUILTIN, HIGH);
else if(y < -0.35f) digitalWrite(LED_BUILTIN, LOW);
else digitalWrite(LED_BUILTIN, (now/500)%2);
// map y ~ [-1..1] to [0..12]
float yn = clampf((y + 1.0f) * 0.5f, 0.0f, 1.0f);
int cols = (int)roundf(yn * 12.0f);
drawBar(cols);
pushFrame();
Serial.print("y=");
Serial.print(y, 4);
Serial.print(" state=[");
Serial.print(state[0],2); Serial.print(",");
Serial.print(state[1],2); Serial.print(",");
Serial.print(state[2],2); Serial.print(",...,");
Serial.print(state[N-1],2);
Serial.println("]");
}
}