File content as of revision 1:49d9a5382ca9:

#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/resNL5.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 < 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 \n\r",cntr*Ts,speed);
        
        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=0; k<1000; k++) {
            fprintf(fp,"t_id(%d,1) = %.5f;\n",k+1,etime[k]);
            fprintf(fp,"est_speed_id(%d,1) = %.5f;\n",k+1,est_speed[k]);
            fprintf(fp,"dc_in_id(%d,1) = %.5f;\n",k+1,dc_in[k]);
        }
    }
    printf("done.\r\n");
 
    // Close file
    fclose(fp);
    
}//main