Diff: main.cpp

Revision:

24:518ec8a4fb6d

Parent:

23:100661e2ad70

Child:

25:3c6e83b449b2

--- a/main.cpp	Fri Mar 13 15:25:57 2020 +0000
+++ b/main.cpp	Sun Jul 19 03:27:01 2020 +0000
@@ -310,6 +310,11 @@
                     if(FLT_Post!=0)printf("POST FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Post,FR_FLT_Post,RL_FLT_Post,RR_FLT_Post);
                     if(FLT_Run!=0)printf("RUN FL,FR,RL,RR\n0x%04X 0x%04X 0x%04X 0x%04X\n", FL_FLT_Run,FR_FLT_Run,RL_FLT_Run,RR_FLT_Run);
                     if(VDU_FLT!=0)printf("VDU\n0x%04X\n\n", VDU_FLT);
+
+                    //Only temperature printing 2020/6/30
+                    printf("Tmotor FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor);
+                    printf("Tmodule FL,FR,RL,RR\t%.1f %.1f %.1f %.1f\r\n", FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);
+
                     FLT_print_cnt = 0;
                 }
 
@@ -348,6 +353,8 @@
                 LED = !LED;     //Fast 5Hz blinky indicate the activity
                 Fault_Ind = LED;
             }
+
+
         }
         // End of low speed state reporting
 
@@ -388,7 +395,8 @@
     //Check ShutDrv voltage when running
     if(VDU_STAT == VDU_Run) {
         if(SDn_voltage < 8.0f) {
-            VDU_FLT |= ShDVol_VDUFLTCode;       //Shutdown circuit unclosed or uv
+            //2020/4/5 TODO remove in real case
+//            VDU_FLT |= ShDVol_VDUFLTCode;       //Shutdown circuit unclosed or uv
         }
     }
 }
@@ -466,22 +474,40 @@
 
 void AWD(void)
 {
-    if(AWD_HMI) {
-        // A simple version is put here for reading
-        Vb_est = 0.25f * (FL_W_ele + FR_W_ele + RL_W_ele + RR_W_ele);
-        YR_ref = Steer_HMI*d2r*Vb_est/(Base+EG*Vb_est*Vb_est);
-        Mz_reg = (YR_ref - YR_imu) * K_yaw;         //K_yaw unfinished 2019/11/15
-        sig = 0.5f - HCG*Trq_HMI/(Base*Rwhl*Mtot*g0);
-        FL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*sig;
-        FR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*sig;
-        RL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*(1.0f-sig);
-        RR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*(1.0f-sig);
-    } else {
-        FL_Tcmd = 0.25f*Trq_HMI;
-        FR_Tcmd = 0.25f*Trq_HMI;
-        RL_Tcmd = 0.25f*Trq_HMI;
-        RR_Tcmd = 0.25f*Trq_HMI;
-    }
+//    if(AWD_HMI) {
+//        // A simple version is put here for reading
+//        Vb_est = 0.25f * (FL_W_ele + FR_W_ele + RL_W_ele + RR_W_ele);
+//        YR_ref = Steer_HMI*d2r*Vb_est/(Base+EG*Vb_est*Vb_est);
+//        Mz_reg = (YR_ref - YR_imu) * K_yaw;         //K_yaw unfinished 2019/11/15
+//        sig = 0.5f - HCG*Trq_HMI/(Base*Rwhl*Mtot*g0);
+//        FL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*sig;
+//        FR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*sig;
+//        RL_Tcmd = (0.5f*Trq_HMI - Mz_reg*Rwhl/Track)*(1.0f-sig);
+//        RR_Tcmd = (0.5f*Trq_HMI + Mz_reg*Rwhl/Track)*(1.0f-sig);
+//    } else {
+//        FL_Tcmd = 0.25f*Trq_HMI;
+//        FR_Tcmd = 0.25f*Trq_HMI;
+//        RL_Tcmd = 0.25f*Trq_HMI;
+//        RR_Tcmd = 0.25f*Trq_HMI;
+//    }
+
+////Add to force normal drive
+//    FL_Tcmd = 0.25f*Trq_HMI;
+//    FR_Tcmd = 0.25f*Trq_HMI;
+//    RL_Tcmd = 0.25f*Trq_HMI;
+//    RR_Tcmd = 0.25f*Trq_HMI;
+
+//Add to force rear drive
+    FL_Tcmd = 0.2f*Trq_HMI;
+    FR_Tcmd = 0.2f*Trq_HMI;
+    RL_Tcmd = 0.5f*Trq_HMI;
+    RR_Tcmd = 0.5f*Trq_HMI;
+
+    //Direction define
+    FL_Tcmd = -1.0f*FL_Tcmd;
+    FR_Tcmd = 1.0f*FR_Tcmd;
+    RL_Tcmd = -1.0f*RL_Tcmd;
+    RR_Tcmd = 1.0f*RR_Tcmd;
 }
 
 void ASL(void)
@@ -882,7 +908,9 @@
     //Cooling auto control, unfinish 2019/11/15
     Max_Tmotor = max_fval(FL_Tmotor, FR_Tmotor, RL_Tmotor, RR_Tmotor);
     Max_Tmodule = max_fval(FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);
-    if(0) {
+
+    //2020/6/14 only for test use AWD_HMI
+    if(AWD_HMI) {
         Aux_Rly = 1;
     } else {
         Aux_Rly = 0;