File content as of revision 0:4dd2d995f7d0:

// 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);
        
    }
}