main.cpp

00001 #include "mbed.h"
00002 #define PI (3.14159)
00003 //=============================================================================
00004 // Four commands for the Instruction Register (B7,B6) - LS7366
00005 //=============================================================================
00006 #define CLR     0x00    //Clear Instruction
00007 #define RD      0x01    //Read Instruction
00008 #define WR      0x02    //Write Instruction
00009 #define LOAD    0x03    //Load Instruction
00010 
00011 //=============================================================================
00012 // Register to Select from the Instruction Register (B5,B4,B3) - LS7366
00013 //=============================================================================
00014 #define NONE        0x00    //No Register Selected
00015 #define MDR0        0x01    //Mode Register 0
00016 #define MDR1        0x02    //Mode Register 1
00017 #define DTR         0x03    //Data Transfer Register
00018 #define CNTR        0x04    //Software Configurable Counter Register
00019 #define OTR         0x05    //Output Transfer Register
00020 #define STR         0x06    //Status Register
00021 #define NONE_REG    0x07    //No Register Selected
00022 
00023 
00024 Serial pc(USBTX,USBRX);
00025 SPI spi(p5, p6, p7);
00026 LocalFileSystem local ("local");
00027 DigitalOut ls7166_cs1(p19);    //CS for LS7366
00028 DigitalOut ls7166_cs2(p20);    //CS for LS7366
00029 DigitalOut mot1_ph1(p21);
00030 PwmOut mot_en1(p23);
00031 //----- LS7366 Encoder/Counter Routines --------------------
00032 void LS7366_cmd(int inst,  int reg){
00033     char cmd;
00034     
00035     spi.format(8, 0);
00036     spi.frequency(2000000);
00037     cmd = (inst << 6) | (reg << 3);
00038 //    printf("\r\ncmd=0X%2X", cmd);
00039     spi.write(cmd);
00040 }
00041 
00042 long LS7366_read_counter(int chan_num){
00043     union bytes{
00044         char byte_enc[4];
00045         long long_enc;
00046     }counter;
00047     
00048     spi.format(8, 0);
00049     spi.frequency(2000000);
00050     
00051     if(chan_num!=2){
00052         ls7166_cs1 = 0;
00053         wait_us(1);
00054         LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR
00055         ls7166_cs1 = 1;
00056         wait_us(1);
00057         ls7166_cs1 = 0;
00058     }
00059     else{
00060         ls7166_cs2 = 0;    
00061         wait_us(1);
00062         LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR
00063         ls7166_cs2 = 1;
00064         wait_us(1);
00065         
00066         ls7166_cs2 = 0;        
00067     }
00068     wait_us(1);
00069     LS7366_cmd(RD,CNTR);  //cmd = 0x60, READ from CNTR
00070     counter.byte_enc[3] = spi.write(0x00);
00071     counter.byte_enc[2] = spi.write(0x00);
00072     counter.byte_enc[1] = spi.write(0x00);
00073     counter.byte_enc[0] = spi.write(0x00);
00074     
00075     if(chan_num!=2){
00076         ls7166_cs1 = 1;    
00077     }            
00078     else{
00079         ls7166_cs2 = 1;    
00080     }        
00081     
00082     return counter.long_enc;  //return count
00083 }
00084 
00085 void LS7366_quad_mode_x4(int chan_num){
00086     
00087     spi.format(8, 0);
00088     spi.frequency(2000000);
00089     
00090     if(chan_num!=2){
00091         ls7166_cs1 = 0;    
00092     }            
00093     else{
00094         ls7166_cs2 = 0;    
00095     }    
00096     wait_us(1);
00097     LS7366_cmd(WR,MDR0);// Write to the MDR0 register
00098     spi.write(0x03); // X4 quadrature count mode
00099     if(chan_num!=2){
00100         ls7166_cs1 = 1;    
00101     }            
00102     else{
00103         ls7166_cs2 = 1;    
00104     }
00105 }
00106 
00107 void LS7366_reset_counter(int chan_num){
00108     
00109     spi.format(8, 0);
00110     spi.frequency(2000000);
00111     
00112     if(chan_num!=2){
00113         ls7166_cs1 = 0;    
00114     }
00115     else{
00116         ls7166_cs2 = 0;    
00117     }    
00118     wait_us(1);
00119     LS7366_cmd(CLR,CNTR);//Clear the counter register
00120     if(chan_num!=2){
00121         ls7166_cs1 = 1;    
00122     }            
00123     else{
00124         ls7166_cs2 = 1;    
00125     }
00126     wait_us(1);
00127     
00128     if(chan_num!=2){
00129         ls7166_cs1 = 0;    
00130     }            
00131     else{
00132         ls7166_cs2 = 0;    
00133     }        
00134     wait_us(1);
00135     LS7366_cmd(LOAD,CNTR);//
00136     if(chan_num!=2){
00137         ls7166_cs1 = 1;    
00138     }            
00139     else{
00140         ls7166_cs1 = 1;    
00141     }
00142 }
00143 
00144 void LS7366_write_DTR(int chan_num,long enc_value)
00145 {
00146     union bytes
00147     {
00148         char byte_enc[4];
00149         long long_enc;
00150     }counter;
00151     
00152     spi.format(8, 0);
00153     spi.frequency(2000000);
00154     
00155     counter.long_enc = enc_value;
00156     
00157     if(chan_num!=2){
00158         ls7166_cs1 = 0;    
00159     }            
00160     else{
00161         ls7166_cs2 = 0;    
00162     }   
00163     wait_us(1);
00164     LS7366_cmd(WR,DTR);//
00165     spi.write(counter.byte_enc[3]);
00166     spi.write(counter.byte_enc[2]);
00167     spi.write(counter.byte_enc[1]);
00168     spi.write(counter.byte_enc[0]);
00169     if(chan_num!=2){
00170         ls7166_cs1 = 1;    
00171     }            
00172     else{
00173         ls7166_cs2 = 1;    
00174     }     
00175     
00176     wait_us(1);
00177     if(chan_num!=2){
00178         ls7166_cs1 = 0;    
00179     }            
00180     else{
00181         ls7166_cs2 = 0;    
00182     }
00183     wait_us(1);
00184     LS7366_cmd(LOAD,CNTR);//
00185     if(chan_num!=2){
00186         ls7166_cs1 = 1;    
00187     }            
00188     else{
00189         ls7166_cs2 = 1;    
00190     }
00191 }   
00192 float Ts = 0.001; // Sampling period 1/Ts Hz 
00193  
00194 // Arrays for data storage
00195 float etime[1000];
00196 float ang_pos[1000];
00197 float est_speed[1000];
00198 float dc_in[1000];
00199 //float ang_posID[1100];
00200 Timer t;
00201  
00202  // Open "results.M" on the local file system for writing
00203 FILE *fp = fopen("/local/resNL5.M", "w");
00204  
00205 float cntr;
00206 float ang,angp,speed;
00207 float dt;
00208 float dc;
00209 long enc1;
00210 int k,k0; //,L,L0;
00211  
00212 int main ()
00213 {    
00214     pc.baud(921600); //Set up serial port baud rate
00215     spi.frequency(5000000);
00216     LS7366_reset_counter(1);
00217     LS7366_quad_mode_x4(1);       
00218     LS7366_write_DTR(1,0);
00219 
00220     LS7366_reset_counter(2);
00221     LS7366_quad_mode_x4(2);       
00222     LS7366_write_DTR(2,0);
00223     cntr = 0.0; // cntr used to keep track of sample period and elpased time
00224     
00225     // initialize data vectors
00226     for(k=0;k<1000;k++)
00227   { etime[k] = 0.0;
00228     ang_pos[k] = 0.0;  
00229     est_speed[k] = 0.0; 
00230     dc_in[k] = 0.0; 
00231   }
00232   k = 0; // Reset index counter
00233   k0 = 0;
00234   angp = 0; // initialize previous angle variable
00235     
00236     while(cntr <= 1000) {
00237         t.start(); // start measuring comp time
00238         
00239         // Read encoder        
00240         enc1 = LS7366_read_counter(1);
00241         
00242         // Convert from counts to radians
00243         ang = -2*PI*enc1/6500.0;    
00244         
00245         // Estimate speed
00246         speed = (ang-angp)/Ts;
00247         
00248         // Age variables
00249         angp = ang;
00250         
00251         // Drive motor for ID
00252         if (cntr*Ts < 1.0) {            
00253             dc = -0.35;                        
00254         }else {
00255             dc = 0.0;
00256         } 
00257         
00258         // Log data
00259             etime[k] = cntr*Ts;
00260             ang_pos[k] = ang;
00261             est_speed[k] = speed;
00262             dc_in[k] = abs(dc);
00263             k++;
00264         //dc = -1;
00265         mot1_ph1 = 0;
00266         mot_en1 = abs(dc);
00267                     
00268         
00269               
00270         t.stop(); // end measuring comp time
00271         dt = Ts-t.read();
00272         //printf("%5.2f\n\r",cntr);
00273         pc.printf("%5.2f %5.2f \n\r",cntr*Ts,speed);
00274         
00275         t.reset();
00276         cntr=cntr+1;
00277         wait(dt); // wait to ensure sampling period set by Ts 
00278         //if (cntr == 200){fclose(fp);}
00279           
00280     }//while
00281     
00282     // Print out log variables in MATLAB structured variable format.
00283     pc.printf("Printing log variables to file on mBed           ... ");
00284     if(1) {
00285         for(k=0; k<1000; k++) {
00286             fprintf(fp,"t_id(%d,1) = %.5f;\n",k+1,etime[k]);
00287             fprintf(fp,"est_speed_id(%d,1) = %.5f;\n",k+1,est_speed[k]);
00288             fprintf(fp,"dc_in_id(%d,1) = %.5f;\n",k+1,dc_in[k]);
00289         }
00290     }
00291     printf("done.\r\n");
00292  
00293     // Close file
00294     fclose(fp);
00295     
00296 }//main