Revision 0:4dd2d995f7d0, committed 2019-05-07

Comitter:

benkatz

Date:

Tue May 07 01:56:53 2019 +0000

Commit message:

works with the python side control

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamics/dynamics.cpp	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,48 @@
+#include "dynamics.h"
+
+         
+void integrate_state(StateStruct * state, float torque, float dt)
+{
+    float s1 = sin(state->q[1]);
+    float c1 = cos(state->q[1]);
+    // Calculate Mass Matrix //
+    state->h <<     J0 + M1*L0*L0 + L1*L1*M1*s1*s1,  L0*L1*M1*c1,
+                    L0*L1*M1*c1,                     J1 + M1*L1*L1;
+    // Calculate Coriolis Matrix //
+    state->c <<     2.0f*L1*L1*M1*s1*c1*state->qd[1] + B0,   -L0*L1*M1*s1*state->qd[1],
+                    -L1*L1*M1*s1*c1*state->qd[0],       B1;
+    // Gravity Vector //
+    state->g <<     0,
+                    -G*L1*M1*sin(state->q[1]);
+    // Torques //
+    state->tau <<  torque,0;
+    
+    // Solve for Accelerations //
+    Vector2f RHS = state->tau- state->g - state->c*state->qd;
+    //state->qdd = state->h.inverse()*RHS;
+    //state->qdd = state->h.colPivHouseholderQr().solve(RHS);
+    Matrix2f h_inv = state->h.inverse();    // For 2x2, inverse is actually faster
+    state->qdd = h_inv*RHS;
+    // Integrate //
+    state->qd = state->qd + state->qdd*dt;
+    state->q = state->q + state->qd*dt;
+    
+}
+
+
+void update_sensors(StateStruct state, SensorStruct * sensors)
+{
+    float PI2 = 2.0f*PI;
+    float off1 = 0;
+    float off2 = 0;
+    if(state.q[0] < 0){off1 = PI2*(((int)(-state.q[0]/PI2)) + 1);}
+    if(state.q[1] < 0){off2 = PI2*(((int)(-state.q[1]/PI2)) + 1);}
+    
+    sensors->encoder_count[0] = (int)((state.q[0]+off1) * (float)B_RES * 0.15915494309f)%B_RES;
+    sensors->encoder_count[1] = (int)((state.q[1]+off2) * (float)P_RES * 0.15915494309f)%P_RES;
+    sensors->q_sensed[0] = state.q[0];
+    sensors->q_sensed[1] = state.q[1];
+    sensors->qd_sensed[0] = state.qd[0];
+    sensors->qd_sensed[1] = state.qd[1];
+    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamics/dynamics.h	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,24 @@
+#ifndef DYNAMICS_H
+#define DYNAMICS_H
+
+#include "mbed.h"
+#include "../Eigen/Dense.h"
+#include "../structs.h"
+
+#define J0      .000115f
+#define L0      .043f
+#define B0      .00001f
+#define J1      .00000237f
+#define L1      .035f
+#define M1      .0054f
+#define B1      .00001f
+#define G       9.8f
+
+#define P_RES   4096    // Pendulum encoder resolution
+#define B_RES   4096   // Base encoder resolution
+
+#define PI      3.14159265359f
+
+void integrate_state(StateStruct * state, float torque, float dt);
+void update_sensors(StateStruct state, SensorStruct * sensors);
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Eigen.lib	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,169 @@
+// Furuta Pendulum Demo Firmware //
+// This version simulates the pendulum dynamics and sensor outputs //
+// Ben Katz, 2019 //
+
+#include "mbed.h"
+#include <iostream>
+#include <Eigen/Dense.h>
+#include "dynamics.h"
+#include "structs.h"
+#include "math_ops.h"
+#include "serial_com.h"
+
+#define DT      .0001f
+
+
+using namespace std;
+using namespace Eigen;
+
+
+Ticker loop;
+DigitalOut dbg(PC_8);
+DigitalOut led(PA_5);
+Serial pc(USBTX, USBRX);
+
+
+float tau;
+
+StateStruct p_state;          
+SensorStruct p_sensor;
+ControlStruct controller;
+CommunicationStruct com;
+
+void control(void);
+
+void serial_isr(void)
+{
+    dbg = 1;
+    int bytes[5];
+    /*
+    while(pc.readable())
+    {
+        bytes[bytecount] = pc.getc();   
+        bytecount ++;
+    }
+    */
+    bytes[0] = pc.getc();
+    bytes[1] = pc.getc();
+    bytes[2] = pc.getc();
+    bytes[3] = pc.getc();
+    bytes[4] = pc.getc();
+    
+    switch (bytes[4]){
+    case(TORQUE_COMMAND):
+        //pc.printf("%f\n", p_state.q[1]);
+        com.torque_command.bytes[3] = bytes[3];
+        com.torque_command.bytes[2] = bytes[2];
+        com.torque_command.bytes[1] = bytes[1];
+        com.torque_command.bytes[0] = bytes[0];
+        controller.tau = com.torque_command.f;
+        //pc.printf("%f\n", com.torque_command.f);
+        //pc.printf("%d\n", bytecount);
+        break;
+    case(READ_ENCODER_0):
+        com.encoder_0.i = p_sensor.encoder_count[0];
+        pc.putc(com.encoder_0.bytes[0]);
+        pc.putc(com.encoder_0.bytes[1]);
+        pc.putc(READ_ENCODER_0);
+        break;
+    case(READ_ENCODER_1):
+        com.encoder_1.i = p_sensor.encoder_count[1];
+        pc.putc(com.encoder_1.bytes[0]);
+        pc.putc(com.encoder_1.bytes[1]);
+        pc.putc(READ_ENCODER_1);
+        break;
+    case(READ_Q_0):
+        com.q0.f = p_sensor.q_sensed[0];
+        pc.putc(com.q0.bytes[0]);
+        pc.putc(com.q0.bytes[1]);
+        pc.putc(com.q0.bytes[2]);
+        pc.putc(com.q0.bytes[3]);
+        pc.putc(READ_Q_0);
+        break;
+    case(READ_Q_1):
+        com.q1.f = p_sensor.q_sensed[1];
+        pc.putc(com.q1.bytes[0]);
+        pc.putc(com.q1.bytes[1]);
+        pc.putc(com.q1.bytes[2]);
+        pc.putc(com.q1.bytes[3]);
+        pc.putc(READ_Q_1);
+        break;
+    case(READ_QD_0):
+        com.qd0.f = p_sensor.qd_sensed[0];
+        pc.putc(com.qd0.bytes[0]);
+        pc.putc(com.qd0.bytes[1]);
+        pc.putc(com.qd0.bytes[2]);
+        pc.putc(com.qd0.bytes[3]);
+        pc.putc(READ_QD_0);
+        break;
+    case(READ_QD_1):
+        com.qd1.f = p_sensor.qd_sensed[1];
+        pc.putc(com.qd1.bytes[0]);
+        pc.putc(com.qd1.bytes[1]);
+        pc.putc(com.qd1.bytes[2]);
+        pc.putc(com.qd1.bytes[3]);
+        pc.putc(READ_QD_1);
+        break;
+    case(READ_ENCODERS):
+        com.encoder_0.i = p_sensor.encoder_count[0];
+        pc.putc(com.encoder_0.bytes[0]);
+        pc.putc(com.encoder_0.bytes[1]);
+        com.encoder_1.i = p_sensor.encoder_count[1];
+        pc.putc(com.encoder_1.bytes[0]);
+        pc.putc(com.encoder_1.bytes[1]);
+        pc.putc(READ_ENCODERS);
+        break;
+    }   // End Switch
+    
+    dbg = 0;
+    
+}
+
+void run_dynamics(void)
+{
+    
+    //dbg = 1;
+    integrate_state(&p_state, controller.tau, DT);   
+    update_sensors(p_state, &p_sensor);
+    control();
+    //dbg = 0;
+    
+}
+
+void control(void)
+{
+    // Swing-up Energy Shaping Controller //
+    if(abs(p_state.q[1]) >.6f)
+    {
+        float cA = cos(p_state.q[1]);
+        tau = .0001f*(cA*cA*cA*cA)*p_state.qd[1]*(9.81f*(1.0f-cA) - p_state.qd[1]*p_state.qd[1]*.0075f);
+        tau = fminf(fmaxf(-1.0f, tau), 1.0f);
+    }
+    
+    // Upright Linear Controller //
+    else
+    {
+        tau = .01f*p_state.qd[0] + 2.5f*p_state.q[1] + .15f*p_state.qd[1];
+        tau = fminf(fmaxf(tau, -1.0f), 1.0f);
+    }
+}
+
+int main() {
+    pc.baud(115200);
+    
+    p_state.q << 0, 3.14f;
+    p_state.qd << 0, .1;
+    tau = 0;
+    //printf("Eigen Testing\n\r");
+    
+    pc.attach(&serial_isr);
+    loop.attach(&run_dynamics, DT);
+    led = 0;
+    
+    while(1) {
+        //printf("%f\n\r", p_state.q[1]);
+        //printf("%.4f  %d  %d\n\r", p_state.q[1], p_sensor.encoder_count[0], p_sensor.encoder_count[1]);
+        wait(.01);
+        
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops.cpp	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,57 @@
+
+#include "../math_ops.h"
+
+
+float fmaxf(float x, float y){
+    /// Returns maximum of x, y ///
+    return (((x)>(y))?(x):(y));
+    }
+
+float fminf(float x, float y){
+    /// Returns minimum of x, y ///
+    return (((x)<(y))?(x):(y));
+    }
+
+float fmaxf3(float x, float y, float z){
+    /// Returns maximum of x, y, z ///
+    return (x > y ? (x > z ? x : z) : (y > z ? y : z));
+    }
+
+float fminf3(float x, float y, float z){
+    /// Returns minimum of x, y, z ///
+    return (x < y ? (x < z ? x : z) : (y < z ? y : z));
+    }
+
+float roundf(float x){
+    /// Returns nearest integer ///
+    
+    return x < 0.0f ? ceilf(x - 0.5f) : floorf(x + 0.5f);
+    }
+    
+void limit_norm(float *x, float *y, float limit){
+    /// Scales the lenght of vector (x, y) to be <= limit ///
+    float norm = sqrt(*x * *x + *y * *y);
+    if(norm > limit){
+        *x = *x * limit/norm;
+        *y = *y * limit/norm;
+        }
+    }
+    
+void limit(float *x, float min, float max){
+    *x = fmaxf(fminf(*x, max), min);
+    }
+
+int float_to_uint(float x, float x_min, float x_max, int bits){
+    /// Converts a float to an unsigned int, given range and number of bits ///
+    float span = x_max - x_min;
+    float offset = x_min;
+    return (int) ((x-offset)*((float)((1<<bits)-1))/span);
+    }
+    
+    
+float uint_to_float(int x_int, float x_min, float x_max, int bits){
+    /// converts unsigned int to float, given range and number of bits ///
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops.h	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,19 @@
+#ifndef MATH_OPS_H
+#define MATH_OPS_H
+
+#define PI 3.14159265359f
+#define SQRT3 1.73205080757f
+
+#include "math.h"
+
+float fmaxf(float x, float y);
+float fminf(float x, float y);
+float fmaxf3(float x, float y, float z);
+float fminf3(float x, float y, float z);
+float roundf(float x);
+void limit_norm(float *x, float *y, float limit);
+void limit(float *x, float min, float max);
+int float_to_uint(float x, float x_min, float x_max, int bits);
+float uint_to_float(int x_int, float x_min, float x_max, int bits);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/serial_com.h	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,21 @@
+#ifndef SERIAL_COM_H
+#define SERIAL_COM_H
+
+#include "mbed.h"
+#include "../structs.h"
+
+// Command Types //
+#define TORQUE_COMMAND      1
+#define READ_ENCODER_0      2
+#define READ_ENCODER_1      3
+#define READ_Q_0            4
+#define READ_Q_1            5
+#define READ_QD_0           6
+#define READ_QD_1           7
+#define READ_ENCODERS       8
+
+
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/structs.h	Tue May 07 01:56:53 2019 +0000
@@ -0,0 +1,47 @@
+#ifndef STRUCTS_H
+#define STRUCTS_H
+#include <Eigen/Dense.h>
+
+using namespace Eigen;
+
+// Union for easily converting floats to bytes for serial //
+union f_b
+{
+float f;
+uint8_t bytes[4];
+};
+
+union u16_b
+{
+    uint16_t i;
+    uint8_t bytes[2];
+};
+
+
+typedef struct
+{
+    Vector2f q, qd, qdd, tau, g;
+    Matrix2f h, c;
+} StateStruct;
+    
+typedef struct
+{
+    int encoder_count[2];
+    float q_sensed[2], qd_sensed[2];
+}SensorStruct;
+    
+typedef struct
+{
+    float tau, p_setpoint, p_gain, v_setpoint, v_gain;
+}ControlStruct;
+
+typedef struct
+{    
+// Inputs //
+u16_b p_setpoint, p_gain, v_setpoint, v_gain;
+// Outputs //
+u16_b encoder_0, encoder_1;
+f_b q0, q1, qd0, qd1, torque_command;
+}CommunicationStruct;
+    
+#endif