Ben Katz / Mbed 2 deprecated Pendulum_demo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Eigen

main.cpp@0:4dd2d995f7d0, 2019-05-07 (annotated)

Committer:
benkatz
Date:
Tue May 07 01:56:53 2019 +0000
Revision:
0:4dd2d995f7d0
works with the python side control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:4dd2d995f7d0 1 // Furuta Pendulum Demo Firmware //
benkatz 0:4dd2d995f7d0 2 // This version simulates the pendulum dynamics and sensor outputs //
benkatz 0:4dd2d995f7d0 3 // Ben Katz, 2019 //
benkatz 0:4dd2d995f7d0 4
benkatz 0:4dd2d995f7d0 5 #include "mbed.h"
benkatz 0:4dd2d995f7d0 6 #include <iostream>
benkatz 0:4dd2d995f7d0 7 #include <Eigen/Dense.h>
benkatz 0:4dd2d995f7d0 8 #include "dynamics.h"
benkatz 0:4dd2d995f7d0 9 #include "structs.h"
benkatz 0:4dd2d995f7d0 10 #include "math_ops.h"
benkatz 0:4dd2d995f7d0 11 #include "serial_com.h"
benkatz 0:4dd2d995f7d0 12
benkatz 0:4dd2d995f7d0 13 #define DT .0001f
benkatz 0:4dd2d995f7d0 14
benkatz 0:4dd2d995f7d0 15
benkatz 0:4dd2d995f7d0 16 using namespace std;
benkatz 0:4dd2d995f7d0 17 using namespace Eigen;
benkatz 0:4dd2d995f7d0 18
benkatz 0:4dd2d995f7d0 19
benkatz 0:4dd2d995f7d0 20 Ticker loop;
benkatz 0:4dd2d995f7d0 21 DigitalOut dbg(PC_8);
benkatz 0:4dd2d995f7d0 22 DigitalOut led(PA_5);
benkatz 0:4dd2d995f7d0 23 Serial pc(USBTX, USBRX);
benkatz 0:4dd2d995f7d0 24
benkatz 0:4dd2d995f7d0 25
benkatz 0:4dd2d995f7d0 26 float tau;
benkatz 0:4dd2d995f7d0 27
benkatz 0:4dd2d995f7d0 28 StateStruct p_state;
benkatz 0:4dd2d995f7d0 29 SensorStruct p_sensor;
benkatz 0:4dd2d995f7d0 30 ControlStruct controller;
benkatz 0:4dd2d995f7d0 31 CommunicationStruct com;
benkatz 0:4dd2d995f7d0 32
benkatz 0:4dd2d995f7d0 33 void control(void);
benkatz 0:4dd2d995f7d0 34
benkatz 0:4dd2d995f7d0 35 void serial_isr(void)
benkatz 0:4dd2d995f7d0 36 {
benkatz 0:4dd2d995f7d0 37 dbg = 1;
benkatz 0:4dd2d995f7d0 38 int bytes[5];
benkatz 0:4dd2d995f7d0 39 /*
benkatz 0:4dd2d995f7d0 40 while(pc.readable())
benkatz 0:4dd2d995f7d0 41 {
benkatz 0:4dd2d995f7d0 42 bytes[bytecount] = pc.getc();
benkatz 0:4dd2d995f7d0 43 bytecount ++;
benkatz 0:4dd2d995f7d0 44 }
benkatz 0:4dd2d995f7d0 45 */
benkatz 0:4dd2d995f7d0 46 bytes[0] = pc.getc();
benkatz 0:4dd2d995f7d0 47 bytes[1] = pc.getc();
benkatz 0:4dd2d995f7d0 48 bytes[2] = pc.getc();
benkatz 0:4dd2d995f7d0 49 bytes[3] = pc.getc();
benkatz 0:4dd2d995f7d0 50 bytes[4] = pc.getc();
benkatz 0:4dd2d995f7d0 51
benkatz 0:4dd2d995f7d0 52 switch (bytes[4]){
benkatz 0:4dd2d995f7d0 53 case(TORQUE_COMMAND):
benkatz 0:4dd2d995f7d0 54 //pc.printf("%f\n", p_state.q[1]);
benkatz 0:4dd2d995f7d0 55 com.torque_command.bytes[3] = bytes[3];
benkatz 0:4dd2d995f7d0 56 com.torque_command.bytes[2] = bytes[2];
benkatz 0:4dd2d995f7d0 57 com.torque_command.bytes[1] = bytes[1];
benkatz 0:4dd2d995f7d0 58 com.torque_command.bytes[0] = bytes[0];
benkatz 0:4dd2d995f7d0 59 controller.tau = com.torque_command.f;
benkatz 0:4dd2d995f7d0 60 //pc.printf("%f\n", com.torque_command.f);
benkatz 0:4dd2d995f7d0 61 //pc.printf("%d\n", bytecount);
benkatz 0:4dd2d995f7d0 62 break;
benkatz 0:4dd2d995f7d0 63 case(READ_ENCODER_0):
benkatz 0:4dd2d995f7d0 64 com.encoder_0.i = p_sensor.encoder_count[0];
benkatz 0:4dd2d995f7d0 65 pc.putc(com.encoder_0.bytes[0]);
benkatz 0:4dd2d995f7d0 66 pc.putc(com.encoder_0.bytes[1]);
benkatz 0:4dd2d995f7d0 67 pc.putc(READ_ENCODER_0);
benkatz 0:4dd2d995f7d0 68 break;
benkatz 0:4dd2d995f7d0 69 case(READ_ENCODER_1):
benkatz 0:4dd2d995f7d0 70 com.encoder_1.i = p_sensor.encoder_count[1];
benkatz 0:4dd2d995f7d0 71 pc.putc(com.encoder_1.bytes[0]);
benkatz 0:4dd2d995f7d0 72 pc.putc(com.encoder_1.bytes[1]);
benkatz 0:4dd2d995f7d0 73 pc.putc(READ_ENCODER_1);
benkatz 0:4dd2d995f7d0 74 break;
benkatz 0:4dd2d995f7d0 75 case(READ_Q_0):
benkatz 0:4dd2d995f7d0 76 com.q0.f = p_sensor.q_sensed[0];
benkatz 0:4dd2d995f7d0 77 pc.putc(com.q0.bytes[0]);
benkatz 0:4dd2d995f7d0 78 pc.putc(com.q0.bytes[1]);
benkatz 0:4dd2d995f7d0 79 pc.putc(com.q0.bytes[2]);
benkatz 0:4dd2d995f7d0 80 pc.putc(com.q0.bytes[3]);
benkatz 0:4dd2d995f7d0 81 pc.putc(READ_Q_0);
benkatz 0:4dd2d995f7d0 82 break;
benkatz 0:4dd2d995f7d0 83 case(READ_Q_1):
benkatz 0:4dd2d995f7d0 84 com.q1.f = p_sensor.q_sensed[1];
benkatz 0:4dd2d995f7d0 85 pc.putc(com.q1.bytes[0]);
benkatz 0:4dd2d995f7d0 86 pc.putc(com.q1.bytes[1]);
benkatz 0:4dd2d995f7d0 87 pc.putc(com.q1.bytes[2]);
benkatz 0:4dd2d995f7d0 88 pc.putc(com.q1.bytes[3]);
benkatz 0:4dd2d995f7d0 89 pc.putc(READ_Q_1);
benkatz 0:4dd2d995f7d0 90 break;
benkatz 0:4dd2d995f7d0 91 case(READ_QD_0):
benkatz 0:4dd2d995f7d0 92 com.qd0.f = p_sensor.qd_sensed[0];
benkatz 0:4dd2d995f7d0 93 pc.putc(com.qd0.bytes[0]);
benkatz 0:4dd2d995f7d0 94 pc.putc(com.qd0.bytes[1]);
benkatz 0:4dd2d995f7d0 95 pc.putc(com.qd0.bytes[2]);
benkatz 0:4dd2d995f7d0 96 pc.putc(com.qd0.bytes[3]);
benkatz 0:4dd2d995f7d0 97 pc.putc(READ_QD_0);
benkatz 0:4dd2d995f7d0 98 break;
benkatz 0:4dd2d995f7d0 99 case(READ_QD_1):
benkatz 0:4dd2d995f7d0 100 com.qd1.f = p_sensor.qd_sensed[1];
benkatz 0:4dd2d995f7d0 101 pc.putc(com.qd1.bytes[0]);
benkatz 0:4dd2d995f7d0 102 pc.putc(com.qd1.bytes[1]);
benkatz 0:4dd2d995f7d0 103 pc.putc(com.qd1.bytes[2]);
benkatz 0:4dd2d995f7d0 104 pc.putc(com.qd1.bytes[3]);
benkatz 0:4dd2d995f7d0 105 pc.putc(READ_QD_1);
benkatz 0:4dd2d995f7d0 106 break;
benkatz 0:4dd2d995f7d0 107 case(READ_ENCODERS):
benkatz 0:4dd2d995f7d0 108 com.encoder_0.i = p_sensor.encoder_count[0];
benkatz 0:4dd2d995f7d0 109 pc.putc(com.encoder_0.bytes[0]);
benkatz 0:4dd2d995f7d0 110 pc.putc(com.encoder_0.bytes[1]);
benkatz 0:4dd2d995f7d0 111 com.encoder_1.i = p_sensor.encoder_count[1];
benkatz 0:4dd2d995f7d0 112 pc.putc(com.encoder_1.bytes[0]);
benkatz 0:4dd2d995f7d0 113 pc.putc(com.encoder_1.bytes[1]);
benkatz 0:4dd2d995f7d0 114 pc.putc(READ_ENCODERS);
benkatz 0:4dd2d995f7d0 115 break;
benkatz 0:4dd2d995f7d0 116 } // End Switch
benkatz 0:4dd2d995f7d0 117
benkatz 0:4dd2d995f7d0 118 dbg = 0;
benkatz 0:4dd2d995f7d0 119
benkatz 0:4dd2d995f7d0 120 }
benkatz 0:4dd2d995f7d0 121
benkatz 0:4dd2d995f7d0 122 void run_dynamics(void)
benkatz 0:4dd2d995f7d0 123 {
benkatz 0:4dd2d995f7d0 124
benkatz 0:4dd2d995f7d0 125 //dbg = 1;
benkatz 0:4dd2d995f7d0 126 integrate_state(&p_state, controller.tau, DT);
benkatz 0:4dd2d995f7d0 127 update_sensors(p_state, &p_sensor);
benkatz 0:4dd2d995f7d0 128 control();
benkatz 0:4dd2d995f7d0 129 //dbg = 0;
benkatz 0:4dd2d995f7d0 130
benkatz 0:4dd2d995f7d0 131 }
benkatz 0:4dd2d995f7d0 132
benkatz 0:4dd2d995f7d0 133 void control(void)
benkatz 0:4dd2d995f7d0 134 {
benkatz 0:4dd2d995f7d0 135 // Swing-up Energy Shaping Controller //
benkatz 0:4dd2d995f7d0 136 if(abs(p_state.q[1]) >.6f)
benkatz 0:4dd2d995f7d0 137 {
benkatz 0:4dd2d995f7d0 138 float cA = cos(p_state.q[1]);
benkatz 0:4dd2d995f7d0 139 tau = .0001f*(cA*cA*cA*cA)*p_state.qd[1]*(9.81f*(1.0f-cA) - p_state.qd[1]*p_state.qd[1]*.0075f);
benkatz 0:4dd2d995f7d0 140 tau = fminf(fmaxf(-1.0f, tau), 1.0f);
benkatz 0:4dd2d995f7d0 141 }
benkatz 0:4dd2d995f7d0 142
benkatz 0:4dd2d995f7d0 143 // Upright Linear Controller //
benkatz 0:4dd2d995f7d0 144 else
benkatz 0:4dd2d995f7d0 145 {
benkatz 0:4dd2d995f7d0 146 tau = .01f*p_state.qd[0] + 2.5f*p_state.q[1] + .15f*p_state.qd[1];
benkatz 0:4dd2d995f7d0 147 tau = fminf(fmaxf(tau, -1.0f), 1.0f);
benkatz 0:4dd2d995f7d0 148 }
benkatz 0:4dd2d995f7d0 149 }
benkatz 0:4dd2d995f7d0 150
benkatz 0:4dd2d995f7d0 151 int main() {
benkatz 0:4dd2d995f7d0 152 pc.baud(115200);
benkatz 0:4dd2d995f7d0 153
benkatz 0:4dd2d995f7d0 154 p_state.q << 0, 3.14f;
benkatz 0:4dd2d995f7d0 155 p_state.qd << 0, .1;
benkatz 0:4dd2d995f7d0 156 tau = 0;
benkatz 0:4dd2d995f7d0 157 //printf("Eigen Testing\n\r");
benkatz 0:4dd2d995f7d0 158
benkatz 0:4dd2d995f7d0 159 pc.attach(&serial_isr);
benkatz 0:4dd2d995f7d0 160 loop.attach(&run_dynamics, DT);
benkatz 0:4dd2d995f7d0 161 led = 0;
benkatz 0:4dd2d995f7d0 162
benkatz 0:4dd2d995f7d0 163 while(1) {
benkatz 0:4dd2d995f7d0 164 //printf("%f\n\r", p_state.q[1]);
benkatz 0:4dd2d995f7d0 165 //printf("%.4f %d %d\n\r", p_state.q[1], p_sensor.encoder_count[0], p_sensor.encoder_count[1]);
benkatz 0:4dd2d995f7d0 166 wait(.01);
benkatz 0:4dd2d995f7d0 167
benkatz 0:4dd2d995f7d0 168 }
benkatz 0:4dd2d995f7d0 169 }