Rich O'Brien / Mbed 2 deprecated Lab7_wse_student

Lab 7 student version with WSE library

Dependencies:   mbed mbedWSEsbc

Fork of Lab6 by USNA ES305

main.cpp@1:f8c132f1ab85, 2014-11-05 (annotated)

Committer:
riobrien
Date:
Wed Nov 05 17:42:26 2014 +0000
Revision:
1:f8c132f1ab85
Parent:
0:5d2df7452db5
Child:
2:6bdd14bda350
None

Who changed what in which revision?

UserRevisionLine numberNew contents of line
riobrien 0:5d2df7452db5 1 #include "mbed.h"
riobrien 1:f8c132f1ab85 2 #include "mbedWSEsbc.h"
riobrien 0:5d2df7452db5 3 #define PI (3.14159)
riobrien 0:5d2df7452db5 4
riobrien 1:f8c132f1ab85 5 LocalFileSystem local ("local");
riobrien 0:5d2df7452db5 6 float Ts = 0.001; // Sampling period 1/Ts Hz
riobrien 0:5d2df7452db5 7
riobrien 0:5d2df7452db5 8 // Arrays for data storage
riobrien 0:5d2df7452db5 9 float etime[1000];
riobrien 0:5d2df7452db5 10 float ang_pos[1000];
riobrien 0:5d2df7452db5 11 float est_speed[1000];
riobrien 0:5d2df7452db5 12 float dc_in[1000];
riobrien 0:5d2df7452db5 13
riobrien 0:5d2df7452db5 14 // Open "results.M" on the local file system for writing
riobrien 1:f8c132f1ab85 15 FILE *fp = fopen("/local/gnMot1e.M", "w");
riobrien 0:5d2df7452db5 16
riobrien 0:5d2df7452db5 17 float cntr;
riobrien 0:5d2df7452db5 18 float ang,angp,speed;
riobrien 0:5d2df7452db5 19 float dt;
riobrien 0:5d2df7452db5 20 float dc;
riobrien 0:5d2df7452db5 21 long enc1;
riobrien 1:f8c132f1ab85 22 float K,dspeed;
riobrien 1:f8c132f1ab85 23 int k;
riobrien 0:5d2df7452db5 24
riobrien 0:5d2df7452db5 25 int main ()
riobrien 0:5d2df7452db5 26 {
riobrien 1:f8c132f1ab85 27 mbedWSEsbcInit(921600);
riobrien 1:f8c132f1ab85 28
riobrien 1:f8c132f1ab85 29 // Reset cntr used to keep track of sample period and elpased time
riobrien 1:f8c132f1ab85 30 cntr = 0.0;
riobrien 1:f8c132f1ab85 31
riobrien 1:f8c132f1ab85 32 // Gain design parameters
riobrien 1:f8c132f1ab85 33 K = 0.00887;
riobrien 1:f8c132f1ab85 34 dspeed = 40; // rad/sec
riobrien 0:5d2df7452db5 35
riobrien 0:5d2df7452db5 36 // initialize data vectors
riobrien 1:f8c132f1ab85 37 for(k=0; k<1000; k++) {
riobrien 1:f8c132f1ab85 38 etime[k] = 0.0;
riobrien 1:f8c132f1ab85 39 ang_pos[k] = 0.0;
riobrien 1:f8c132f1ab85 40 est_speed[k] = 0.0;
riobrien 1:f8c132f1ab85 41 dc_in[k] = 0.0;
riobrien 1:f8c132f1ab85 42 }
riobrien 1:f8c132f1ab85 43 k = 0; // Reset index counter
riobrien 1:f8c132f1ab85 44 angp = 0; // initialize previous angle variable to zero
riobrien 1:f8c132f1ab85 45
riobrien 0:5d2df7452db5 46 while(cntr <= 1000) {
riobrien 0:5d2df7452db5 47 t.start(); // start measuring comp time
riobrien 1:f8c132f1ab85 48
riobrien 1:f8c132f1ab85 49 // Read encoder
riobrien 0:5d2df7452db5 50 enc1 = LS7366_read_counter(1);
riobrien 1:f8c132f1ab85 51
riobrien 0:5d2df7452db5 52 // Convert from counts to radians
riobrien 1:f8c132f1ab85 53 ang = -2*PI*enc1/6500.0;
riobrien 1:f8c132f1ab85 54
riobrien 0:5d2df7452db5 55 // Estimate speed
riobrien 0:5d2df7452db5 56 speed = (ang-angp)/Ts;
riobrien 1:f8c132f1ab85 57
riobrien 0:5d2df7452db5 58 // Age variables
riobrien 0:5d2df7452db5 59 angp = ang;
riobrien 1:f8c132f1ab85 60
riobrien 1:f8c132f1ab85 61 // Drive motor
riobrien 1:f8c132f1ab85 62 dc = -0.35;
riobrien 0:5d2df7452db5 63
riobrien 1:f8c132f1ab85 64 if (cntr >= 1000) {
riobrien 1:f8c132f1ab85 65 dc = 0.0;
riobrien 1:f8c132f1ab85 66 }
riobrien 1:f8c132f1ab85 67
riobrien 0:5d2df7452db5 68 // Log data
riobrien 1:f8c132f1ab85 69 etime[k] = cntr*Ts;
riobrien 1:f8c132f1ab85 70 ang_pos[k] = ang;
riobrien 1:f8c132f1ab85 71 est_speed[k] = speed;
riobrien 1:f8c132f1ab85 72 dc_in[k] = dc;
riobrien 1:f8c132f1ab85 73
riobrien 1:f8c132f1ab85 74 k++;
riobrien 1:f8c132f1ab85 75
riobrien 1:f8c132f1ab85 76 // motor control
riobrien 0:5d2df7452db5 77 mot1_ph1 = 0;
riobrien 1:f8c132f1ab85 78 mot1_ph2 = 1;
riobrien 0:5d2df7452db5 79 mot_en1 = abs(dc);
riobrien 1:f8c132f1ab85 80
riobrien 0:5d2df7452db5 81 t.stop(); // end measuring comp time
riobrien 0:5d2df7452db5 82 dt = Ts-t.read();
riobrien 0:5d2df7452db5 83 //printf("%5.2f\n\r",cntr);
riobrien 1:f8c132f1ab85 84 pc.printf("%5.2f %5.2f %5.2f \n\r",cntr*Ts,speed,dc);
riobrien 1:f8c132f1ab85 85 //pc.printf("%5.2f %d \n\r",cntr*Ts,enc1);
riobrien 1:f8c132f1ab85 86
riobrien 0:5d2df7452db5 87 t.reset();
riobrien 0:5d2df7452db5 88 cntr=cntr+1;
riobrien 1:f8c132f1ab85 89 wait(dt); // wait to ensure sampling period set by Ts
riobrien 0:5d2df7452db5 90 //if (cntr == 200){fclose(fp);}
riobrien 1:f8c132f1ab85 91
riobrien 0:5d2df7452db5 92 }//while
riobrien 1:f8c132f1ab85 93
riobrien 0:5d2df7452db5 94 // Print out log variables in MATLAB structured variable format.
riobrien 0:5d2df7452db5 95 pc.printf("Printing log variables to file on mBed ... ");
riobrien 1:f8c132f1ab85 96
riobrien 1:f8c132f1ab85 97 dc_in[0] = K*dspeed;
riobrien 1:f8c132f1ab85 98
riobrien 0:5d2df7452db5 99 if(1) {
riobrien 1:f8c132f1ab85 100 for(k=0; k<1000; k++) {
riobrien 1:f8c132f1ab85 101 fprintf(fp,"t(%d,1) = %.5f;\n",k+1,etime[k]);
riobrien 1:f8c132f1ab85 102 fprintf(fp,"est_speed(%d,1) = %.5f;\n",k+1,est_speed[k]);
riobrien 1:f8c132f1ab85 103 fprintf(fp,"dc_in(%d,1) = %.5f;\n",k+1,dc_in[k]);
riobrien 1:f8c132f1ab85 104 }
riobrien 0:5d2df7452db5 105 }
riobrien 0:5d2df7452db5 106 printf("done.\r\n");
riobrien 1:f8c132f1ab85 107
riobrien 0:5d2df7452db5 108 // Close file
riobrien 0:5d2df7452db5 109 fclose(fp);
riobrien 0:5d2df7452db5 110
riobrien 0:5d2df7452db5 111 }//main