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@1:5c05e0d08e61, 2013-12-02 (annotated)

Committer:
jaoramos
Date:
Mon Dec 02 00:09:50 2013 +0000
Revision:
1:5c05e0d08e61
Parent:
0:9f2b0ea63eac
Child:
2:011e6115c77a
record angles velocities and current

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 0:9f2b0ea63eac 4
jaoramos 1:5c05e0d08e61 5
jaoramos 1:5c05e0d08e61 6 #define MOTOR_PPR 1200
jaoramos 1:5c05e0d08e61 7 #define ENCODER_PPR 1024
jaoramos 1:5c05e0d08e61 8
jaoramos 1:5c05e0d08e61 9 #define QUADRATURE_TYPE 2
jaoramos 1:5c05e0d08e61 10 #define OUR_PI 3.141592653589793
jaoramos 1:5c05e0d08e61 11 #define DATA_COLS 6
jaoramos 1:5c05e0d08e61 12 #define NR_SAMPLES 1500
jaoramos 1:5c05e0d08e61 13 #define buffer_size 3500
jaoramos 1:5c05e0d08e61 14 #define MAX_VOLTAGE 3.3
jaoramos 1:5c05e0d08e61 15 #define VOLTS_PER_AMP 0.14
jaoramos 1:5c05e0d08e61 16
jaoramos 0:9f2b0ea63eac 17
jaoramos 0:9f2b0ea63eac 18 Serial pc(USBTX, USBRX);
jaoramos 0:9f2b0ea63eac 19
jaoramos 1:5c05e0d08e61 20 QEI encoder(p29, p30, NC, ENCODER_PPR);
jaoramos 1:5c05e0d08e61 21 QEI motor(p25, p26, NC, MOTOR_PPR);
jaoramos 0:9f2b0ea63eac 22 Timer T;
jaoramos 0:9f2b0ea63eac 23
jaoramos 1:5c05e0d08e61 24 //Curent Measurement
jaoramos 1:5c05e0d08e61 25 AnalogIn aIn(p16); //pin 15 set as analog input. Pins 15-20 can be used as analog inputs.
jaoramos 1:5c05e0d08e61 26
jaoramos 1:5c05e0d08e61 27
jaoramos 0:9f2b0ea63eac 28 // open a file for data logger
jaoramos 0:9f2b0ea63eac 29 LocalFileSystem local("local");
jaoramos 1:5c05e0d08e61 30 //const int buffer_size = DATA_COLS * NR_SAMPLES;
jaoramos 1:5c05e0d08e61 31 float theta1, theta2, dtheta1, dtheta2;
jaoramos 1:5c05e0d08e61 32 float mCurrent = 0.0;
jaoramos 1:5c05e0d08e61 33 //int pulses0 = 0;
jaoramos 1:5c05e0d08e61 34 //int deltaPulses;
jaoramos 0:9f2b0ea63eac 35 float t0 = 0.0;
jaoramos 1:5c05e0d08e61 36 float t = 0.0, dt;
jaoramos 1:5c05e0d08e61 37
jaoramos 1:5c05e0d08e61 38 float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(QUADRATURE_TYPE));
jaoramos 1:5c05e0d08e61 39 float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(QUADRATURE_TYPE));
jaoramos 1:5c05e0d08e61 40
jaoramos 0:9f2b0ea63eac 41 float* buffer;
jaoramos 1:5c05e0d08e61 42 float lambda = 30;
jaoramos 0:9f2b0ea63eac 43 int index;
jaoramos 0:9f2b0ea63eac 44
jaoramos 0:9f2b0ea63eac 45 void saving(void const *args) {
jaoramos 0:9f2b0ea63eac 46 index = 0;
jaoramos 0:9f2b0ea63eac 47 while (true) {
jaoramos 1:5c05e0d08e61 48 buffer[index] = theta1;
jaoramos 1:5c05e0d08e61 49 buffer[index+1] = theta2;
jaoramos 1:5c05e0d08e61 50 buffer[index+2] = dtheta1;
jaoramos 1:5c05e0d08e61 51 buffer[index+3] = dtheta2;
jaoramos 1:5c05e0d08e61 52 buffer[index+4] = mCurrent;
jaoramos 1:5c05e0d08e61 53 buffer[index+5] = t;
jaoramos 1:5c05e0d08e61 54
jaoramos 1:5c05e0d08e61 55 index = index+DATA_COLS;
jaoramos 1:5c05e0d08e61 56 Thread::wait(20);
jaoramos 0:9f2b0ea63eac 57 }
jaoramos 0:9f2b0ea63eac 58 }
jaoramos 0:9f2b0ea63eac 59
jaoramos 0:9f2b0ea63eac 60 void computing(void const *args) {
jaoramos 1:5c05e0d08e61 61 float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0;
jaoramos 1:5c05e0d08e61 62
jaoramos 1:5c05e0d08e61 63 int pulsesPend, pulsesMot;
jaoramos 0:9f2b0ea63eac 64 while (true) {
jaoramos 0:9f2b0ea63eac 65 t = T.read();
jaoramos 1:5c05e0d08e61 66
jaoramos 1:5c05e0d08e61 67 mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP;
jaoramos 1:5c05e0d08e61 68 pulsesPend = -encoder.getPulses();
jaoramos 1:5c05e0d08e61 69 pulsesMot = motor.getPulses();
jaoramos 1:5c05e0d08e61 70
jaoramos 1:5c05e0d08e61 71 dt = t - t0; //time difference
jaoramos 1:5c05e0d08e61 72 theta2 = float(pulsesPend)*encoder_conv;
jaoramos 1:5c05e0d08e61 73 theta1 = float(pulsesMot)*motor_conv;
jaoramos 0:9f2b0ea63eac 74
jaoramos 1:5c05e0d08e61 75 //calculate dtheta1
jaoramos 1:5c05e0d08e61 76 dz1 = - lambda * z1 + lambda * theta1;
jaoramos 1:5c05e0d08e61 77 z1 = z1 + dz1 * dt;
jaoramos 1:5c05e0d08e61 78 dtheta1 = dz1;
jaoramos 0:9f2b0ea63eac 79
jaoramos 1:5c05e0d08e61 80 //calculate dtheta2
jaoramos 1:5c05e0d08e61 81 dz2 = - lambda * z2 + lambda * theta2;
jaoramos 1:5c05e0d08e61 82 z2 = z2 + dz2 * dt;
jaoramos 1:5c05e0d08e61 83 dtheta2 = dz2;
jaoramos 1:5c05e0d08e61 84
jaoramos 0:9f2b0ea63eac 85 t0 = t;
jaoramos 0:9f2b0ea63eac 86 Thread::wait(1);
jaoramos 0:9f2b0ea63eac 87 }
jaoramos 0:9f2b0ea63eac 88 }
jaoramos 0:9f2b0ea63eac 89
jaoramos 0:9f2b0ea63eac 90 void saveToFile ()
jaoramos 0:9f2b0ea63eac 91 {
jaoramos 0:9f2b0ea63eac 92 FILE *fp = fopen("/local/data.csv", "w");
jaoramos 0:9f2b0ea63eac 93 if (!fp) {
jaoramos 0:9f2b0ea63eac 94 fprintf(stderr, "File could not be openend \n\r");
jaoramos 0:9f2b0ea63eac 95 exit(1);
jaoramos 0:9f2b0ea63eac 96 }
jaoramos 0:9f2b0ea63eac 97
jaoramos 0:9f2b0ea63eac 98 wait(2.0);
jaoramos 0:9f2b0ea63eac 99
jaoramos 1:5c05e0d08e61 100 for (int i=0; i < index; i=i+DATA_COLS)
jaoramos 1:5c05e0d08e61 101 {
jaoramos 1:5c05e0d08e61 102 for (int j = 0; j< DATA_COLS; j++)
jaoramos 1:5c05e0d08e61 103 {
jaoramos 1:5c05e0d08e61 104 fprintf(fp,"%f,", buffer[i+j]);
jaoramos 1:5c05e0d08e61 105 }
jaoramos 1:5c05e0d08e61 106 fprintf(fp,"\n");
jaoramos 0:9f2b0ea63eac 107 }
jaoramos 0:9f2b0ea63eac 108 pc.printf("closing file\n\r");
jaoramos 0:9f2b0ea63eac 109 fclose(fp);
jaoramos 0:9f2b0ea63eac 110 wait(2.0);;
jaoramos 0:9f2b0ea63eac 111 }
jaoramos 0:9f2b0ea63eac 112
jaoramos 0:9f2b0ea63eac 113 int main() {
jaoramos 0:9f2b0ea63eac 114 //allocate memory for the buffer
jaoramos 1:5c05e0d08e61 115 pc.printf("creating buffer!\r\n");
jaoramos 1:5c05e0d08e61 116 buffer = new float[buffer_size];
jaoramos 1:5c05e0d08e61 117 pc.printf("done creating buffer!\r\n");
jaoramos 0:9f2b0ea63eac 118 T.start();
jaoramos 0:9f2b0ea63eac 119 Thread thrd2(computing,NULL,osPriorityRealtime);
jaoramos 0:9f2b0ea63eac 120 Thread thrd3(saving,NULL,osPriorityNormal);
jaoramos 0:9f2b0ea63eac 121
jaoramos 0:9f2b0ea63eac 122 pc.printf("Start!\r\n");
jaoramos 0:9f2b0ea63eac 123 pc.printf("Time: %f\r\n", t);
jaoramos 0:9f2b0ea63eac 124 while (t < 10.0)
jaoramos 0:9f2b0ea63eac 125 {
jaoramos 0:9f2b0ea63eac 126 pc.printf("Time: %f\r\n", t);
jaoramos 0:9f2b0ea63eac 127 Thread::wait(1000);
jaoramos 0:9f2b0ea63eac 128 }
jaoramos 0:9f2b0ea63eac 129 pc.printf("Done!\r\n");
jaoramos 0:9f2b0ea63eac 130 thrd2.terminate();
jaoramos 0:9f2b0ea63eac 131 thrd3.terminate();
jaoramos 0:9f2b0ea63eac 132 saveToFile();
jaoramos 0:9f2b0ea63eac 133 }