Diff: main.cpp

Revision:

0:4dd2d995f7d0

diff -r 000000000000 -r 4dd2d995f7d0 main.cpp
--- /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);
+        
+    }
+}