Diff: main.cpp

Revision:

0:5d2df7452db5

Child:

1:49d9a5382ca9

--- /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