init, have brushed and brushless sim and gnuplot viz, also some usb hid testingHEADmaster

1 files changed, 386 insertions, 0 deletions

diff --git a/main.c b/main.c
new file mode 100644
index 0000000..5facab5
--- /dev/null
+++ b/main.c
@@ -0,0 +1,386 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+#include "transpose.h"
+
+#define PI 3.1415
+
+// utils
+#define typecheck(type, var)  \
+    (_Static_assert(__builtin_types_compatible_p(type, typeof(var)), "Incompatible Types!"))
+#define MAX(a,b)               \
+    ({  typeof(a) _a = (a);    \
+        typeof(b) _b = (b);    \
+        _a > _b ? _a : _b; })
+#define MIN(a,b)               \
+    ({  typeof(a) _a = (a);    \
+        typeof(b) _b = (b);    \
+        _a > _b ? _b : _a; })
+#define ARR_LENGTH(arr)        \
+    (sizeof((arr))/sizeof((arr)[0]))
+#define CLOSEST_POW2(n)        \
+    (1UL << (32 - __builtin_clz((unsigned int)((n) - 1))))
+#define OVERLOAD_MACRO(_0, _1, _2, _3, _4, NAME, ...) NAME
+
+// types for typedefing
+#define VECTOR(vec, type, nmemb) \
+    type vec##nmemb __attribute__((vector_size(CLOSEST_POW2(nmemb * sizeof(type)))))
+#define MATRIX(mat, vec, n, m)   \
+    struct { vec##m c[n]; } mat##n##x##m
+
+// vector operations
+#define VEC(...) {__VA_ARGS__}
+#define vec_element(vec, n) ((vec)[(n)])
+#define vec_promote(vec, type)                                  \
+    ({  __builtin_choose_expr(                                  \
+            __builtin_types_compatible_p(typeof(vec), type),    \
+            *(type *)&vec,                                      \
+            ({  type res = {0};                                 \
+                memcpy(&res, &vec,                              \
+                       MIN(sizeof(res), sizeof(vec)));          \
+                res; }));                                       \
+    })
+#define vec_print2(vec, fmt)             \
+    ({  printf("{ ");                   \
+        for(size_t i = 0;               \
+            i < ARR_LENGTH(vec); i++)   \
+            printf(fmt" ", vec[i]);     \
+        printf("}\n");                  \
+    })
+#define vec_print3(vec, fmt, nmemb)     \
+    ({  printf("{ ");                   \
+        for(size_t i = 0;               \
+            i < nmemb; i++)             \
+            printf(fmt" ", vec[i]);     \
+        printf("}\n");                  \
+    })
+#define vec_print(...)                                \
+    OVERLOAD_MACRO(_0, ##__VA_ARGS__, _4, vec_print3, \
+                   vec_print2, _1, _0) (__VA_ARGS__)
+
+// matrix operations
+#define MAT(...) {{ TRANSPOSE(__VA_ARGS__) }}
+#define mat_element(mat, n, m)  ((mat).c[(n)][(m)])
+#define mat_column(mat, n) ((mat).c[(n)])
+#define mat_promote(mat, type)                                          \
+    ({  __builtin_choose_expr(                                          \
+            __builtin_types_compatible_p(typeof(mat), type),            \
+            *(type *)&mat,                                              \
+            ({  type res = {0};                                         \
+                for(size_t i = 0; i < MIN(ARR_LENGTH(mat.c),            \
+                                          ARR_LENGTH(res.c)); i++)      \
+                    res.c[i] = vec_promote(mat.c[i], typeof(res.c[i])); \
+                res; }));                                               \
+    })
+#define mat_print4(mat, fmt, n, m)            \
+    ({  for(int i = 0; i < m; i++) {          \
+            printf("{ ");                     \
+            for(int j = 0; j < n; j++)        \
+                printf(fmt" ", mat.c[j][i]);  \
+            printf("}\n");                    \
+        }                                     \
+    })
+#define mat_print2(mat, fmt)                    \
+    ({  size_t n = ARR_LENGHT((mat).c);         \
+        size_t m = ARR_LENGHT((mat).c[0]);      \
+        for(int i = 0; i < m; i++) {            \
+            printf("{ ");                       \
+            for(int j = 0; j < n; j++)          \
+                printf(fmt" ", (mat).c[j][i]);  \
+            printf("}\n");                      \
+        }                                       \
+    })
+#define mat_print(...)                                \
+    OVERLOAD_MACRO(_0, ##__VA_ARGS__, mat_print4, _3, \
+                   mat_print2, _1, _0) (__VA_ARGS__)
+
+// mixed operations
+#define vecmat_mul(_v, _m)                                \
+    ({  typeof(_m.c[0]) res = {0};                        \
+        size_t n = MIN(ARR_LENGTH(_m.c), ARR_LENGTH((_v))); \
+        for(size_t i = 0; i < n; i++)                     \
+            res += _m.c[i] * _v[i];                       \
+        res; })
+
+typedef VECTOR(vec, double, 2);
+typedef VECTOR(vec, double, 3);
+typedef VECTOR(vec, double, 6);
+typedef MATRIX(mat, vec, 2, 2);
+typedef MATRIX(mat, vec, 3, 2);
+
+typedef VECTOR(vec, double, 1);
+typedef VECTOR(vec, double, 5);
+typedef VECTOR(vec, double, 4);
+typedef MATRIX(mat, vec, 3, 3);
+typedef MATRIX(mat, vec, 2, 3);
+typedef MATRIX(mat, vec, 3, 1);
+typedef MATRIX(mat, vec, 4, 4);
+typedef MATRIX(mat, vec, 5, 5);
+typedef MATRIX(mat, vec, 4, 5);
+
+// todo: smart current limit
+//       trapezoid pid
+
+struct pid_ctx {
+    double Kp;
+    double Ki;
+    double Kd;
+    
+    double prev_e;
+    double Ie;
+};
+
+double pid(struct pid_ctx *ctx, double measurement, double setpoint, double dt)
+{
+    double e = setpoint - measurement;
+    double De = (e - ctx->prev_e) / dt;
+    ctx->Ie += e * dt;
+    ctx->prev_e = e;
+    
+    return
+        ctx->Kp * e +
+        ctx->Kd * De +
+        ctx->Ki * ctx->Ie;
+}
+
+int brushed(void)
+{
+    char filename[] = "data1.dat";
+    
+    // PID
+    double setpoint = 4;
+    struct pid_ctx pid_velocity = {20.0, 0.0, 0.0, 0.0, 0.0};
+    struct pid_ctx pid_position = {30.0, 0.0, 0.0};
+    
+    // times
+    double dt = 0.00001;
+    double tfinal = 0.5;
+    
+    // limits
+    double voltage_max = 12;
+    double current_max = 30;
+    double torque_lose_rate = 1;
+
+   
+    // motor constants
+    // double J  = 300 * 0.1; // rotor inertia [kg*m^2]
+    // double b  =  10 * 0.5; // damping coefficient [-]
+    // double Kt =  10 * 1.0; // torque constant [Nm/A]
+    // double L  = 100 * 1.0; // inductance [H]
+    // double R  =  20 * 5.0; // resistance [Ohm]
+    // double Ke =  40 * 1.0; // back emf constant [V/m s^-1]
+    
+    double J  = 0.0044; // rotor inertia [kg*m^2]
+    double b  = 0.0011; // damping coefficient [Nm*s/rad]
+    double Kt = 0.22;   // torque constant [Nm/A]
+    double L  = 0.01;   // inductance [H]
+    double R  = 4;      // resistance [Ohm]
+    double Ke = 0.22;   // back emf constant [Vs/rad]
+    
+    // state space
+    mat3x3 A = MAT((0.0,  1.0,   0.0),
+                   (0.0, -b/J,   Kt/J),
+                   (0.0, -Ke/L, -R/L));
+    
+    mat2x3 B = MAT((0.0,   0.0),
+                   (0.0,  -1.0/J),
+                   (1.0/L, 0.0));
+    
+    // mat3x1 C = MAT((0.0, 1.0, 0.0));
+
+    
+    vec3 x = VEC(0.0, 0.0, 0.0);
+    vec2 u = VEC(0.0, 0.0);
+
+    // Simulation
+    FILE *fp = fopen(filename, "w");
+    if(!fp) return 1;
+    
+    for(double i = 0.0f; i < tfinal; i += dt) {
+        // state space and euler integration
+        vec3 xdot = vecmat_mul(x, A) + vecmat_mul(u, B);
+        x += xdot * dt;        
+
+        // pid control
+        if((int)i*100000 % 1000 == 0) {
+            double velocity_setpoint =
+                pid(&pid_position, vec_element(x, 0), setpoint, dt);
+            vec_element(u, 0) =
+                pid(&pid_velocity, vec_element(x, 1), velocity_setpoint, dt);
+
+        }
+            
+        // limits
+        if(vec_element(u, 0) > voltage_max) vec_element(u, 0) = voltage_max;
+        // if(vec_element(x, 2) > current_max) vec_element(x, 2) = current_max;
+
+        // timed inputs
+#define around(i, v) ((i) > ((v)-0.000005) && (i) < ((v)+0.000005))
+#define torque_surge(t, torque) if(around(i, t)) vec_element(u, 1) = (torque)
+        
+        // torque limits
+        if(vec_element(u, 1) < 0.0) vec_element(u, 1) = 0.0;
+        if(vec_element(u, 1) > 0.0) vec_element(u, 1) -= torque_lose_rate;
+                
+        // torque_surge(0.025, 0.2);
+        // torque_surge(0.075, 0.2);
+        // torque_surge(0.100, 0.2);
+        // torque_surge(0.250, 0.2);
+        torque_surge(0.300, 20);
+
+        // setpoint movement
+        if(around(i, 2.25)) setpoint = 5;
+        
+        fprintf(fp, "%f %f %f %f %f %f\n", i,
+                vec_element(x, 0),
+                vec_element(x, 1),
+                vec_element(x, 2),
+                vec_element(u, 0),
+                vec_element(u, 1));
+    }
+
+    fclose(fp);
+    return 0;
+}
+
+double fa(double t) {return sin(t); }
+double fb(double t) {return fa(t + (2.0/3.0 * PI)); }
+double fc(double t) {return fb(t + (2.0/3.0 * PI)); }
+
+int brushless()
+{
+    // motor constants
+    double R = 1.0;
+    double L = 1.1;
+    double M = 1.0;
+    double J = 1.0;
+    double b = 1.0;
+    double P = 2.0;
+    double l = 1.0;
+
+    // state space
+#define L1      (L-M)
+#define emfa(a) l/J*fa(a)
+#define emfb(a) l/J*fb(a)
+#define emfc(a) l/J*fc(a)
+#define _SQRT3_2 (sqrt(3.0)/2.0)
+    
+#define A_MAT(a)                                    \
+    MAT((-R/L1,   0,       0,       -emfa(a), 0),   \
+        (0,      -R/L1,    0,       -emfb(a), 0),   \
+        (0,       0,      -R/L1,    -emfc(a), 0),   \
+        (emfa(a), emfb(a), emfc(a), -b/J,     0),   \
+        (0,       0,       0,        P/2.0,   0))
+    
+#define emf_VEC(o, a) ((vec3)VEC(fa(a), fb(a), fc(a)) * l * o);
+
+    mat5x5 A = A_MAT(0);
+
+    mat4x5 B = MAT((1.0/L1, 0, 0, 0),
+                   (0, 1.0/L1, 0, 0),
+                   (0, 0, 1.0/L1, 0),
+                   (0, 0, 0, 1.0/L1),
+                   (0, 0, 0, 0));
+
+    vec5 x   = VEC(0);
+    vec4 u   = VEC(0);
+    vec3 emf = emf_VEC(0, 0);
+
+    mat2x3 clarke = MAT((1.0,   0.0),
+                        (-0.5,  _SQRT3_2),
+                        (-0.5, -_SQRT3_2));
+
+    // simulation
+    char filename[] = "data2.dat";
+    double time = 100;
+    double dt = 0.0001;
+
+    double U = 0.001;
+    
+    FILE *fp = fopen(filename, "w");
+    if(!fp) return 1;
+    
+    double i2 = 0;
+    for(double i = 0; i < time; i += dt) {
+        A   = (mat5x5) A_MAT(vec_element(x, 4));
+        emf = (vec3)   emf_VEC(vec_element(x, 3), vec_element(x, 4));
+        
+        vec5 xdot = vecmat_mul(x, A) + vecmat_mul(u, B) + vec_promote(emf, vec5);
+        x += xdot * dt;
+        
+        if((int)i*1000 % 100 == 0) {            
+            vec3 voltages = vecmat_mul(
+                U * (vec2)VEC(sin(vec_element(x, 4)),
+                              cos(vec_element(x, 4))),
+                clarke) + 0.001;
+
+        
+            // u = vec_promote(voltages, vec4);
+            u = (vec4)VEC(0.0, 1.0, -1.0);
+        }
+
+        fprintf(fp, "%f %f %f %f %f %f %f %f %f %f\n", i,
+                vec_element(x, 0),
+                vec_element(x, 1),
+                vec_element(x, 2),
+                vec_element(x, 3),
+                vec_element(x, 4),
+                vec_element(u, 0),
+                vec_element(u, 1),
+                vec_element(u, 2),
+                vec_element(u, 3));
+    }
+    
+}
+
+int main(void)
+{
+    // return brushed();
+    return brushless();
+}
+
+void vec_example(void)
+{
+    // vec2 v2 = VEC(1.0, 2.5);
+    // vec3 v3 = vec_promote(v2, typeof(v3));
+    // vec_element(v3, 2) = 8;
+
+    // mat3x2 m = MAT((1.0, 2.0, -1.0),
+    //                (0.0, 1.0,  2.5));
+    // mat_element(m, 0, 1) = 6.9;
+
+    // vec2 r = vecmat_mul(v3, m);
+
+    // vec_print(v3, "%.2f", 3);
+    // printf("      times\n");
+    // mat_print(m, "%.2f", 3, 2);
+    // printf("        =\n");
+    // vec_print(r, "%.2f", 2);
+    
+    // mat4x6 m2 = mat_promote(m,  typeof(m2));
+    // printf("\npromoted bigger\n");
+    // mat_print(m2, "%.2f", 4, 6);
+    
+    // mat2x2 m3 = mat_promote(m2, typeof(m3));
+    // printf("\npromoted smaller\n");
+    // mat_print(m3, "%.2f", 2, 2);
+
+    // FILE *fp = fopen("test.dat", "w");
+    // // fprintf(fp, "TIME, NUMBER1, NUMBER2\n");
+    // for(size_t i = 0; i < 1000000; i++) {
+    //     // fprintf(fp, "%ld,%ld,%ld\n", i, i % 256, i % 137);
+    //     fprintf(fp, "%ld %ld %ld\n", i, i % 256, i % 137);
+    //     fflush(fp);
+    //     usleep(50000);
+    // }
+    // fclose(fp);
+
+
+    vec2 _tv21 = {0};
+    vec2 _tv22 = {1};
+    vec2 _tv23 = _tv21 + _tv22;
+    vec6 _tv61 = {0};
+    vec6 _tv62 = {1};
+    vec6 _tv63 = _tv61 + _tv62;
+}