Diff: main.cpp

Revision:

55:71a6e5fe5805

Parent:

53:e85efce8c1eb

--- a/main.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp	Tue Dec 15 07:28:27 2020 +0000
@@ -58,9 +58,13 @@
 volatile int state = REST_MODE;
 volatile int state_change;
 
+Timer t;
+
 void onMsgReceived() {
     //msgAvailable = true;
     //printf("%d\n\r", rxMsg.id);
+    //t.reset();
+    //t.start();
     can.read(rxMsg);  
     if((rxMsg.id == CAN_ID)){
         controller.timeout = 0;
@@ -74,13 +78,20 @@
             gpio.led->write(0);; 
             }
         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
+            //t.reset();
+            //t.start();
             spi.ZeroPosition();
+            //t.stop();
+            //printf("Set zero position time taken was %lf seconds\n", t.read());
+            
             }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
             }
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         can.write(txMsg);
+        //t.stop();
+        //printf("can time taken was %lf seconds\n",t.read());
         }
     
 }
@@ -280,6 +291,7 @@
                     state_change = 1;
                     break;
                 case 'z':
+
                     spi.SetMechOffset(0);
                     spi.Sample(DT);
                     wait_us(20);
@@ -289,6 +301,7 @@
                         prefs.close();    
                         prefs.load(); 
                     spi.SetMechOffset(M_OFFSET);
+
                     printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
                     
                     break;
@@ -358,6 +371,7 @@
             
         }
     }
+
        
 int main() {
     controller.v_bus = V_BUS;
@@ -434,9 +448,7 @@
     printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
     printf(" CAN ID:  %d\n\r", CAN_ID);
     
-
-
-
+    
     //printf(" %d\n\r", drv.read_register(DCR));
     //wait_us(100);
     //printf(" %d\n\r", drv.read_register(CSACR));
@@ -453,13 +465,34 @@
     while(1) {
         drv.print_faults();
         wait(.1);
+        
+        //if(state == MOTOR_MODE)
+        //    printf("i_q*kt_out: %f \n\r",controller.i_q_filt*KT_OUT);
+        
+        //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+        //can.write(txMsg);
        //printf("%.4f\n\r", controller.v_bus);
-       /*
+
+        /*
         if(state == MOTOR_MODE)
         {
             //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
             //printf("%.3f  %.3f  %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
             //printf("%.3f\n\r", controller.dtheta_mech);
+            float pos, intpart;
+            pos = controller.theta_mech;
+            modf(pos/(2*PI),&intpart);
+            pos = pos - intpart*2*PI;
+            
+            if(abs(pos) <= 0.001)
+                pos = abs(pos);
+            else if(pos < 0)
+                pos = pos + 2*PI;
+            
+            //printf("intpart: %f\n\r",intpart);
+            //printf("theta_mech: %f\n\r",controller.theta_mech);
+            printf("pos: %f\n\r",pos);
+            
             wait(.002);
         }
         */