USNA ES305
None
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:1de151a3fae9
--- /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