Diff: main.cpp

Revision:

77:81c2cbc5d906

Parent:

76:4fd876d4cf2b

Child:

78:4f481ff040ee

--- a/main.cpp	Tue Oct 18 16:22:24 2022 +0000
+++ b/main.cpp	Thu Nov 10 15:35:33 2022 +0000
@@ -1,3 +1,10 @@
+//SET THESE FOR YOUR MOTOR
+#define V_U12_MIN -65.0f
+#define V_U12_MAX 65.0f
+
+
+
+
 /// high-bandwidth 3-phase motor control, for robots
 /// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, David Otten, and others
 /// Hardware documentation can be found at build-its.blogspot.com
@@ -61,6 +68,12 @@
 volatile int count = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
+volatile int logger = 0;
+
+//Position Sensor Temperature Fix - INITIALIZE THESE IN THE MAIN SETUP
+volatile float prev_mech = 0.0;
+volatile float prev_elec = 0.0;
+volatile int failure_counter = 0;
 
 void onMsgReceived() {
     //msgAvailable = true;
@@ -237,14 +250,30 @@
         /*
         if(count < 10){printf("%d\n\r", spi.GetRawPosition());}
         count ++;
-        */        
+        */
+                
+        //FILTER
+        float velocity = (1.0f/GR)*spi.GetMechVelocity();
+        
         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
-        controller.theta_elec = spi.GetElecPosition();
-        controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); // should these be divided by gear ratio???
-        controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  // mech pos isn't pre-multiplied by GR in position sensor function...
-        controller.dtheta_elec = spi.GetElecVelocity();
+        
+        if (velocity > V_U12_MAX || velocity < V_U12_MAX) {
+                controller.dtheta_mech = 0.0;
+                controller.dtheta_elec = 0.0;
+                controller.theta_elec = prev_elec;
+                controller.theta_mech = prev_mech;
+                failure_counter++;
+        } else {
+                controller.theta_elec = spi.GetElecPosition();
+                controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); // should these be divided by gear ratio???
+                controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  // mech pos isn't pre-multiplied by GR in position sensor function...
+                controller.dtheta_elec = spi.GetElecVelocity();
+                prev_elec = controller.theta_elec;
+                prev_mech = controller.theta_mech;
+        }
+        
         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
         
 
@@ -308,6 +337,7 @@
                 if(state_change){
                     enter_torque_mode();
                     count = 0;
+                    counter = 0;
                     }
                 else{
                 /*
@@ -333,6 +363,18 @@
                 update_observer(&controller, &observer);
                 field_weaken(&controller);
                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
+                
+                //LOG THE POSITION AND ADD TO THE STACK
+                for( int i = 0; i<LOG_POINTS-1; i++) {
+                    pos_high_frequency[i] = pos_high_frequency[i+1];
+                }
+                pos_high_frequency[LOG_POINTS-1] = spi.GetMechPosition();
+                
+                if (counter == 1000000) {
+                    pc.printf("Num Failure: %d\n\r", failure_counter);
+                    counter = 0;
+                }
+                
                 controller.timeout++;
 
                 if(controller.otw_flag)
@@ -343,6 +385,7 @@
                 }
                 
                 count++; 
+                counter++;
                 }   
                 
                   
@@ -411,6 +454,15 @@
                     printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
                     
                     break;
+                 case 'p':
+                    //turn off switching
+                    state = REST_MODE;
+                    state_change = 1;
+                    //print the characters to terminal
+                    for (int i=0; i< LOG_POINTS; i++) {
+                        printf("%f\n\r", pos_high_frequency[i]); //return carrige 
+                    }
+                    break;
                 }
                 
                 }
@@ -520,6 +572,7 @@
                 case 'd':
                     controller.i_q_ref = 0;
                     controller.i_d_ref = 0;
+                    break;
                 case 'p':
                     //turn off switching
                     state = REST_MODE;
@@ -528,6 +581,7 @@
                     for (int i=0; i< LOG_POINTS; i++) {
                         printf("%f\n\r", pos_high_frequency[i]); //return carrige 
                     }
+                    break;
                 }
             }
             
@@ -617,6 +671,10 @@
     if(spi.GetMechPosition() > PI){spi.SetMechOffset(M_OFFSET+2.0f*PI);}        // now zeroes to +- 30 degrees about nominal, independent of rollover point
     else if (spi.GetMechPosition() < -PI){spi.SetMechOffset(M_OFFSET-2.0f*PI);}
     
+    //INITIALIZE THE PREVIOUS ANGLES FOR FILTER
+    prev_mech = spi.GetMechPosition();
+    prev_elec = spi.GetElecPosition();
+    
     int lut[128] = {0};
     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
@@ -642,19 +700,12 @@
     pc.attach(&serial_interrupt);                                               // attach serial interrupt
 
 
-    //int counter = 0;
+    int counter = 0;
     while(1) {
         //drv.print_faults();
         wait(.1);
         
         if(controller.otw_flag){gpio.led->write(!gpio.led->read());}
-        
-        if(state == MOTOR_MODE) {
-            for (int i=0; i<= LOG_POINTS-2; i++) {
-                pos_high_frequency[i] = pos_high_frequency[i+i];
-            }
-            pos_high_frequency[499] = controller.theta_mech;
-        }
              /*
         if(state == MOTOR_MODE)
         {