Revision 0:1de151a3fae9, committed 2014-11-20

Comitter:

riobrien

Date:

Thu Nov 20 14:48:44 2014 +0000

Commit message:

None

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 20 14:48:44 2014 +0000
@@ -0,0 +1,321 @@
+#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/Lab9a.M", "w");
+ 
+float cntr;
+float ang,angp,speed;
+float dt;
+float dc,dc_prev,err,err_prev;
+float Kpi,zpi;
+long enc1;
+float K,dspeed;
+int k,k0; 
+ 
+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
+    K = 0.00887;
+    Kpi = 0.011392;
+    zpi = 37.4;
+    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
+  dc_prev = 0;
+  err_prev = 0;  
+  
+  
+    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 using PI ctrl
+        err = dspeed-speed;        
+        dc = dc_prev + Kpi*(1+zpi*Ts/2)*err - Kpi*(1-zpi*Ts/2)*err_prev;
+        
+        // Age variables
+        dc_prev = dc;
+        err_prev = err;
+        
+        // Apply saturation
+        if (dc > 1.0){dc = 1.0;}
+        else {
+            if (dc < -1.0){dc = -1.0;}
+            }
+
+        // Stop motor at t = 1 sec
+        if (cntr >= 1000) {
+            dc = 0.0;}
+            
+                    
+        // Log data
+            etime[k] = cntr*Ts;
+            ang_pos[k] = ang;
+            est_speed[k] = speed;
+            dc_in[k] = 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  %5.2f \n\r",cntr*Ts,speed,dc,err);
+        //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 
+        //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] = Kpi*(1+zpi*Ts/2)*dspeed;
+    
+    if(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);
+    
+}//main

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Nov 20 14:48:44 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/552587b429a1
\ No newline at end of file