Revision 25:f5741040c4bb, committed 2017-04-09

Comitter:

benkatz

Date:

Sun Apr 09 03:05:52 2017 +0000

Parent:

24:58c2d7571207

Child:

26:2b865c00d7e9

Commit message:

Set current bandwidth from serial interface

Changed in this revision

--- a/Calibration/calibration.cpp	Fri Apr 07 16:23:39 2017 +0000
+++ b/Calibration/calibration.cpp	Sun Apr 09 03:05:52 2017 +0000
@@ -13,17 +13,17 @@
     printf("\n\r Checking phase ordering\n\r");
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .2;                                                         //Put all volts on the D-Axis
+    float v_d = .2;                                                             //Put all volts on the D-Axis
     float v_q = 0.0;
     float v_u, v_v, v_w = 0;
     float dtc_u, dtc_v, dtc_w = .5;
     int sample_counter = 0;
     
     ///Set voltage angle to zero, wait for rotor position to settle
-    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                             //inverse dq0 transform on voltages
-    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                        //space vector modulation
+    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                                 //inverse dq0 transform on voltages
+    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                            //space vector modulation
     for(int i = 0; i<20000; i++){
-        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                                    // Set duty cycles
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                                        // Set duty cycles
         TIM1->CCR2 = 0x708*(1.0f-dtc_v);
         TIM1->CCR1 = 0x708*(1.0f-dtc_w);
         wait_us(100);
@@ -31,7 +31,7 @@
     //ps->ZeroPosition();
     ps->Sample(); 
     wait_us(1000);
-    float theta_start = ps->GetMechPosition();                              //get initial rotor position
+    float theta_start = ps->GetMechPosition();                                  //get initial rotor position
     controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //Calculate phase currents from ADC readings
     controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
     controller->i_a = -controller->i_b - controller->i_c;
@@ -40,14 +40,14 @@
     printf("\n\rCurrent\n\r");
     printf("%f    %f   %f\n\r\n\r", controller->i_d, controller->i_q, current);
     /// Rotate voltage angle
-    while(theta_ref < 4*PI){                                                //rotate for 2 electrical cycles
-        abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                         //inverse dq0 transform on voltages
-        svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                    //space vector modulation
+    while(theta_ref < 4*PI){                                                    //rotate for 2 electrical cycles
+        abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                             //inverse dq0 transform on voltages
+        svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                        //space vector modulation
         wait_us(100);
-        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                                    //Set duty cycles
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                                        //Set duty cycles
         TIM1->CCR2 = 0x708*(1.0f-dtc_v);
         TIM1->CCR1 = 0x708*(1.0f-dtc_w);
-       ps->Sample();                                                        //sample position sensor
+       ps->Sample();                                                            //sample position sensor
        theta_actual = ps->GetMechPosition();
        if(sample_counter > 200){
            sample_counter = 0 ;
@@ -71,26 +71,26 @@
     /// and (in the future) corrects nonlinearity due to position sensor eccentricity
     printf("Starting calibration procedure\n\r");
     
-    const int n = 128*NPP;                                                  // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
-    const int n2 = 10;                                                      // increments between saved samples (for smoothing motion)
-    float delta = 2*PI*NPP/(n*n2);                                          // change in angle between samples
-    float error_f[n] = {0};                                                 // error vector rotating forwards
-    float error_b[n] = {0};                                                 // error vector rotating backwards
+    const int n = 128*NPP;                                                      // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
+    const int n2 = 50;                                                          // increments between saved samples (for smoothing motion)
+    float delta = 2*PI*NPP/(n*n2);                                              // change in angle between samples
+    float error_f[n] = {0};                                                     // error vector rotating forwards
+    float error_b[n] = {0};                                                     // error vector rotating backwards
     int raw_f[n] = {0};
     int raw_b[n] = {0};
     float theta_ref = 0;
     float theta_actual = 0;
-    float v_d = .2;                                                         // Put volts on the D-Axis
+    float v_d = .2;                                                             // Put volts on the D-Axis
     float v_q = 0.0;
     float v_u, v_v, v_w = 0;
     float dtc_u, dtc_v, dtc_w = .5;
     
         
     ///Set voltage angle to zero, wait for rotor position to settle
-    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                             // inverse dq0 transform on voltages
-    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                        // space vector modulation
+    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                                 // inverse dq0 transform on voltages
+    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                            // space vector modulation
     for(int i = 0; i<40000; i++){
-        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                                    // Set duty cycles
+        TIM1->CCR3 = 0x708*(1.0f-dtc_u);                                        // Set duty cycles
         if(PHASE_ORDER){                                   
             TIM1->CCR2 = 0x708*(1.0f-dtc_v);
             TIM1->CCR1 = 0x708*(1.0f-dtc_w);
@@ -107,14 +107,14 @@
     controller->i_a = -controller->i_b - controller->i_c;
     dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
     float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));
-    for(int i = 0; i<n; i++){                                               // rotate forwards
+    for(int i = 0; i<n; i++){                                                   // rotate forwards
        for(int j = 0; j<n2; j++){   
         theta_ref += delta;
-       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                          // inverse dq0 transform on voltages
-       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                     // space vector modulation
+       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                              // inverse dq0 transform on voltages
+       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                         // space vector modulation
         TIM1->CCR3 = 0x708*(1.0f-dtc_u);
-        if(PHASE_ORDER){
-            TIM1->CCR2 = 0x708*(1.0f-dtc_v);
+        if(PHASE_ORDER){                                                        // Check phase ordering
+            TIM1->CCR2 = 0x708*(1.0f-dtc_v);                                    // Set duty cycles
             TIM1->CCR1 = 0x708*(1.0f-dtc_w);
             }
         else{
@@ -132,11 +132,11 @@
        //theta_ref += delta;
         }
     printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
-    for(int i = 0; i<n; i++){                                               // rotate backwards
+    for(int i = 0; i<n; i++){                                                   // rotate backwards
        for(int j = 0; j<n2; j++){
        theta_ref -= delta;
-       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                          // inverse dq0 transform on voltages
-       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                     // space vector modulation
+       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                              // inverse dq0 transform on voltages
+       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                         // space vector modulation
         TIM1->CCR3 = 0x708*(1.0f-dtc_u);
         if(PHASE_ORDER){
             TIM1->CCR2 = 0x708*(1.0f-dtc_v);
@@ -149,8 +149,8 @@
             wait_us(100);
             ps->Sample();
         }
-       ps->Sample();                                                        // sample position sensor
-       theta_actual = ps->GetMechPosition();                                // get mechanical position
+       ps->Sample();                                                            // sample position sensor
+       theta_actual = ps->GetMechPosition();                                    // get mechanical position
        error_b[i] = theta_ref/NPP - theta_actual;
        raw_b[i] = ps->GetRawPosition();
        printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
@@ -159,33 +159,33 @@
         
         float offset = 0;                                  
         for(int i = 0; i<n; i++){
-            offset += (error_f[i] + error_b[n-1-i])/(2.0f*n);               // calclate average position sensor offset
+            offset += (error_f[i] + error_b[n-1-i])/(2.0f*n);                   // calclate average position sensor offset
             }
-        offset = fmod(offset*NPP, 2*PI);                                    // convert mechanical angle to electrical angle
+        offset = fmod(offset*NPP, 2*PI);                                        // convert mechanical angle to electrical angle
         
             
-        ps->SetElecOffset(offset);                                          // Set position sensor offset
+        ps->SetElecOffset(offset);                                              // Set position sensor offset
         __float_reg[0] = offset;
         E_OFFSET = offset;
         
         /// Perform filtering to linearize position sensor eccentricity
         /// FIR n-sample average, where n = number of samples in one electrical cycle
         /// This filter has zero gain at electrical frequency and all integer multiples
-        /// So cogging should be completely filtered out.
+        /// So cogging effects should be completely filtered out.
         
         float error[n] = {0};
         const int window = 128;
         float error_filt[n] = {0};
         float cogging_current[window] = {0};
         float mean = 0;
-        for (int i = 0; i<n; i++){                                          //Average the forward and back directions
+        for (int i = 0; i<n; i++){                                              //Average the forward and back directions
             error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
             }
         for (int i = 0; i<n; i++){
             for(int j = 0; j<window; j++){
-                int ind = -window/2 + j + i;                                // Indexes from -window/2 to + window/2
+                int ind = -window/2 + j + i;                                    // Indexes from -window/2 to + window/2
                 if(ind<0){
-                    ind += n;}                                              // Moving average wraps around
+                    ind += n;}                                                  // Moving average wraps around
                 else if(ind > n-1) {
                     ind -= n;}
                 error_filt[i] += error[ind]/(float)window;
@@ -196,12 +196,12 @@
             //printf("%.4f   %4f    %.4f   %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
             mean += error_filt[i]/n;
             }
-        int raw_offset = (raw_f[0] + raw_b[n-1])/2;                         //Insensitive to errors in this direction, so 2 points is plenty
+        int raw_offset = (raw_f[0] + raw_b[n-1])/2;                             //Insensitive to errors in this direction, so 2 points is plenty
         const int  n_lut = 128;
         int lut[n_lut];
         printf("\n\r Encoder non-linearity compensation table\n\r");
         printf(" Sample Number : Lookup Index : Lookup Value : Cogging Current Lookup\n\r\n\r");
-        for (int i = 0; i<n_lut; i++){                                      // build lookup table
+        for (int i = 0; i<n_lut; i++){                                          // build lookup table
             int ind = (raw_offset>>7) + i;
             if(ind > (n_lut-1)){ 
                 ind -= n_lut;
@@ -210,14 +210,14 @@
             printf("%d   %d   %d   %f\n\r", i, ind, lut[ind],  cogging_current[i]);
             }
             
-        ps->WriteLUT(lut);                                            // write lookup table to position sensor object
+        ps->WriteLUT(lut);                                                      // write lookup table to position sensor object
         //memcpy(controller->cogging, cogging_current, sizeof(controller->cogging));  //compensation doesn't actually work yet....
-        memcpy(&ENCODER_LUT, lut, sizeof(lut));
+        memcpy(&ENCODER_LUT, lut, sizeof(lut));                                 // copy the lookup table to the flash array
         printf("\n\rEncoder Electrical Offset (rad) %f\n\r",  offset);
         
         if (!prefs->ready()) prefs->open();
-        prefs->flush();
-        //prefs->close();
+        prefs->flush();                                                         // write offset and lookup table to flash
+        prefs->close();
 
 
     }
\ No newline at end of file

--- a/Config/current_controller_config.h	Fri Apr 07 16:23:39 2017 +0000
+++ b/Config/current_controller_config.h	Sun Apr 09 03:05:52 2017 +0000
@@ -3,8 +3,9 @@
 
 #define K_D .05f                     // Volts/Amp
 #define K_Q .05f                     // Volts/Amp
-#define KI_D 0.04f                  // 1/samples
-#define KI_Q 0.04f                  // 1/samples
+#define K_SCALE 0.0001f            // K_loop/Loop BW (Hz)
+#define KI_D 0.06f                  // 1/samples
+#define KI_Q 0.06f                  // 1/samples
 #define V_BUS 24.0f                 // Volts
 
 #define D_INT_LIM V_BUS/(K_D*KI_D)  // Amps*samples

--- a/Config/motor_config.h	Fri Apr 07 16:23:39 2017 +0000
+++ b/Config/motor_config.h	Sun Apr 09 03:05:52 2017 +0000
@@ -1,7 +1,7 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
-#define R_PHASE 0.105f            //Ohms
+#define R_PHASE 0.105f          //Ohms
 #define L_D 0.00003f            //Henries
 #define L_Q 0.00003f            //Henries
 #define KT .07f                 //N-m per peak phase amp (= RMS amps/1.5)

--- a/Config/user_config.h	Fri Apr 07 16:23:39 2017 +0000
+++ b/Config/user_config.h	Sun Apr 09 03:05:52 2017 +0000
@@ -4,14 +4,14 @@
 #define USER_CONFIG_H
 
 
-#define E_OFFSET                __float_reg[0]
-#define M_OFFSET                __float_reg[1]
-#define I_BW                    __float_reg[2]
-#define TORQUE_LIMIT            __float_reg[3]
+#define E_OFFSET                __float_reg[0]                                  // Encoder electrical offset
+#define M_OFFSET                __float_reg[1]                                  // Encoder mechanical offset
+#define I_BW                    __float_reg[2]                                  // Current loop bandwidth
+#define TORQUE_LIMIT            __float_reg[3]                                  // Torque limit (current limit = torque_limit/(kt*gear ratio))
 
-#define PHASE_ORDER             __int_reg[0]
-#define CAN_ID                  __int_reg[1]
-#define ENCODER_LUT             __int_reg[4]
+#define PHASE_ORDER             __int_reg[0]                                    // Phase swapping during calibration
+#define CAN_ID                  __int_reg[1]                                    // CAN bus ID
+#define ENCODER_LUT             __int_reg[4]                                    // Encoder offset LUT - 128 elements long
 
 extern float __float_reg[];
 extern int __int_reg[];

--- a/FOC/foc.cpp	Fri Apr 07 16:23:39 2017 +0000
+++ b/FOC/foc.cpp	Sun Apr 09 03:05:52 2017 +0000
@@ -6,6 +6,7 @@
 
 
 void abc( float theta, float d, float q, float *a, float *b, float *c){
+    /// Inverse DQ0 Transform ///
     ///Phase current amplitude = lengh of dq vector///
     ///i.e. iq = 1, id = 0, peak phase current of 1///
 
@@ -15,6 +16,7 @@
     }
     
 void dq0(float theta, float a, float b, float c, float *d, float *q){
+    /// DQ0 Transform ///
     ///Phase current amplitude = lengh of dq vector///
     ///i.e. iq = 1, id = 0, peak phase current of 1///
     
@@ -23,7 +25,8 @@
     }
     
 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
-    ///u,v,w amplitude = v_bus for full modulation depth///
+    /// Space Vector Modulation ///
+    /// u,v,w amplitude = v_bus for full modulation depth ///
     
     float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))/2.0f;
     *dtc_u = fminf(fmaxf(((u - v_offset)*0.5f/v_bus + ((DTC_MAX-DTC_MIN)/2)), DTC_MIN), DTC_MAX);
@@ -32,11 +35,11 @@
     
     }
 
-void zero_current(int *offset_1, int *offset_2){
+void zero_current(int *offset_1, int *offset_2){                                // Measure zero-offset of the current sensors
     int adc1_offset = 0;
     int adc2_offset = 0;
     int n = 1024;
-    for (int i = 0; i<n; i++){
+    for (int i = 0; i<n; i++){                                                  // Average n samples of the ADC
         ADC1->CR2  |= 0x40000000; 
         wait(.001);
         adc2_offset += ADC2->DR;
@@ -53,26 +56,25 @@
 
 
 void commutate(ControllerStruct *controller, GPIOStruct *gpio, float theta){
-       
-       controller->loop_count ++;
-       if(PHASE_ORDER){
-           controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //Calculate phase currents from ADC readings
+       /// Commutation Loop ///
+       controller->loop_count ++;   
+       if(PHASE_ORDER){                                                                          // Check current sensor ordering
+           controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // Calculate phase currents from ADC readings
            controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
            }
         else{
-            controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    //Calculate phase currents from ADC readings
+            controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
            controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
            }
-       controller->i_a = -controller->i_b - controller->i_c;
-       
-       
+       controller->i_a = -controller->i_b - controller->i_c;                                   
        dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
        
-       ///Cogging Compensation Lookup///
+       ///Cogging compensation lookup, doesn't actually work yet///
        //int ind = theta * (128.0f/(2.0f*PI));
        //float cogging_current = controller->cogging[ind];
        //float cogging_current = 1.0f*cos(6*theta);
-       ///Controller///
+       
+       /// PI Controller ///
        float i_d_error = controller->i_d_ref - controller->i_d;
        float i_q_error = controller->i_q_ref - controller->i_q;// + cogging_current;
        float v_d_ff = 2.0f*(2*controller->i_d_ref*R_PHASE);   //feed-forward voltage
@@ -83,28 +85,24 @@
        //v_d_ff = 0;
        //v_q_ff = 0;
        
-       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_Q*KI_Q));
+       limit_norm(&controller->d_int, &controller->q_int, V_BUS/(K_Q*KI_Q));        // Limit integrators to prevent windup
        //controller->d_int = fminf(fmaxf(controller->d_int, -D_INT_LIM), D_INT_LIM);
        //controller->q_int = fminf(fmaxf(controller->q_int, -Q_INT_LIM), Q_INT_LIM);
        
        
-       controller->v_d = K_D*i_d_error + K_D*KI_D*controller->d_int;// + v_d_ff;  
-       controller->v_q = K_Q*i_q_error + K_Q*KI_Q*controller->q_int;// + v_q_ff; 
+       controller->v_d = K_SCALE*I_BW*i_d_error + K_SCALE*I_BW*controller->d_int;// + v_d_ff;  
+       controller->v_q = K_SCALE*I_BW*i_q_error + K_SCALE*I_BW*controller->q_int;// + v_q_ff; 
        
        //controller->v_d = v_d_ff;
        //controller->v_q = v_q_ff; 
        
-       limit_norm(&controller->v_d, &controller->v_q, controller->v_bus);
-       
+       limit_norm(&controller->v_d, &controller->v_q, controller->v_bus);       // Normalize voltage vector to lie within curcle of radius v_bus
        abc(controller->theta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //inverse dq0 transform on voltages
        svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
 
-       //gpio->pwm_u->write(1.0f-controller->dtc_u);  //write duty cycles
-       //gpio->pwm_v->write(1.0f-controller->dtc_v);
-       //gpio->pwm_w->write(1.0f-controller->dtc_w);  
        
-       if(PHASE_ORDER){
-            TIM1->CCR3 = 0x708*(1.0f-controller->dtc_u);
+       if(PHASE_ORDER){                                                         // Check which phase order to use, 
+            TIM1->CCR3 = 0x708*(1.0f-controller->dtc_u);                        // Write duty cycles
             TIM1->CCR2 = 0x708*(1.0f-controller->dtc_v);
             TIM1->CCR1 = 0x708*(1.0f-controller->dtc_w);
         }
@@ -113,25 +111,20 @@
             TIM1->CCR1 = 0x708*(1.0f-controller->dtc_v);
             TIM1->CCR2 = 0x708*(1.0f-controller->dtc_w);
         }
-       //gpio->pwm_u->write(1.0f - .05f);  //write duty cycles
-        //gpio->pwm_v->write(1.0f - .05f);
-        //gpio->pwm_w->write(1.0f - .1f);
-       //TIM1->CCR1 = 0x708*(1.0f-controller->dtc_u);
-        //TIM1->CCR2 = 0x708*(1.0f-controller->dtc_v);
-        //TIM1->CCR3 = 0x708*(1.0f-controller->dtc_w);
-       controller->theta_elec = theta;   //For some reason putting this at the front breaks thins
+
+       controller->theta_elec = theta;                                          //For some reason putting this at the front breaks thins
        
 
-       if(controller->loop_count >400){
+       //if(controller->loop_count >400){
            //controller->i_q_ref = -controller->i_q_ref;
-           controller->loop_count  = 0;
+          // controller->loop_count  = 0;
            
            //printf("%d   %f\n\r", ind, cogging_current);
            //printf("%f\n\r", controller->theta_elec);
            //pc.printf("%f    %f    %f\n\r", controller->i_a, controller->i_b, controller->i_c);
            //pc.printf("%f    %f\n\r", controller->i_d, controller->i_q);
            //pc.printf("%d    %d\n\r", controller->adc1_raw, controller->adc2_raw);
-            }
+        //    }
     }
 /*    
 void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){

--- a/PositionSensor/PositionSensor.cpp	Fri Apr 07 16:23:39 2017 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sun Apr 09 03:05:52 2017 +0000
@@ -26,9 +26,12 @@
 void PositionSensorAM5147::Sample(){
     cs->write(0);
     raw = spi->write(readAngleCmd);
-    raw &= 0x3FFF;    //Extract last 14 bits
+    raw &= 0x3FFF;                                                              //Extract last 14 bits
     cs->write(1);
-    int angle = raw + offset_lut[raw>>7];
+    int off_1 = offset_lut[raw>>7];
+    int off_2 = offset_lut[((raw>>7)+1)%128];
+    int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7);        // Interpolate between lookup table entries
+    int angle = raw + off_interp;                                               // Correct for nonlinearity with lookup table from calibration
     if(angle - old_counts > _CPR/2){
         rotations -= 1;
         }

--- a/main.cpp	Fri Apr 07 16:23:39 2017 +0000
+++ b/main.cpp	Sun Apr 09 03:05:52 2017 +0000
@@ -113,6 +113,7 @@
     printf(" e - Display Encoder\n\r");
     printf(" esc - Exit to Menu\n\r");
     state_change = 0;
+    gpio.enable->write(0);
     }
 
 void enter_setup_state(void){
@@ -127,20 +128,21 @@
     
 void enter_torque_mode(void){
     controller.i_d_ref = 0;
-    controller.i_q_ref = 0;
-    reset_foc(&controller);                                                     //resets integrators, and other control loop parameters
-    gpio.enable->write(1);
-    GPIOC->ODR ^= (1 << 5);                                                     //turn on status LED
+    controller.i_q_ref = 1;                                                     // Current Setpoints
+    reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
+    gpio.enable->write(1);                                                      // Enable gate drive
+    GPIOC->ODR ^= (1 << 5);                                                     // Turn on status LED
+    state_change = 0;
     }
     
 void calibrate(void){
-    gpio.enable->write(1);
-    GPIOC->ODR ^= (1 << 5);     
-    order_phases(&spi, &gpio, &controller, &prefs);
-    calibrate(&spi, &gpio, &controller, &prefs);
-    GPIOC->ODR ^= (1 << 5);
+    gpio.enable->write(1);                                                      // Enable gate drive
+    GPIOC->ODR ^= (1 << 5);                                                     // Turn on status LED
+    order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
+    calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
+    GPIOC->ODR ^= (1 << 5);                                                     // Turn off status LED
     wait(.2);
-    gpio.enable->write(0);
+    gpio.enable->write(0);                                                      // Turn off gate drive
     printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
      state_change = 0;
      
@@ -160,10 +162,10 @@
         //toggle = 1;
 
         ///Sample current always ///
-        ADC1->CR2  |= 0x40000000;                                               //Begin sample and conversion
+        ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
         //volatile int delay;   
         //for (delay = 0; delay < 55; delay++);
-        controller.adc2_raw = ADC2->DR;
+        controller.adc2_raw = ADC2->DR;                                         // Read ADC1 and ADC2 Data Registers
         controller.adc1_raw = ADC1->DR;
         ///
         
@@ -183,14 +185,17 @@
                 break;
              
             case TORQUE_MODE:                                                   // Run torque control
+                if(state_change){
+                    enter_torque_mode();
+                    }
                 count++;
                 controller.theta_elec = spi.GetElecPosition();                  
                 commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
                 spi.Sample();                                                   // Sample position sensor
                 if(count > 100){
                      count = 0;
-                     readCAN();
-                     controller.i_q_ref = ((float)(canCmd-1000))/100;
+                     //readCAN();
+                     //controller.i_q_ref = ((float)(canCmd-1000))/100;
                     //pc.printf("%f\n\r ", controller.theta_elec);
                      }
                 break;
@@ -213,20 +218,25 @@
   TIM1->SR = 0x0;                                                               // reset the status register
 }
 
-/// Manage state machine with commands from serial terminal or configurator gui ///
+
 char cmd_val[8] = {0};
 char cmd_id = 0;
+char char_count = 0;
 
-char char_count = 0;
+/// Manage state machine with commands from serial terminal or configurator gui ///
+/// Called when data received over serial ///
 void serial_interrupt(void){
     while(pc.readable()){
         char c = pc.getc();
+        if(c == 27){
+                state = REST_MODE;
+                state_change = 1;
+                char_count = 0;
+                cmd_id = 0;
+                for(int i = 0; i<8; i++){cmd_val[i] = 0;}
+                }
         if(state == REST_MODE){
             switch (c){
-                case 27:
-                    state = REST_MODE;
-                    state_change = 1;
-                    break;
                 case 'c':
                     state = CALIBRATION_MODE;
                     state_change = 1;
@@ -246,14 +256,7 @@
                     }
                 }
         else if(state == SETUP_MODE){
-            if(c == 27){
-                state = REST_MODE;
-                state_change = 1;
-                char_count = 0;
-                cmd_id = 0;
-                for(int i = 0; i<8; i++){cmd_val[i] = 0;}
-                }
-            else if(c == 13){
+            if(c == 13){
                 switch (cmd_id){
                     case 'b':
                         I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
@@ -302,11 +305,8 @@
        
 int main() {
     
-
-
     controller.v_bus = V_BUS;
     controller.mode = 0;
-    //spi.ZeroPosition(); 
     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
 
     wait(.1);
@@ -329,11 +329,11 @@
     can.filter(0x020 << 25, 0xF0000004, CANAny, 0);
     
     
-    prefs.load();
-    spi.SetElecOffset(E_OFFSET);
+    prefs.load();                                                               // Read flash
+    spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
     int lut[128] = {0};
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
-    spi.WriteLUT(lut);
+    spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
     
     pc.baud(115200);                                                            // set serial baud rate
     wait(.01);
@@ -347,11 +347,6 @@
     pc.attach(&serial_interrupt);                                               // attach serial interrupt
     
     state_change = 1;
-    //enter_menu_state(); //Print available commands, wait for command
-    //enter_calibration_mode();
-    //wait_us(100);
-    
-    //enter_torque_mode();
 
     
     while(1) {

--- a/math_ops.cpp	Fri Apr 07 16:23:39 2017 +0000
+++ b/math_ops.cpp	Sun Apr 09 03:05:52 2017 +0000
@@ -3,22 +3,27 @@
 
 
 float fmaxf(float x, float y){
+    /// Returns maximum of x, y ///
     return (((x)>(y))?(x):(y));
     }
 
 float fminf(float x, float y){
+    /// Returns minimum of x, y ///
     return (((x)<(y))?(x):(y));
     }
 
 float fmaxf3(float x, float y, float z){
+    /// Returns maximum of x, y, z ///
     return (x > y ? (x > z ? x : z) : (y > z ? y : z));
     }
 
 float fminf3(float x, float y, float z){
+    /// Returns minimum of x, y, z ///
     return (x < y ? (x < z ? x : z) : (y < z ? y : z));
     }
     
 void limit_norm(float *x, float *y, float limit){
+    /// Scales the lenght of vector (x, y) to be <= limit ///
     float norm = sqrt(*x * *x + *y * *y);
     if(norm > limit){
         *x = *x * limit/norm;