SHENG-HEN HSIEH
/
VDU_2021
Just a regular publish
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;