![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Lab 7 student version with WSE library
Fork of Lab6 by
Diff: main.cpp
- Revision:
- 0:5d2df7452db5
- Child:
- 1:f8c132f1ab85
diff -r 000000000000 -r 5d2df7452db5 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 10 19:20:07 2014 +0000 @@ -0,0 +1,306 @@ +#include "mbed.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; + } +} +float Ts = 0.001; // Sampling period 1/Ts Hz + +// Arrays for data storage +float etime[1000]; +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"); + +float cntr; +float ang,angp,speed; +float dt; +float dc; +long enc1; +int k,k0; //,L,L0; + +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 + + // 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 + + while(cntr <= 1000) { + t.start(); // start measuring comp time + + // Read encoder + enc1 = LS7366_read_counter(1); + + // Convert from counts to radians + 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; + } + + // Log data + etime[k] = cntr*Ts; + ang_pos[k] = ang; + est_speed[k] = speed; + dc_in[k] = abs(dc); + k++; + //dc = -1; + mot1_ph1 = 0; + 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); + + t.reset(); + cntr=cntr+1; + 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 ... "); + 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]); + } + } + printf("done.\r\n"); + + // Close file + fclose(fp); + +}//main