Diff: main.cpp

Revision:

78:4f481ff040ee

Parent:

77:81c2cbc5d906

--- a/main.cpp	Thu Nov 10 15:35:33 2022 +0000
+++ b/main.cpp	Fri Nov 11 16:58:43 2022 +0000
@@ -69,10 +69,13 @@
 volatile int state = REST_MODE;
 volatile int state_change;
 volatile int logger = 0;
+volatile int counter = 0;
 
 //Position Sensor Temperature Fix - INITIALIZE THESE IN THE MAIN SETUP
 volatile float prev_mech = 0.0;
 volatile float prev_elec = 0.0;
+volatile float prev_dmech = 0.0;
+volatile float prev_delec = 0.0;
 volatile int failure_counter = 0;
 
 void onMsgReceived() {
@@ -259,12 +262,13 @@
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
         
-        if (velocity > V_U12_MAX || velocity < V_U12_MAX) {
-                controller.dtheta_mech = 0.0;
+        if (velocity > V_U12_MAX || velocity < V_U12_MIN) {
+                controller.dtheta_mech = 0.0; //previous velocity? 
                 controller.dtheta_elec = 0.0;
                 controller.theta_elec = prev_elec;
                 controller.theta_mech = prev_mech;
                 failure_counter++;
+                //pc.printf("FAILURE\n\r");
         } else {
                 controller.theta_elec = spi.GetElecPosition();
                 controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); // should these be divided by gear ratio???
@@ -272,6 +276,8 @@
                 controller.dtheta_elec = spi.GetElecVelocity();
                 prev_elec = controller.theta_elec;
                 prev_mech = controller.theta_mech;
+                prev_dmech = controller.dtheta_mech;
+                prev_delec = controller.dtheta_elec;
         }
         
         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement