Diff: main.cpp

Revision:

1:f8c132f1ab85

Parent:

0:5d2df7452db5

Child:

2:6bdd14bda350

--- a/main.cpp	Fri Oct 10 19:20:07 2014 +0000
+++ b/main.cpp	Wed Nov 05 17:42:26 2014 +0000
@@ -1,194 +1,8 @@
 #include "mbed.h"
+#include "mbedWSEsbc.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;    
-    }
-}   
+LocalFileSystem local ("local");
 float Ts = 0.001; // Sampling period 1/Ts Hz 
  
 // Arrays for data storage
@@ -196,110 +10,101 @@
 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");
+FILE *fp = fopen("/local/gnMot1e.M", "w");
  
 float cntr;
 float ang,angp,speed;
 float dt;
 float dc;
 long enc1;
-int k,k0; //,L,L0;
+float K,dspeed;
+int k; 
  
 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
+    mbedWSEsbcInit(921600);
+    
+    // Reset cntr used to keep track of sample period and elpased time
+    cntr = 0.0; 
+    
+    // Gain design parameters
+    K = 0.00887;
+    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
-    
+    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
+    angp = 0; // initialize previous angle variable to zero
+ 
     while(cntr <= 1000) {
         t.start(); // start measuring comp time
-        
-        // Read encoder        
+
+        // Read encoder
         enc1 = LS7366_read_counter(1);
-        
+
         // Convert from counts to radians
-        ang = -2*PI*enc1/6500.0;    
-        
+        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;
-        } 
+
+        // Drive motor 
+        dc = -0.35;
         
+        if (cntr >= 1000) {
+            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;
+        etime[k] = cntr*Ts;
+        ang_pos[k] = ang;
+        est_speed[k] = speed;
+        dc_in[k] = dc;
+
+        k++;
+        
+        // motor control
         mot1_ph1 = 0;
+        mot1_ph2 = 1;
         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);
-        
+        pc.printf("%5.2f %5.2f %5.2f \n\r",cntr*Ts,speed,dc);
+        //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 
+        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] = K*dspeed;
+
     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]);
-        }       
+        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);