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@7:59613b7a1631, 2013-12-04 (annotated)

Committer:
jaoramos
Date:
Wed Dec 04 01:49:01 2013 +0000
Revision:
7:59613b7a1631
Parent:
6:16da0de99a8c
Child:
8:57c2b7c94ce8
working before adding moving average

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 1:5c05e0d08e61 6
jaoramos 4:8fcaff7801b0 7 #define MOTOR_PPR 300
jaoramos 1:5c05e0d08e61 8 #define ENCODER_PPR 1024
jaoramos 1:5c05e0d08e61 9
jaoramos 6:16da0de99a8c 10 #define ENC_QUADRATURE_TYPE 4
jaoramos 6:16da0de99a8c 11 #define MOT_QUADRATURE_TYPE 2
jaoramos 1:5c05e0d08e61 12 #define OUR_PI 3.141592653589793
jaoramos 5:d41998e421ed 13 #define DATA_COLS 7
jaoramos 5:d41998e421ed 14 #define BUFFER_SIZE 4200
jaoramos 1:5c05e0d08e61 15 #define MAX_VOLTAGE 3.3
jaoramos 1:5c05e0d08e61 16 #define VOLTS_PER_AMP 0.14
jaoramos 7:59613b7a1631 17 #define PROGRAM_RUNTIME 15.0
jaoramos 0:9f2b0ea63eac 18
jaoramos 0:9f2b0ea63eac 19 Serial pc(USBTX, USBRX);
jaoramos 0:9f2b0ea63eac 20
jaoramos 6:16da0de99a8c 21 QEI encoder(p29, p30, NC, ENCODER_PPR, QEI::X4_ENCODING);
jaoramos 1:5c05e0d08e61 22 QEI motor(p25, p26, NC, MOTOR_PPR);
jaoramos 0:9f2b0ea63eac 23 Timer T;
jaoramos 0:9f2b0ea63eac 24
jaoramos 1:5c05e0d08e61 25 //Curent Measurement
jaoramos 1:5c05e0d08e61 26 AnalogIn aIn(p16); //pin 15 set as analog input. Pins 15-20 can be used as analog inputs.
jaoramos 1:5c05e0d08e61 27
jaoramos 2:011e6115c77a 28 //Motor direction and PWM
jaoramos 2:011e6115c77a 29 DigitalOut dOut1(p5);
jaoramos 2:011e6115c77a 30 DigitalOut dOut2(p7);
jaoramos 2:011e6115c77a 31 PwmOut pwmOut(p21);
jaoramos 1:5c05e0d08e61 32
jaoramos 0:9f2b0ea63eac 33 // open a file for data logger
jaoramos 0:9f2b0ea63eac 34 LocalFileSystem local("local");
jaoramos 1:5c05e0d08e61 35 float theta1, theta2, dtheta1, dtheta2;
jaoramos 1:5c05e0d08e61 36 float mCurrent = 0.0;
jaoramos 5:d41998e421ed 37 float inputVoltage = 0.0;
jaoramos 1:5c05e0d08e61 38 //int pulses0 = 0;
jaoramos 1:5c05e0d08e61 39 //int deltaPulses;
jaoramos 0:9f2b0ea63eac 40 float t0 = 0.0;
jaoramos 1:5c05e0d08e61 41 float t = 0.0, dt;
jaoramos 5:d41998e421ed 42
jaoramos 5:d41998e421ed 43 //Controller gains - Full-state Feedback
jaoramos 5:d41998e421ed 44 //float k1 = -0.0316, k2 = 9.7076, k3 = -0.4095, k4 = 1.2340, k5 = 0.0410;
jaoramos 5:d41998e421ed 45 float k1 = -0.3162, k2 = 18.278, k3 = -0.8964, k4 = 2.4441, k5 = 0.1843;
jaoramos 1:5c05e0d08e61 46
jaoramos 6:16da0de99a8c 47 float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(ENC_QUADRATURE_TYPE));
jaoramos 6:16da0de99a8c 48 float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(MOT_QUADRATURE_TYPE));
jaoramos 1:5c05e0d08e61 49
jaoramos 0:9f2b0ea63eac 50 float* buffer;
jaoramos 2:011e6115c77a 51 float lambda1 = 30, lambda2 = 30, lambda3 = 15;
jaoramos 0:9f2b0ea63eac 52 int index;
jaoramos 4:8fcaff7801b0 53 int pulsesPend, pulsesMot;
jaoramos 6:16da0de99a8c 54 bool flag = 0;
jaoramos 0:9f2b0ea63eac 55
jaoramos 0:9f2b0ea63eac 56 void saving(void const *args) {
jaoramos 0:9f2b0ea63eac 57 index = 0;
jaoramos 7:59613b7a1631 58 while ((index < BUFFER_SIZE)&&(flag == 1)&&(false)) {
jaoramos 1:5c05e0d08e61 59 buffer[index] = theta1;
jaoramos 1:5c05e0d08e61 60 buffer[index+1] = theta2;
jaoramos 1:5c05e0d08e61 61 buffer[index+2] = dtheta1;
jaoramos 1:5c05e0d08e61 62 buffer[index+3] = dtheta2;
jaoramos 1:5c05e0d08e61 63 buffer[index+4] = mCurrent;
jaoramos 5:d41998e421ed 64 buffer[index+5] = inputVoltage;
jaoramos 5:d41998e421ed 65 buffer[index+6] = t;
jaoramos 4:8fcaff7801b0 66 index = index + DATA_COLS;
jaoramos 1:5c05e0d08e61 67 Thread::wait(20);
jaoramos 0:9f2b0ea63eac 68 }
jaoramos 0:9f2b0ea63eac 69 }
jaoramos 0:9f2b0ea63eac 70
jaoramos 3:967aee5fed5b 71 void setVoltage(float inputVoltage)
jaoramos 3:967aee5fed5b 72 {
jaoramos 3:967aee5fed5b 73 if(inputVoltage<0.0) {
jaoramos 3:967aee5fed5b 74 inputVoltage = -inputVoltage;
jaoramos 3:967aee5fed5b 75 dOut1=0;
jaoramos 3:967aee5fed5b 76 dOut2=1;
jaoramos 3:967aee5fed5b 77 } else {
jaoramos 3:967aee5fed5b 78 dOut1=1;
jaoramos 3:967aee5fed5b 79 dOut2=0;
jaoramos 3:967aee5fed5b 80 }
jaoramos 3:967aee5fed5b 81 float dutyCycle = inputVoltage/MAX_VOLTAGE;
jaoramos 3:967aee5fed5b 82 dutyCycle = (dutyCycle > 1.0)? 1.0 : dutyCycle;
jaoramos 3:967aee5fed5b 83 pwmOut.write(dutyCycle);
jaoramos 3:967aee5fed5b 84 }
jaoramos 3:967aee5fed5b 85
jaoramos 0:9f2b0ea63eac 86 void computing(void const *args) {
jaoramos 5:d41998e421ed 87 float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0;
jaoramos 4:8fcaff7801b0 88
jaoramos 7:59613b7a1631 89 while (true ) {
jaoramos 0:9f2b0ea63eac 90 t = T.read();
jaoramos 1:5c05e0d08e61 91
jaoramos 2:011e6115c77a 92 //set pwm
jaoramos 4:8fcaff7801b0 93 // ADD A SANITY CHECK ON THETA
jaoramos 6:16da0de99a8c 94 if (cos(theta2) < 0.98) {
jaoramos 6:16da0de99a8c 95 flag = 0;
jaoramos 4:8fcaff7801b0 96 inputVoltage = 0.0;
jaoramos 6:16da0de99a8c 97 } else {
jaoramos 6:16da0de99a8c 98 flag = 1;
jaoramos 6:16da0de99a8c 99 inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
jaoramos 6:16da0de99a8c 100 }
jaoramos 3:967aee5fed5b 101 setVoltage(inputVoltage);
jaoramos 2:011e6115c77a 102
jaoramos 2:011e6115c77a 103 //read current
jaoramos 1:5c05e0d08e61 104 mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP;
jaoramos 3:967aee5fed5b 105 if(dOut1 == 0)
jaoramos 3:967aee5fed5b 106 mCurrent = -mCurrent;
jaoramos 1:5c05e0d08e61 107 pulsesPend = -encoder.getPulses();
jaoramos 1:5c05e0d08e61 108 pulsesMot = motor.getPulses();
jaoramos 1:5c05e0d08e61 109
jaoramos 1:5c05e0d08e61 110 dt = t - t0; //time difference
jaoramos 4:8fcaff7801b0 111 theta2 = float(pulsesPend)*encoder_conv + OUR_PI;
jaoramos 1:5c05e0d08e61 112 theta1 = float(pulsesMot)*motor_conv;
jaoramos 0:9f2b0ea63eac 113
jaoramos 1:5c05e0d08e61 114 //calculate dtheta1
jaoramos 2:011e6115c77a 115 dz1 = - lambda1 * z1 + lambda1 * theta1;
jaoramos 1:5c05e0d08e61 116 z1 = z1 + dz1 * dt;
jaoramos 1:5c05e0d08e61 117 dtheta1 = dz1;
jaoramos 0:9f2b0ea63eac 118
jaoramos 1:5c05e0d08e61 119 //calculate dtheta2
jaoramos 2:011e6115c77a 120 dz2 = - lambda2 * z2 + lambda2 * theta2;
jaoramos 1:5c05e0d08e61 121 z2 = z2 + dz2 * dt;
jaoramos 1:5c05e0d08e61 122 dtheta2 = dz2;
jaoramos 2:011e6115c77a 123
jaoramos 2:011e6115c77a 124 //filter current
jaoramos 2:011e6115c77a 125 dz3 = -lambda3 * z3 + lambda3 * mCurrent;
jaoramos 2:011e6115c77a 126 z3 = z3 + dz3 * dt;
jaoramos 2:011e6115c77a 127 mCurrent = z3;
jaoramos 2:011e6115c77a 128
jaoramos 0:9f2b0ea63eac 129 t0 = t;
jaoramos 0:9f2b0ea63eac 130 Thread::wait(1);
jaoramos 0:9f2b0ea63eac 131 }
jaoramos 0:9f2b0ea63eac 132 }
jaoramos 0:9f2b0ea63eac 133
jaoramos 0:9f2b0ea63eac 134 void saveToFile ()
jaoramos 0:9f2b0ea63eac 135 {
jaoramos 0:9f2b0ea63eac 136 FILE *fp = fopen("/local/data.csv", "w");
jaoramos 0:9f2b0ea63eac 137 if (!fp) {
jaoramos 0:9f2b0ea63eac 138 fprintf(stderr, "File could not be openend \n\r");
jaoramos 0:9f2b0ea63eac 139 exit(1);
jaoramos 0:9f2b0ea63eac 140 }
jaoramos 0:9f2b0ea63eac 141
jaoramos 0:9f2b0ea63eac 142 wait(2.0);
jaoramos 0:9f2b0ea63eac 143
jaoramos 5:d41998e421ed 144 for (int i=0; i < index; i = i + DATA_COLS)
jaoramos 1:5c05e0d08e61 145 {
jaoramos 5:d41998e421ed 146 for (int j = 0; j < DATA_COLS; j++)
jaoramos 1:5c05e0d08e61 147 {
jaoramos 1:5c05e0d08e61 148 fprintf(fp,"%f,", buffer[i+j]);
jaoramos 1:5c05e0d08e61 149 }
jaoramos 1:5c05e0d08e61 150 fprintf(fp,"\n");
jaoramos 0:9f2b0ea63eac 151 }
jaoramos 0:9f2b0ea63eac 152 pc.printf("closing file\n\r");
jaoramos 0:9f2b0ea63eac 153 fclose(fp);
jaoramos 0:9f2b0ea63eac 154 wait(2.0);;
jaoramos 0:9f2b0ea63eac 155 }
jaoramos 0:9f2b0ea63eac 156
jaoramos 0:9f2b0ea63eac 157 int main() {
jaoramos 0:9f2b0ea63eac 158 //allocate memory for the buffer
jaoramos 1:5c05e0d08e61 159 pc.printf("creating buffer!\r\n");
jaoramos 5:d41998e421ed 160 buffer = new float[BUFFER_SIZE];
jaoramos 1:5c05e0d08e61 161 pc.printf("done creating buffer!\r\n");
jaoramos 0:9f2b0ea63eac 162 T.start();
jaoramos 5:d41998e421ed 163 pwmOut.period(0.0001);
jaoramos 0:9f2b0ea63eac 164
jaoramos 5:d41998e421ed 165 Thread thrd2(computing,NULL,osPriorityRealtime);
jaoramos 5:d41998e421ed 166 pc.printf("started computing thread!\r\n");
jaoramos 5:d41998e421ed 167 Thread thrd3(saving,NULL,osPriorityNormal);
jaoramos 5:d41998e421ed 168 pc.printf("started saving thread!\r\n");
jaoramos 5:d41998e421ed 169
jaoramos 2:011e6115c77a 170
jaoramos 0:9f2b0ea63eac 171 pc.printf("Start!\r\n");
jaoramos 0:9f2b0ea63eac 172 pc.printf("Time: %f\r\n", t);
jaoramos 7:59613b7a1631 173 while (t < PROGRAM_RUNTIME)
jaoramos 0:9f2b0ea63eac 174 {
jaoramos 4:8fcaff7801b0 175 //pc.printf("Time: %f\r\n", t);
jaoramos 0:9f2b0ea63eac 176 Thread::wait(1000);
jaoramos 0:9f2b0ea63eac 177 }
jaoramos 2:011e6115c77a 178 pc.printf("Done at Index: %d\r\n",index);
jaoramos 2:011e6115c77a 179 pwmOut.write(0.0);
jaoramos 0:9f2b0ea63eac 180 thrd2.terminate();
jaoramos 0:9f2b0ea63eac 181 thrd3.terminate();
jaoramos 0:9f2b0ea63eac 182 saveToFile();
jaoramos 0:9f2b0ea63eac 183 }