![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Lab 7 student version with WSE library
Fork of Lab6 by
main.cpp
- Committer:
- riobrien
- Date:
- 2014-10-10
- Revision:
- 0:5d2df7452db5
- Child:
- 1:f8c132f1ab85
File content as of revision 0:5d2df7452db5:
#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