![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Lab 7 student version with WSE library
Fork of Lab6 by
Diff: main.cpp
- Revision:
- 1:f8c132f1ab85
- Parent:
- 0:5d2df7452db5
- Child:
- 2:6bdd14bda350
diff -r 5d2df7452db5 -r f8c132f1ab85 main.cpp --- a/main.cpp Fri Oct 10 19:20:07 2014 +0000 +++ b/main.cpp Wed Nov 05 17:42:26 2014 +0000 @@ -1,194 +1,8 @@ #include "mbed.h" +#include "mbedWSEsbc.h" #define PI (3.14159) -//============================================================================= -// Four commands for the Instruction Register (B7,B6) - LS7366 -//============================================================================= -#define CLR 0x00 //Clear Instruction -#define RD 0x01 //Read Instruction -#define WR 0x02 //Write Instruction -#define LOAD 0x03 //Load Instruction - -//============================================================================= -// Register to Select from the Instruction Register (B5,B4,B3) - LS7366 -//============================================================================= -#define NONE 0x00 //No Register Selected -#define MDR0 0x01 //Mode Register 0 -#define MDR1 0x02 //Mode Register 1 -#define DTR 0x03 //Data Transfer Register -#define CNTR 0x04 //Software Configurable Counter Register -#define OTR 0x05 //Output Transfer Register -#define STR 0x06 //Status Register -#define NONE_REG 0x07 //No Register Selected - - -Serial pc(USBTX,USBRX); -SPI spi(p5, p6, p7); -LocalFileSystem local ("local"); -DigitalOut ls7166_cs1(p19); //CS for LS7366 -DigitalOut ls7166_cs2(p20); //CS for LS7366 -DigitalOut mot1_ph1(p21); -PwmOut mot_en1(p23); -//----- LS7366 Encoder/Counter Routines -------------------- -void LS7366_cmd(int inst, int reg){ - char cmd; - - spi.format(8, 0); - spi.frequency(2000000); - cmd = (inst << 6) | (reg << 3); -// printf("\r\ncmd=0X%2X", cmd); - spi.write(cmd); -} - -long LS7366_read_counter(int chan_num){ - union bytes{ - char byte_enc[4]; - long long_enc; - }counter; - - spi.format(8, 0); - spi.frequency(2000000); - - if(chan_num!=2){ - ls7166_cs1 = 0; - wait_us(1); - LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR - ls7166_cs1 = 1; - wait_us(1); - ls7166_cs1 = 0; - } - else{ - ls7166_cs2 = 0; - wait_us(1); - LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR - ls7166_cs2 = 1; - wait_us(1); - - ls7166_cs2 = 0; - } - wait_us(1); - LS7366_cmd(RD,CNTR); //cmd = 0x60, READ from CNTR - counter.byte_enc[3] = spi.write(0x00); - counter.byte_enc[2] = spi.write(0x00); - counter.byte_enc[1] = spi.write(0x00); - counter.byte_enc[0] = spi.write(0x00); - - if(chan_num!=2){ - ls7166_cs1 = 1; - } - else{ - ls7166_cs2 = 1; - } - - return counter.long_enc; //return count -} -void LS7366_quad_mode_x4(int chan_num){ - - spi.format(8, 0); - spi.frequency(2000000); - - if(chan_num!=2){ - ls7166_cs1 = 0; - } - else{ - ls7166_cs2 = 0; - } - wait_us(1); - LS7366_cmd(WR,MDR0);// Write to the MDR0 register - spi.write(0x03); // X4 quadrature count mode - if(chan_num!=2){ - ls7166_cs1 = 1; - } - else{ - ls7166_cs2 = 1; - } -} - -void LS7366_reset_counter(int chan_num){ - - spi.format(8, 0); - spi.frequency(2000000); - - if(chan_num!=2){ - ls7166_cs1 = 0; - } - else{ - ls7166_cs2 = 0; - } - wait_us(1); - LS7366_cmd(CLR,CNTR);//Clear the counter register - if(chan_num!=2){ - ls7166_cs1 = 1; - } - else{ - ls7166_cs2 = 1; - } - wait_us(1); - - if(chan_num!=2){ - ls7166_cs1 = 0; - } - else{ - ls7166_cs2 = 0; - } - wait_us(1); - LS7366_cmd(LOAD,CNTR);// - if(chan_num!=2){ - ls7166_cs1 = 1; - } - else{ - ls7166_cs1 = 1; - } -} - -void LS7366_write_DTR(int chan_num,long enc_value) -{ - union bytes - { - char byte_enc[4]; - long long_enc; - }counter; - - spi.format(8, 0); - spi.frequency(2000000); - - counter.long_enc = enc_value; - - if(chan_num!=2){ - ls7166_cs1 = 0; - } - else{ - ls7166_cs2 = 0; - } - wait_us(1); - LS7366_cmd(WR,DTR);// - spi.write(counter.byte_enc[3]); - spi.write(counter.byte_enc[2]); - spi.write(counter.byte_enc[1]); - spi.write(counter.byte_enc[0]); - if(chan_num!=2){ - ls7166_cs1 = 1; - } - else{ - ls7166_cs2 = 1; - } - - wait_us(1); - if(chan_num!=2){ - ls7166_cs1 = 0; - } - else{ - ls7166_cs2 = 0; - } - wait_us(1); - LS7366_cmd(LOAD,CNTR);// - if(chan_num!=2){ - ls7166_cs1 = 1; - } - else{ - ls7166_cs2 = 1; - } -} +LocalFileSystem local ("local"); float Ts = 0.001; // Sampling period 1/Ts Hz // Arrays for data storage @@ -196,110 +10,101 @@ float ang_pos[1000]; float est_speed[1000]; float dc_in[1000]; -//float ang_posID[1100]; -Timer t; // Open "results.M" on the local file system for writing -FILE *fp = fopen("/local/results5.M", "w"); +FILE *fp = fopen("/local/gnMot1e.M", "w"); float cntr; float ang,angp,speed; float dt; float dc; long enc1; -int k,k0; //,L,L0; +float K,dspeed; +int k; int main () { - pc.baud(921600); //Set up serial port baud rate - spi.frequency(5000000); - LS7366_reset_counter(1); - LS7366_quad_mode_x4(1); - LS7366_write_DTR(1,0); - - LS7366_reset_counter(2); - LS7366_quad_mode_x4(2); - LS7366_write_DTR(2,0); - cntr = 0.0; // cntr used to keep track of sample period and elpased time + mbedWSEsbcInit(921600); + + // Reset cntr used to keep track of sample period and elpased time + cntr = 0.0; + + // Gain design parameters + K = 0.00887; + dspeed = 40; // rad/sec // initialize data vectors - for(k=0;k<1000;k++) - { etime[k] = 0.0; - ang_pos[k] = 0.0; - est_speed[k] = 0.0; - dc_in[k] = 0.0; - } - k = 0; // Reset index counter - k0 = 0; - angp = 0; // initialize previous angle variable - + for(k=0; k<1000; k++) { + etime[k] = 0.0; + ang_pos[k] = 0.0; + est_speed[k] = 0.0; + dc_in[k] = 0.0; + } + k = 0; // Reset index counter + angp = 0; // initialize previous angle variable to zero + while(cntr <= 1000) { t.start(); // start measuring comp time - - // Read encoder + + // Read encoder enc1 = LS7366_read_counter(1); - + // Convert from counts to radians - ang = -2*PI*enc1/6500.0; - + ang = -2*PI*enc1/6500.0; + // Estimate speed speed = (ang-angp)/Ts; - + // Age variables angp = ang; - - // Drive motor for ID - if (cntr*Ts <= 0.1) { - dc = 0.0; - } else if (cntr*Ts <= 0.55) { - dc = -0.075; - k0 = k; - } else if (cntr*Ts < 1.0) { - dc = -0.35; - }else { - dc = 0.0; - } + + // Drive motor + dc = -0.35; + if (cntr >= 1000) { + dc = 0.0; + } + // Log data - etime[k] = cntr*Ts; - ang_pos[k] = ang; - est_speed[k] = speed; - dc_in[k] = abs(dc); - k++; - //dc = -1; + etime[k] = cntr*Ts; + ang_pos[k] = ang; + est_speed[k] = speed; + dc_in[k] = dc; + + k++; + + // motor control mot1_ph1 = 0; + mot1_ph2 = 1; mot_en1 = abs(dc); - - - + t.stop(); // end measuring comp time dt = Ts-t.read(); //printf("%5.2f\n\r",cntr); - pc.printf("%5.2f %5.2f %5.2f \n\r",cntr*Ts,speed,k0); - + pc.printf("%5.2f %5.2f %5.2f \n\r",cntr*Ts,speed,dc); + //pc.printf("%5.2f %d \n\r",cntr*Ts,enc1); + t.reset(); cntr=cntr+1; - wait(dt); // wait to ensure sampling period set by Ts + wait(dt); // wait to ensure sampling period set by Ts //if (cntr == 200){fclose(fp);} - + }//while - + // Print out log variables in MATLAB structured variable format. pc.printf("Printing log variables to file on mBed ... "); + + dc_in[0] = K*dspeed; + if(1) { - for(k=101; k<k0; k++) { - fprintf(fp,"t_su(%d,1) = %.5f;\n",k+1-101,etime[k]-etime[101]); - fprintf(fp,"est_speed_su(%d,1) = %.5f;\n",k+1-101,est_speed[k]-est_speed[101]); - fprintf(fp,"dc_in_su(%d,1) = %.5f;\n",k+1-101,dc_in[k]); - } - for(k=k0; k<1000; k++) { - fprintf(fp,"t_id(%d,1) = %.5f;\n",k+1-k0,etime[k]-etime[k0]); - fprintf(fp,"est_speed_id(%d,1) = %.5f;\n",k+1-k0,est_speed[k]-est_speed[k0]); - fprintf(fp,"dc_in_id(%d,1) = %.5f;\n",k+1-k0,dc_in[k]-dc_in[k0-1]); - } + for(k=0; k<1000; k++) { + fprintf(fp,"t(%d,1) = %.5f;\n",k+1,etime[k]); + fprintf(fp,"est_speed(%d,1) = %.5f;\n",k+1,est_speed[k]); + fprintf(fp,"dc_in(%d,1) = %.5f;\n",k+1,dc_in[k]); + } } printf("done.\r\n"); - + // Close file fclose(fp);