Diff: main.cpp

Revision:

58:32e8927fe39f

Parent:

57:0795d2add37e

Child:

60:0c8caaa91182

diff -r 0795d2add37e -r 32e8927fe39f main.cpp
--- a/main.cpp	Thu Nov 07 18:14:51 2019 +0000
+++ b/main.cpp	Tue Feb 04 14:19:58 2020 +0000
@@ -53,7 +53,8 @@
 //DigitalOut drv_en_gate(PA_11);
 DRV832x drv(&drv_spi, &drv_cs);
 
-PositionSensoriCMU spi(16777216, 0.0, NPP);  // Read 24 bits off the sensor
+//PositionSensoriCMU spi(16777216, 0.0, NPP);  // Read 24 bits off the sensor
+PositionSensoriCMU spi(524288, 0.0, NPP);  // Read 24 bits off the sensor, only 19bits are encoder data
 
 volatile int count = 0;
 volatile int state = REST_MODE;
@@ -62,6 +63,7 @@
 void onMsgReceived() {
     //msgAvailable = true;
     //printf("%d\n\r", rxMsg.id);
+    gpio.led->write(1);  
     can.read(rxMsg);  
     if((rxMsg.id == CAN_ID)){
         controller.timeout = 0;
@@ -80,9 +82,11 @@
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
             }
-        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); // multiply theta, dtheta_mech by GR here?
+        //pack_reply(&txMsg, 3.14159f, 6.28318f, 1.00f); // test values
         can.write(txMsg);
         }
+    gpio.led->write(0);  
     
 }
 
@@ -158,9 +162,9 @@
     wait(.05);
     R_NOMINAL = 0;
     state = INIT_TEMP_MODE;
-    //printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
+    printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
     //drv.disable_gd();
-    //state_change = 0;
+    state_change = 0;
      
     }
     
@@ -191,8 +195,8 @@
         controller.adc1_raw = ADC1->DR;
         controller.adc3_raw = ADC3->DR;
         controller.theta_elec = spi.GetElecPosition();
-        controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
-        controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
+        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();
         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement