SHENG-HEN HSIEH
Just a regular publish
Diff: main.cpp
- Revision:
- 25:3c6e83b449b2
- Parent:
- 24:518ec8a4fb6d
- Child:
- 26:ad4fbceeb4ae
--- a/main.cpp Sun Jul 19 03:27:01 2020 +0000
+++ b/main.cpp Sun Feb 21 04:19:41 2021 +0000
@@ -25,6 +25,8 @@
#define d2r 0.01745329252f
#define dt 0.01f
+#define st2r 0.033f //steer signal to actual rad
+
DigitalOut Aux_Rly(PC_10,0); //Control aux relay, 1 active
DigitalOut Fault_Ind(PC_12,0); //Indicate fault bt flashing, 1 active
DigitalOut LED(D13, 0); //Internal LED output, general purpose
@@ -279,6 +281,8 @@
// pc.printf("%.3f,%.3f,%.3f\n\r", YR_imu, Ax_imu, Ay_imu);
// pc.printf("%.3f,%.3f,%.3f\n\r", Roll_imu, Pitch_imu, Yaw_imu);
// pc.printf("%.3f\n\r", Trq_HMI*100.0f);
+// pc.printf("%.3f\n\r", Steer_HMI);
+// pc.printf("%.1f\n\r", Vx_wss);
}
// End of high speed loop
@@ -297,12 +301,12 @@
//Merge Faults
//USE THIS FOR FULL FUNCTION
-// FLT_Post = FL_FLT_Post|FR_FLT_Post|RL_FLT_Post|RR_FLT_Post;
-// FLT_Run = FL_FLT_Run|FR_FLT_Run|RL_FLT_Run|RR_FLT_Run;
+ FLT_Post = FL_FLT_Post|FR_FLT_Post|RL_FLT_Post|RR_FLT_Post;
+ FLT_Run = FL_FLT_Run|FR_FLT_Run|RL_FLT_Run|RR_FLT_Run;
//ONLY FOR TEST TODO
- FLT_Post = RL_FLT_Post; // Use only for single module test
- FLT_Run = RL_FLT_Run; //2020/3/10
+// FLT_Post = RL_FLT_Post; // Use only for single module test
+// FLT_Run = RL_FLT_Run; //2020/3/10
//UART
FLT_print_cnt += FLT_print;
@@ -374,9 +378,9 @@
void POST(void)
{
//Check IMU status abnormal
- if(fabsf(YR_imu) > 250) { //half turn per sec, strange
- VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
- }
+// if(fabsf(YR_imu) > 250) { //half turn per sec, strange
+// VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
+// }
}
void RUNT(void)
@@ -396,9 +400,14 @@
if(VDU_STAT == VDU_Run) {
if(SDn_voltage < 8.0f) {
//2020/4/5 TODO remove in real case
-// VDU_FLT |= ShDVol_VDUFLTCode; //Shutdown circuit unclosed or uv
+ VDU_FLT |= ShDVol_VDUFLTCode; //Shutdown circuit unclosed or uv
}
}
+
+ //Check IMU status abnormal add in 2020/10/07
+ if(fabsf(YR_imu) > 250.0f) { //half turn per sec, strange
+ VDU_FLT |= IMUSTA_VDUFLTCode; //IMU status error
+ }
}
void Aux_read(void)
@@ -457,6 +466,7 @@
{
#ifndef IMU_direct
//Get readings threw back ground, direction not checked 2019/12/20
+ // Direction checked 2020/10/29
YR_imu = imu.imuProcessedData.rate[2];
Ax_imu = imu.imuProcessedData.accel[0];
Ay_imu = imu.imuProcessedData.accel[1];
@@ -474,22 +484,91 @@
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;
+ float Tayc_tmp = 0; //active part of yaw control system
+ float Tlsd_tmp = 0; //limit slip differetial torque
+ float YdelW_ele = 0; //Ideal L-R electric speed difference
+
+ //Get estimate from wheel only
+ Vx_wss = (FL_W_ele+FR_W_ele+RL_W_ele+RR_W_ele)*0.25f/4.0f/11.0f*0.235f; // average, pole pair, reducer, wheel radius
+
+ //Simple comp filter is fine without GSS onboard
+ Vx_fil += (Ax_imu-0.01f)*9.807f*0.01f; //-0.01 here is to be calibrated
+ Vx_fil = Vx_fil*0.98f + Vx_wss*0.02f; //0.98, 0.02 is coefficient
+
+ if(AWD_HMI) {
+ // A simple version is put here for reading
+ //YR_ref = Steer_HMI*st2r*Vb_est/(Base+EG*Vb_est*Vb_est);
+
+ /*Still in test section, ignore*/
+ //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);
+
+ /*A basic static distribution*/
+ FL_Tcmd = 0.11f*Trq_HMI;
+ FR_Tcmd = 0.11f*Trq_HMI;
+ RL_Tcmd = 0.25f*Trq_HMI;
+ RR_Tcmd = 0.25f*Trq_HMI;
+
+ /*A basic Yaw control*/
+ YR_ref = (Steer_HMI*st2r)*Vx_fil/Base; // Center calibration moved to can recieve
+ Tayc_tmp = (YR_ref - YR_imu*d2r)*10.0f; // map 1 rad/s YR difference to ~ 10Nm
+ Tayc_tmp = constrain(Tayc_tmp,-5.0f,5.0f);
+ FL_Tcmd += -Tayc_tmp;
+ FR_Tcmd += Tayc_tmp;
+ RL_Tcmd += -Tayc_tmp;
+ RR_Tcmd += Tayc_tmp;
+
+ /*A basic ideal differential controller*/
+ YdelW_ele = YR_ref*Track/0.235f*11.0f*4.0f; // dir, rate to speed, wheel radius, reducer, pole pair
+ //YdelW_ele > 0 when left turning
+
+ /*A basic static distribution + differential for test 2020/7/19*/
+ //Front differential
+ Tlsd_tmp = (FL_W_ele - FR_W_ele + YdelW_ele)*0.0025f; // map 1000 rad/s difference to ~ 5Nm
+ Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f);
+ FL_Tcmd += -Tlsd_tmp;
+ FR_Tcmd += Tlsd_tmp;
+
+
+ //Rear differential
+ Tlsd_tmp = (RL_W_ele - RR_W_ele + YdelW_ele)*0.0025f; // map 1000 rad/s difference to ~ 5Nm
+ Tlsd_tmp = constrain(Tlsd_tmp,-5.0f,5.0f);
+ RL_Tcmd += -Tlsd_tmp;
+ RR_Tcmd += Tlsd_tmp;
+
+ } else {
+ //2020/8/19 for fast test pure rear
+ Tlsd_tmp = (FL_W_ele - FR_W_ele)*0.01f; // map 1000 rad/s difference to ~ 10Nm
+ Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f);
+ if(Tlsd_tmp>0.0f) { //L > R
+ FL_Tcmd = 0.15f*Trq_HMI - Tlsd_tmp;
+ FR_Tcmd = 0.15f*Trq_HMI;
+ } else { //L < R, Tlsd_tmp < 0
+ FL_Tcmd = 0.15f*Trq_HMI;
+ FR_Tcmd = 0.15f*Trq_HMI + Tlsd_tmp;
+ }
+ //Rear differential
+ Tlsd_tmp = (RL_W_ele - RR_W_ele)*0.005f; // map 1000 rad/s difference to ~ 5Nm
+ Tlsd_tmp = constrain(Tlsd_tmp,-10.0f,10.0f);
+ if(Tlsd_tmp>0.0f) { //L > R
+ RL_Tcmd = 0.25f*Trq_HMI - Tlsd_tmp;
+ RR_Tcmd = 0.25f*Trq_HMI;
+ } else { //L < R, Tlsd_tmp < 0
+ RL_Tcmd = 0.25f*Trq_HMI;
+ RR_Tcmd = 0.25f*Trq_HMI + Tlsd_tmp;
+ }
+
+
+
+// // A basic static distribution 2020/7/19
+// FL_Tcmd = 0.15f*Trq_HMI;
+// FR_Tcmd = 0.15f*Trq_HMI;
// RL_Tcmd = 0.25f*Trq_HMI;
// RR_Tcmd = 0.25f*Trq_HMI;
-// }
+ }//last line of: if(AWD_HMI){}
////Add to force normal drive
// FL_Tcmd = 0.25f*Trq_HMI;
@@ -498,16 +577,16 @@
// 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;
+// 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;
+//Direction define, move to can sendout
+// 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)
@@ -529,16 +608,15 @@
//HSB from FL motor drive
FL_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT
tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
- FL_W_ele = tmp*1.0f;
+ FL_W_ele = tmp*-1.0f;
tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
- FL_Trq_fil3 = tmp * 0.01f;
+ FL_Trq_fil3 = tmp * -0.01f;
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
- FL_Trq_est = tmp * 0.01f;
+ FL_Trq_est = tmp * -0.01f;
FL_online = 5;
//If fault
if(FL_DSM == 3U) {
- // USE THIS FOR FULL FUNCTION
-// VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
}
break;
@@ -553,8 +631,7 @@
FR_Trq_est = tmp * 0.01f;
FR_online = 5;
if(FR_DSM == 3U) {
- // USE THIS FOR FULL FUNCTION
-// VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
}
break;
@@ -562,11 +639,11 @@
//HSB from RL motor drive
RL_DSM = can_msg_Rx.data[6] & 0x03; //Get DSM_STAT
tmp = can_msg_Rx.data[5] << 8 | can_msg_Rx.data[4];
- RL_W_ele = tmp*1.0f;
+ RL_W_ele = tmp*-1.0f;
tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
- RL_Trq_fil3 = tmp * 0.01f;
+ RL_Trq_fil3 = tmp * -0.01f;
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
- RL_Trq_est = tmp * 0.01f;
+ RL_Trq_est = tmp * -0.01f;
RL_online = 5;
if(RL_DSM == 3U) {
VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
@@ -584,8 +661,7 @@
RR_Trq_est = tmp * 0.01f;
RR_online = 5;
if(RR_DSM == 3U) {
- // USE THIS FOR FULL FUNCTION
-// VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
+ VDU_FLT |= DSM_VDUFLTCode; //DSM Fault
}
break;
@@ -595,7 +671,7 @@
RST_HMI = can_msg_Rx.data[5] & 0x01; //Get RST request
RTD_HMI = can_msg_Rx.data[4] & 0x01; //Get RTD switch
tmp = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
- Steer_HMI = tmp * 0.01f;
+ Steer_HMI = tmp * 0.01f - 0.0f; //0.7f here is to calibrated center
tmp = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
Trq_HMI = tmp * 0.01f;
PSU_online = 5;
@@ -642,6 +718,10 @@
RR_FLT_Run = can_msg_Rx.data[3] << 8 | can_msg_Rx.data[2];
RR_FLT_Post = can_msg_Rx.data[1] << 8 | can_msg_Rx.data[0];
break;
+
+ case PedalStat_ID://10
+ RTD_SW = 0x01&can_msg_Rx.data[1];
+ break;
// end of 10Hz msg
}
}
@@ -663,7 +743,7 @@
void Tx_Tcmd_CAN1(void) // 100 Hz
{
int16_t tmp;
- tmp = (int16_t) (FL_Tcmd * 100.0f);
+ tmp = (int16_t) (FL_Tcmd * -100.0f);
temp_msg[0] = tmp;
temp_msg[1] = tmp >> 8U;
temp_msg[2] = RTD_cmd;
@@ -684,7 +764,7 @@
CANpendTX();
can1.write(can_msg_Tx);
- tmp = (int16_t) (RL_Tcmd * 100.0f);
+ tmp = (int16_t) (RL_Tcmd * -100.0f);
temp_msg[0] = tmp;
temp_msg[1] = tmp >> 8U;
can_msg_Tx = CANMessage(RL_CMD_ID,temp_msg,8,CANData,CANStandard);
@@ -698,11 +778,34 @@
CANpendTX();
can1.write(can_msg_Tx);
+ // IMU attitude readings shitting out
+ tmp = (int16_t) (YR_imu * 100.0f);
+ temp_msg[0] = tmp;
+ temp_msg[1] = tmp >> 8U;
+ tmp = (int16_t) (Roll_imu * 100.0f);
+ temp_msg[2] = tmp;
+ temp_msg[3] = tmp >> 8U;
+ tmp = (int16_t) (Pitch_imu * 100.0f);
+ temp_msg[4] = tmp;
+ temp_msg[5] = tmp >> 8U;
+ temp_msg[6] = (int8_t)(Ax_imu * 50.0f);
+ temp_msg[7] = (int8_t)(Ay_imu * 50.0f);
+ can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
+ CANpendTX();
+ can1.write(can_msg_Tx);
+
+ // Some internal control variables shitting out
+ tmp = (int16_t) (Vx_fil * 100.0f);
+ temp_msg[0] = tmp;
+ temp_msg[1] = tmp >> 8U;
+ can_msg_Tx = CANMessage(RegularVar_ID,temp_msg,2,CANData,CANStandard);
+ CANpendTX();
+ can1.write(can_msg_Tx);
}
void Tx_Qdrv_CAN1(void) // 10 Hz
{
- int16_t tmp;
+ //int16_t tmp;
// Auxilary sensor reading shitting out
temp_msg[0] = AUX_1_u16;
temp_msg[1] = AUX_1_u16 >> 8U;
@@ -721,29 +824,13 @@
temp_msg[1] = VDU_FLT >> 8U;
temp_msg[2] = VDU_STAT;
temp_msg[3] = (int8_t)(SDn_voltage*10.0f);
- temp_msg[4] = (int8_t)(Brk_voltage*10.0f);
+ temp_msg[4] = (int8_t)(Brk_voltage*50.0f);
temp_msg[5] = 0U;
temp_msg[6] = 0U;
temp_msg[7] = 0U;
can_msg_Tx = CANMessage(Qdrv_stat_ID,temp_msg,8,CANData,CANStandard);
CANpendTX();
can1.write(can_msg_Tx);
-
- // IMU attitude readings shitting out, 10Hz on CAN but 100Hz for internal use
- tmp = (int16_t) (YR_imu * 10000.0f);
- temp_msg[0] = tmp;
- temp_msg[1] = tmp >> 8U;
- tmp = (int16_t) (Roll_imu * 100.0f);
- temp_msg[2] = tmp;
- temp_msg[3] = tmp >> 8U;
- tmp = (int16_t) (Pitch_imu * 100.0f);
- temp_msg[4] = tmp;
- temp_msg[5] = tmp >> 8U;
- temp_msg[6] = (int8_t)Ax_imu;
- temp_msg[7] = (int8_t)Ay_imu;
- can_msg_Tx = CANMessage(IMU_sense_ID,temp_msg,8,CANData,CANStandard);
- CANpendTX();
- can1.write(can_msg_Tx);
}
void CANpendTX(void)
@@ -773,6 +860,7 @@
can1.filter(RL_LSB_ID,0xFFFF,CANStandard,6);
can1.filter(RR_LSB_ID,0xFFFF,CANStandard,7);
can1.filter(HMI_cmd_ID,0xFFFF,CANStandard,8); // PSU online monitoring
+ can1.filter(PedalStat_ID,0xFFFF,CANStandard,9); // PSU online monitoring
// can1.mode(CAN::GlobalTest); // Add only for testing 2019/11/13
can1.attach(&Rx_CAN1, CAN::RxIrq); // CAN1 Recieve Irq
}
@@ -910,10 +998,10 @@
Max_Tmodule = max_fval(FL_Tmodule, FR_Tmodule, RL_Tmodule, RR_Tmodule);
//2020/6/14 only for test use AWD_HMI
- if(AWD_HMI) {
+ if(RTD_SW) {
+ Aux_Rly = 0;
+ } else {
Aux_Rly = 1;
- } else {
- Aux_Rly = 0;
}
}
@@ -934,4 +1022,3 @@
if(i4 > max) max = i4;
return max;
}
-// pc.printf("SOC: %.2f\n", Module_Total*0.01f);
\ No newline at end of file