Diff: main.cpp

Revision:

58:fb799e99a5f7

Parent:

57:c26b0093783b

Child:

59:568e7be5232f

--- a/main.cpp	Sat Jun 27 07:50:17 2020 +0000
+++ b/main.cpp	Mon Jun 29 03:34:16 2020 +0000
@@ -36,7 +36,7 @@
 #include "CAN_com.h"
 #include "DRV.h"
 #include "mode.h"
-//#include "millis.h"
+
 int io_mode = IO_MODE_NONE; // the default mode is serial
 
 float __float_reg[64];                                                          // Floats stored in flash
@@ -78,7 +78,7 @@
 
 void serial_interrupt(void);
 void stepInt();
-void cond_printf(const char *format, ...);
+extern void cond_printf(const char *format, ...);
 
 // set the current mode of the UART connector
 void set_io_mode(int new_io_mode) {    
@@ -277,7 +277,19 @@
     wait_us(10);
     state_change = 0;
     }
-    
+void printFirmwareVer(){
+    cond_printf("\n\r\n\r Hobby King Cheetah\n\r\n\r");
+    wait_us(10);
+    cond_printf("\n\r Debug Info:\n\r");
+    cond_printf(" Firmware Version: %s\n\r", VERSION_NUM);
+    cond_printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
+    cond_printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
+    cond_printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
+    cond_printf(" Gear Ratio %.4f:1\r\n", GR);
+    cond_printf(" Mapped Position %.4f to %.4f Radians\n\r", P_MIN, P_MAX);
+    cond_printf(" PHASE_ORDER %d \r\n", PHASE_ORDER);
+    cond_printf(" CAN ID:  %d\n\r", CAN_ID);
+}    
 void enter_torque_mode(void){
     drv.enable_gd();
     //gpio.enable->write(1);
@@ -348,6 +360,7 @@
              
             case MOTOR_MODE:                                                   // Run torque control
                 if(state_change){
+                    controller.p_des = controller.theta_mech + 0.710 ; 
                     enter_torque_mode();
                     count = 0;
                     }
@@ -411,6 +424,13 @@
                 gpio.led->write(0);; 
                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
                 }
+         if(c == 't'){                                                            
+        cond_printf("Pos: %.3f Vel: %.3f Cur: %.3f\r\n", controller.theta_mech, controller.theta_elec, controller.i_q_filt*KT_OUT);
+         }   
+         else if(c == 'y'){                                                            
+        printFirmwareVer();
+         } 
+         
         if(state == REST_MODE){
             switch (c){
                 case 'c':
@@ -525,7 +545,19 @@
             
         }
     }
+/*
+Khi
+Controller.P_des = Controller.theta_mech + ( - 0.710)
 
+-0.710 là ???
+
+-> khi enable motor set controller.p_des = 
+
+
+
+
+*/
+       
        
 int main() {
     controller.v_bus = V_BUS;
@@ -591,17 +623,8 @@
     
     set_io_mode(IO_MODE_SERIAL);
     //set_io_mode(IO_MODE_STEP_DIR);
-    wait_us(100);
-    cond_printf("\n\r\n\r Hobby King Cheetah\n\r\n\r");
-    wait_us(10);
-    cond_printf("\n\r Debug Info:\n\r");
-    cond_printf(" Firmware Version: %s\n\r", VERSION_NUM);
-    cond_printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
-    cond_printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
-    cond_printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
-    cond_printf(" Gear Ratio %.4f:1\r\n", GR);
-    cond_printf(" Mapped Position %.4f to %.4f Radians\n\r", P_MIN, P_MAX);
-    cond_printf(" CAN ID:  %d\n\r", CAN_ID);
+    wait_us(1000);
+    printFirmwareVer();
         
     state_change = 1;
     timeSche = 0 ;