Joao Luiz Almeida de Souza Ramos / Mbed 2 deprecated furutacontroller

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:   QEI mbed-rtos mbed

main.cpp@12:d09b8ffa176f, 2013-12-04 (annotated)

Committer:
jaoramos
Date:
Wed Dec 04 04:36:27 2013 +0000
Revision:
12:d09b8ffa176f
Parent:
11:7e19e51b325d
Child:
13:6b3533b4f664
combined swing up and stabilization works!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jaoramos 0:9f2b0ea63eac 1 #include "mbed.h"
jaoramos 0:9f2b0ea63eac 2 #include "rtos.h"
jaoramos 0:9f2b0ea63eac 3 #include "QEI.h"
jaoramos 2:011e6115c77a 4 #include <fstream>
jaoramos 2:011e6115c77a 5 #include <iomanip>
jaoramos 8:57c2b7c94ce8 6 #include "stdlib.h"
jaoramos 1:5c05e0d08e61 7
jaoramos 4:8fcaff7801b0 8 #define MOTOR_PPR 300
jaoramos 1:5c05e0d08e61 9 #define ENCODER_PPR 1024
jaoramos 1:5c05e0d08e61 10
jaoramos 6:16da0de99a8c 11 #define ENC_QUADRATURE_TYPE 4
jaoramos 6:16da0de99a8c 12 #define MOT_QUADRATURE_TYPE 2
jaoramos 1:5c05e0d08e61 13 #define OUR_PI 3.141592653589793
jaoramos 5:d41998e421ed 14 #define DATA_COLS 7
jaoramos 5:d41998e421ed 15 #define BUFFER_SIZE 4200
jaoramos 1:5c05e0d08e61 16 #define MAX_VOLTAGE 3.3
jaoramos 1:5c05e0d08e61 17 #define VOLTS_PER_AMP 0.14
jaoramos 7:59613b7a1631 18 #define PROGRAM_RUNTIME 15.0
jaoramos 0:9f2b0ea63eac 19
jaoramos 0:9f2b0ea63eac 20 Serial pc(USBTX, USBRX);
jaoramos 0:9f2b0ea63eac 21
jaoramos 6:16da0de99a8c 22 QEI encoder(p29, p30, NC, ENCODER_PPR, QEI::X4_ENCODING);
jaoramos 1:5c05e0d08e61 23 QEI motor(p25, p26, NC, MOTOR_PPR);
jaoramos 0:9f2b0ea63eac 24 Timer T;
jaoramos 0:9f2b0ea63eac 25
jaoramos 1:5c05e0d08e61 26 //Curent Measurement
jaoramos 1:5c05e0d08e61 27 AnalogIn aIn(p16); //pin 15 set as analog input. Pins 15-20 can be used as analog inputs.
jaoramos 1:5c05e0d08e61 28
jaoramos 2:011e6115c77a 29 //Motor direction and PWM
jaoramos 2:011e6115c77a 30 DigitalOut dOut1(p5);
jaoramos 2:011e6115c77a 31 DigitalOut dOut2(p7);
jaoramos 2:011e6115c77a 32 PwmOut pwmOut(p21);
jaoramos 1:5c05e0d08e61 33
jaoramos 0:9f2b0ea63eac 34 // open a file for data logger
jaoramos 0:9f2b0ea63eac 35 LocalFileSystem local("local");
jaoramos 8:57c2b7c94ce8 36 float theta1, theta2, dtheta1, dtheta2, dtheta2MvFil;
jaoramos 1:5c05e0d08e61 37 float mCurrent = 0.0;
jaoramos 5:d41998e421ed 38 float inputVoltage = 0.0;
jaoramos 1:5c05e0d08e61 39 //int pulses0 = 0;
jaoramos 1:5c05e0d08e61 40 //int deltaPulses;
jaoramos 0:9f2b0ea63eac 41 float t0 = 0.0;
jaoramos 1:5c05e0d08e61 42 float t = 0.0, dt;
jaoramos 5:d41998e421ed 43
jaoramos 5:d41998e421ed 44 //Controller gains - Full-state Feedback
jaoramos 5:d41998e421ed 45 //float k1 = -0.0316, k2 = 9.7076, k3 = -0.4095, k4 = 1.2340, k5 = 0.0410;
jaoramos 11:7e19e51b325d 46 //float k1 = -0.3162, k2 = 18.278, k3 = -0.8964, k4 = 2.4441, k5 = 0.1843;
jaoramos 11:7e19e51b325d 47 float k1 = -0.1000 , k2 = 12.0293, k3 = -0.5357, k4 = 1.5522, k5 = 0.1684;
jaoramos 1:5c05e0d08e61 48
jaoramos 6:16da0de99a8c 49 float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(ENC_QUADRATURE_TYPE));
jaoramos 6:16da0de99a8c 50 float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(MOT_QUADRATURE_TYPE));
jaoramos 1:5c05e0d08e61 51
jaoramos 0:9f2b0ea63eac 52 float* buffer;
jaoramos 2:011e6115c77a 53 float lambda1 = 30, lambda2 = 30, lambda3 = 15;
jaoramos 0:9f2b0ea63eac 54 int index;
jaoramos 4:8fcaff7801b0 55 int pulsesPend, pulsesMot;
jaoramos 10:d14c702c1992 56 bool flag = 1;
jaoramos 0:9f2b0ea63eac 57
jaoramos 0:9f2b0ea63eac 58 void saving(void const *args) {
jaoramos 0:9f2b0ea63eac 59 index = 0;
jaoramos 10:d14c702c1992 60 while ((index < BUFFER_SIZE)&&(flag == 1)) {
jaoramos 1:5c05e0d08e61 61 buffer[index] = theta1;
jaoramos 1:5c05e0d08e61 62 buffer[index+1] = theta2;
jaoramos 1:5c05e0d08e61 63 buffer[index+2] = dtheta1;
jaoramos 1:5c05e0d08e61 64 buffer[index+3] = dtheta2;
jaoramos 1:5c05e0d08e61 65 buffer[index+4] = mCurrent;
jaoramos 5:d41998e421ed 66 buffer[index+5] = inputVoltage;
jaoramos 5:d41998e421ed 67 buffer[index+6] = t;
jaoramos 4:8fcaff7801b0 68 index = index + DATA_COLS;
jaoramos 1:5c05e0d08e61 69 Thread::wait(20);
jaoramos 0:9f2b0ea63eac 70 }
jaoramos 0:9f2b0ea63eac 71 }
jaoramos 0:9f2b0ea63eac 72
jaoramos 8:57c2b7c94ce8 73 float c2,
jaoramos 8:57c2b7c94ce8 74 g = 9.8,
jaoramos 8:57c2b7c94ce8 75 m2 = 0.0391 + 0.0259,
jaoramos 8:57c2b7c94ce8 76 L1 = (51+44.55)*0.001,
jaoramos 8:57c2b7c94ce8 77 l1 = -0.03478,
jaoramos 8:57c2b7c94ce8 78 L2 = 0.2983,
jaoramos 8:57c2b7c94ce8 79 l2 = 0.05847,
jaoramos 8:57c2b7c94ce8 80 I2x = 0.000534,
jaoramos 8:57c2b7c94ce8 81 I2y = 0.000841,
jaoramos 8:57c2b7c94ce8 82 I2z = 0.00031,
jaoramos 8:57c2b7c94ce8 83 Ixz2 = -0.00024;
jaoramos 9:4ff9849fc8f6 84
jaoramos 8:57c2b7c94ce8 85 float currentEnergy;
jaoramos 8:57c2b7c94ce8 86
jaoramos 8:57c2b7c94ce8 87 float calcEnergy(void)
jaoramos 8:57c2b7c94ce8 88 {
jaoramos 8:57c2b7c94ce8 89 c2 = cos(theta2);
jaoramos 10:d14c702c1992 90 //return (I2x*dtheta2*dtheta2)/2.0 + (I2y*dtheta1*dtheta1)/2.0 + (L1*L1*dtheta1*dtheta1*m2)/2.0 + (dtheta1*dtheta1*l2*l2*m2)/2.0 + (dtheta2*dtheta2*l2*l2*m2)/2.0
jaoramos 10:d14c702c1992 91 //- (I2y*dtheta1*dtheta1*c2*c2)/2.0 + (I2z*dtheta1*dtheta1*c2*c2)/2.0 + Ixz2*dtheta1*dtheta2*c2 + g*l2*m2*c2 - (dtheta1*dtheta1*l2*l2*m2*c2*c2)/2.0
jaoramos 10:d14c702c1992 92 //- L1*dtheta1*dtheta2*l2*m2*c2;
jaoramos 10:d14c702c1992 93 return (I2x*dtheta2*dtheta2)/2.0 + g*l2*m2*c2;
jaoramos 8:57c2b7c94ce8 94 }
jaoramos 8:57c2b7c94ce8 95
jaoramos 3:967aee5fed5b 96 void setVoltage(float inputVoltage)
jaoramos 3:967aee5fed5b 97 {
jaoramos 3:967aee5fed5b 98 if(inputVoltage<0.0) {
jaoramos 3:967aee5fed5b 99 inputVoltage = -inputVoltage;
jaoramos 3:967aee5fed5b 100 dOut1=0;
jaoramos 3:967aee5fed5b 101 dOut2=1;
jaoramos 3:967aee5fed5b 102 } else {
jaoramos 3:967aee5fed5b 103 dOut1=1;
jaoramos 3:967aee5fed5b 104 dOut2=0;
jaoramos 3:967aee5fed5b 105 }
jaoramos 3:967aee5fed5b 106 float dutyCycle = inputVoltage/MAX_VOLTAGE;
jaoramos 3:967aee5fed5b 107 dutyCycle = (dutyCycle > 1.0)? 1.0 : dutyCycle;
jaoramos 3:967aee5fed5b 108 pwmOut.write(dutyCycle);
jaoramos 3:967aee5fed5b 109 }
jaoramos 8:57c2b7c94ce8 110 //
jaoramos 8:57c2b7c94ce8 111 //#define MOVING_AVERAGE_SIZE 10
jaoramos 8:57c2b7c94ce8 112 //
jaoramos 8:57c2b7c94ce8 113 //float movingAverage(float unfiltered)
jaoramos 8:57c2b7c94ce8 114 //{
jaoramos 8:57c2b7c94ce8 115 // static std::deque<float> movAvgBuffer; // empty deque of floats
jaoramos 8:57c2b7c94ce8 116 // static float sum = 0.0;
jaoramos 8:57c2b7c94ce8 117 //
jaoramos 8:57c2b7c94ce8 118 // movAvgBuffer.push_front(unfiltered);
jaoramos 8:57c2b7c94ce8 119 // sum += unfiltered;
jaoramos 8:57c2b7c94ce8 120 //
jaoramos 8:57c2b7c94ce8 121 // if( movAvgBuffer.size() <= MOVING_AVERAGE_SIZE)
jaoramos 8:57c2b7c94ce8 122 // {
jaoramos 8:57c2b7c94ce8 123 // return (sum/float(movAvgBuffer.size()));
jaoramos 8:57c2b7c94ce8 124 // }
jaoramos 8:57c2b7c94ce8 125 // else
jaoramos 8:57c2b7c94ce8 126 // {
jaoramos 8:57c2b7c94ce8 127 // sum -= movAvgBuffer.back();
jaoramos 8:57c2b7c94ce8 128 // movAvgBuffer.pop_back();
jaoramos 8:57c2b7c94ce8 129 //
jaoramos 8:57c2b7c94ce8 130 // return (sum/float(MOVING_AVERAGE_SIZE));
jaoramos 8:57c2b7c94ce8 131 // }
jaoramos 8:57c2b7c94ce8 132 //}
jaoramos 3:967aee5fed5b 133
jaoramos 0:9f2b0ea63eac 134 void computing(void const *args) {
jaoramos 9:4ff9849fc8f6 135 float z1=0.0, z2=0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0;
jaoramos 9:4ff9849fc8f6 136 bool firstTime = true;
jaoramos 12:d09b8ffa176f 137 bool controller = 0;
jaoramos 7:59613b7a1631 138 while (true ) {
jaoramos 0:9f2b0ea63eac 139 t = T.read();
jaoramos 1:5c05e0d08e61 140
jaoramos 8:57c2b7c94ce8 141
jaoramos 2:011e6115c77a 142 //read current
jaoramos 1:5c05e0d08e61 143 mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP;
jaoramos 3:967aee5fed5b 144 if(dOut1 == 0)
jaoramos 3:967aee5fed5b 145 mCurrent = -mCurrent;
jaoramos 1:5c05e0d08e61 146 pulsesPend = -encoder.getPulses();
jaoramos 1:5c05e0d08e61 147 pulsesMot = motor.getPulses();
jaoramos 1:5c05e0d08e61 148
jaoramos 1:5c05e0d08e61 149 dt = t - t0; //time difference
jaoramos 4:8fcaff7801b0 150 theta2 = float(pulsesPend)*encoder_conv + OUR_PI;
jaoramos 1:5c05e0d08e61 151 theta1 = float(pulsesMot)*motor_conv;
jaoramos 9:4ff9849fc8f6 152 if(firstTime)
jaoramos 9:4ff9849fc8f6 153 {
jaoramos 9:4ff9849fc8f6 154 // z1 and z2 are in the beginning the same as the angle so that dtheta1 and dtheta2 are zero
jaoramos 9:4ff9849fc8f6 155 z1 = theta1;
jaoramos 10:d14c702c1992 156 z2 = 1.001*theta2;
jaoramos 9:4ff9849fc8f6 157 firstTime = false;
jaoramos 9:4ff9849fc8f6 158 }
jaoramos 1:5c05e0d08e61 159 //calculate dtheta1
jaoramos 2:011e6115c77a 160 dz1 = - lambda1 * z1 + lambda1 * theta1;
jaoramos 1:5c05e0d08e61 161 z1 = z1 + dz1 * dt;
jaoramos 1:5c05e0d08e61 162 dtheta1 = dz1;
jaoramos 0:9f2b0ea63eac 163
jaoramos 1:5c05e0d08e61 164 //calculate dtheta2
jaoramos 2:011e6115c77a 165 dz2 = - lambda2 * z2 + lambda2 * theta2;
jaoramos 1:5c05e0d08e61 166 z2 = z2 + dz2 * dt;
jaoramos 1:5c05e0d08e61 167 dtheta2 = dz2;
jaoramos 2:011e6115c77a 168
jaoramos 8:57c2b7c94ce8 169 //dtheta2MvFil = movingAverage(dtheta2);
jaoramos 8:57c2b7c94ce8 170
jaoramos 2:011e6115c77a 171 //filter current
jaoramos 2:011e6115c77a 172 dz3 = -lambda3 * z3 + lambda3 * mCurrent;
jaoramos 2:011e6115c77a 173 z3 = z3 + dz3 * dt;
jaoramos 2:011e6115c77a 174 mCurrent = z3;
jaoramos 2:011e6115c77a 175
jaoramos 12:d09b8ffa176f 176 if (sin(theta2) < 0.0)
jaoramos 12:d09b8ffa176f 177 controller = 1;
jaoramos 12:d09b8ffa176f 178 if (cos(theta2) < 0.96)
jaoramos 12:d09b8ffa176f 179 controller = 0;
jaoramos 12:d09b8ffa176f 180
jaoramos 10:d14c702c1992 181 //set pwm
jaoramos 12:d09b8ffa176f 182 if ( controller == 0) {
jaoramos 12:d09b8ffa176f 183
jaoramos 9:4ff9849fc8f6 184 flag = 1;
jaoramos 8:57c2b7c94ce8 185 currentEnergy = calcEnergy();
jaoramos 11:7e19e51b325d 186 inputVoltage = -2.3*dtheta2*(0.0372 - currentEnergy);
jaoramos 12:d09b8ffa176f 187 //inputVoltage = 0.0;
jaoramos 8:57c2b7c94ce8 188
jaoramos 8:57c2b7c94ce8 189 } else {
jaoramos 8:57c2b7c94ce8 190 flag = 1;
jaoramos 10:d14c702c1992 191 inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
jaoramos 8:57c2b7c94ce8 192 }
jaoramos 9:4ff9849fc8f6 193
jaoramos 8:57c2b7c94ce8 194 setVoltage(inputVoltage);
jaoramos 8:57c2b7c94ce8 195
jaoramos 0:9f2b0ea63eac 196 t0 = t;
jaoramos 0:9f2b0ea63eac 197 Thread::wait(1);
jaoramos 0:9f2b0ea63eac 198 }
jaoramos 0:9f2b0ea63eac 199 }
jaoramos 0:9f2b0ea63eac 200
jaoramos 0:9f2b0ea63eac 201 void saveToFile ()
jaoramos 0:9f2b0ea63eac 202 {
jaoramos 0:9f2b0ea63eac 203 FILE *fp = fopen("/local/data.csv", "w");
jaoramos 0:9f2b0ea63eac 204 if (!fp) {
jaoramos 0:9f2b0ea63eac 205 fprintf(stderr, "File could not be openend \n\r");
jaoramos 0:9f2b0ea63eac 206 exit(1);
jaoramos 0:9f2b0ea63eac 207 }
jaoramos 0:9f2b0ea63eac 208
jaoramos 0:9f2b0ea63eac 209 wait(2.0);
jaoramos 0:9f2b0ea63eac 210
jaoramos 5:d41998e421ed 211 for (int i=0; i < index; i = i + DATA_COLS)
jaoramos 1:5c05e0d08e61 212 {
jaoramos 5:d41998e421ed 213 for (int j = 0; j < DATA_COLS; j++)
jaoramos 1:5c05e0d08e61 214 {
jaoramos 1:5c05e0d08e61 215 fprintf(fp,"%f,", buffer[i+j]);
jaoramos 1:5c05e0d08e61 216 }
jaoramos 1:5c05e0d08e61 217 fprintf(fp,"\n");
jaoramos 0:9f2b0ea63eac 218 }
jaoramos 0:9f2b0ea63eac 219 pc.printf("closing file\n\r");
jaoramos 0:9f2b0ea63eac 220 fclose(fp);
jaoramos 0:9f2b0ea63eac 221 wait(2.0);;
jaoramos 0:9f2b0ea63eac 222 }
jaoramos 0:9f2b0ea63eac 223
jaoramos 0:9f2b0ea63eac 224 int main() {
jaoramos 0:9f2b0ea63eac 225 //allocate memory for the buffer
jaoramos 1:5c05e0d08e61 226 pc.printf("creating buffer!\r\n");
jaoramos 5:d41998e421ed 227 buffer = new float[BUFFER_SIZE];
jaoramos 1:5c05e0d08e61 228 pc.printf("done creating buffer!\r\n");
jaoramos 0:9f2b0ea63eac 229 T.start();
jaoramos 5:d41998e421ed 230 pwmOut.period(0.0001);
jaoramos 0:9f2b0ea63eac 231
jaoramos 9:4ff9849fc8f6 232 Thread::wait(2000);
jaoramos 9:4ff9849fc8f6 233
jaoramos 5:d41998e421ed 234 Thread thrd2(computing,NULL,osPriorityRealtime);
jaoramos 5:d41998e421ed 235 pc.printf("started computing thread!\r\n");
jaoramos 5:d41998e421ed 236 Thread thrd3(saving,NULL,osPriorityNormal);
jaoramos 5:d41998e421ed 237 pc.printf("started saving thread!\r\n");
jaoramos 5:d41998e421ed 238
jaoramos 2:011e6115c77a 239
jaoramos 0:9f2b0ea63eac 240 pc.printf("Start!\r\n");
jaoramos 0:9f2b0ea63eac 241 pc.printf("Time: %f\r\n", t);
jaoramos 8:57c2b7c94ce8 242
jaoramos 7:59613b7a1631 243 while (t < PROGRAM_RUNTIME)
jaoramos 0:9f2b0ea63eac 244 {
jaoramos 9:4ff9849fc8f6 245 //pc.printf("at time: %f energy: %f\n",t, currentEnergy);
jaoramos 8:57c2b7c94ce8 246
jaoramos 8:57c2b7c94ce8 247 Thread::wait(200);
jaoramos 0:9f2b0ea63eac 248 }
jaoramos 2:011e6115c77a 249 pc.printf("Done at Index: %d\r\n",index);
jaoramos 2:011e6115c77a 250 pwmOut.write(0.0);
jaoramos 0:9f2b0ea63eac 251 thrd2.terminate();
jaoramos 0:9f2b0ea63eac 252 thrd3.terminate();
jaoramos 0:9f2b0ea63eac 253 saveToFile();
jaoramos 0:9f2b0ea63eac 254 }