/*
* State Machine Processing
*/
void ImplVUE32_3(void)
{
int i = 0;
int sum = 0;
if(flag_1ms_b)
{
flag_1ms_b = 0;
asm volatile ("di"); //Disable int
filter_wheel();
asm volatile ("ei"); //Enable int
gResourceMemory[E_ID_WHEELVELOCITYSSENSOR_FR] = wheel_period_to_kph(spdo1_mean, spd1_moving);
gResourceMemory[E_ID_WHEELVELOCITYSSENSOR_FL] = wheel_period_to_kph(spdo2_mean, spd2_moving);
}
if(unDPRPreviousState != gResourceMemory[E_ID_DPR] && gResourceMemory[E_ID_DPR] == PARK)
{
unDPRPreviousState = gResourceMemory[E_ID_DPR];
DriveDisable(gDrivesVUE32_3, LeftDrive);
DriveDisable(gDrivesVUE32_3, RightDrive);
fDirectionMode = 0.0;
}
if(unDPRPreviousState != gResourceMemory[E_ID_DPR] && (gResourceMemory[E_ID_DPR] == REVERSE || gResourceMemory[E_ID_DPR] == DRIVE))
{
unDPRPreviousState = gResourceMemory[E_ID_DPR];
DriveEnable(gDrivesVUE32_3, LeftDrive);
DriveEnable(gDrivesVUE32_3, RightDrive);
if(gResourceMemory[E_ID_DPR] == REVERSE)
fDirectionMode = -1.0;
else if(gResourceMemory[E_ID_DPR] == DRIVE)
fDirectionMode = 1.0;
}
EVERY_X_MS(20)
DriveStateMachine(gDrivesVUE32_3, LeftDrive, (float)gResourceMemory[E_ID_ACCELERATOR]*0.13*fDirectionMode, (unsigned short)gResourceMemory[E_ID_LEFT_MOTOR_TEMP_ADC]);
DriveStateMachine(gDrivesVUE32_3, RightDrive, (float)gResourceMemory[E_ID_ACCELERATOR]*0.13*fDirectionMode, (unsigned short)gResourceMemory[E_ID_RIGHT_MOTOR_TEMP_ADC]);
END_OF_EVERY
EVERY_X_MS(250)
ReturnDriveInformation(gDrivesVUE32_3, LeftDrive, &gResourceMemory[E_ID_LEFT_MOTOR_SPEED], &gResourceMemory[E_ID_LEFT_MOTOR_CURRENT], &gResourceMemory[E_ID_LEFT_MOTOR_TEMP], &gResourceMemory[E_ID_LEFT_CONTROLLER_TEMP], &gResourceMemory[E_ID_LEFT_DRIVE_BATTERY_CURRENT], &gResourceMemory[E_ID_LEFT_DRIVE_BATTERY_VOLTAGE], &gResourceMemory[E_ID_LEFT_DRIVE_STATUS]);
ReturnDriveInformation(gDrivesVUE32_3, RightDrive, &gResourceMemory[E_ID_RIGHT_MOTOR_SPEED], &gResourceMemory[E_ID_RIGHT_MOTOR_CURRENT], &gResourceMemory[E_ID_RIGHT_MOTOR_TEMP], &gResourceMemory[E_ID_RIGHT_CONTROLLER_TEMP], &gResourceMemory[E_ID_RIGHT_DRIVE_BATTERY_CURRENT], &gResourceMemory[E_ID_RIGHT_DRIVE_BATTERY_VOLTAGE], &gResourceMemory[E_ID_RIGHT_DRIVE_STATUS]);
//Compute mean between both speed motor
// TODO : Fix sign error
gResourceMemory[E_ID_GLOBAL_CAR_SPEED] = (gDrivesVUE32_3[RightDrive].nMotorSpeed-gDrivesVUE32_3[LeftDrive].nMotorSpeed)/2;
SetResourceValue(E_ID_GLOBAL_CAR_SPEED, VUE32_1, sizeof(unsigned short), gResourceMemory[E_ID_GLOBAL_CAR_SPEED]);
END_OF_EVERY
EVERY_X_MS(50)
NETV_MESSAGE msg = {{0}};
msg.msg_priority = NETV_PRIORITY_HIGHEST;
msg.msg_type = NETV_TYPE_SYNCHRONIZE;
msg.msg_cmd = E_ID_VERSION;
msg.msg_source = GetMyAddr(); //GetMyAddr();
msg.msg_dest = 0x05;
msg.msg_comm_iface = NETV_COMM_IFACE_ALL;
msg.msg_data_length = 0;
msg.msg_remote = 1;
netv_send_message(&msg);
END_OF_EVERY
EVERY_X_MS(50)
// Drive mode
//if (fDirectionMode == 1)
//{
userCommand = (float)gResourceMemory[E_ID_ACCELERATOR]*0.13;
carState.w3 = (((float)abs(gResourceMemory[E_ID_RIGHT_MOTOR_SPEED])/3.0)*60.0)*(2.0*3.1416*(0.55/2.0))/1000.0;
carState.w4 = (((float)abs(gResourceMemory[E_ID_LEFT_MOTOR_SPEED])/3.0)*60.0)*(2.0*3.1416*(0.55/2.0))/1000.0;
FloatToInt conv;
conv.val = carState.w3;
gResourceMemory[E_ID_TEST_ALEX_MOTORSPEED1] = conv.raw;
conv.val = carState.w4;
gResourceMemory[E_ID_TEST_ALEX_MOTORSPEED2] = conv.raw;
command = comp(carState, userCommand, gainCorrection, threshold);
gResourceMemory[E_ID_COMP_MOTOR_COMMAND_1] = command.tmWh3;
gResourceMemory[E_ID_COMP_MOTOR_COMMAND_2] = command.tmWh4;
//***********Uncomment the two following lines to activate the torque vectoring********
//DriveStateMachine(gDrivesVUE32_3, LeftDrive, command.tmWh3*fDirectionMode, (unsigned short)gResourceMemory[E_ID_LEFT_MOTOR_TEMP_ADC]);
//DriveStateMachine(gDrivesVUE32_3, RightDrive, command.tmWh4*fDirectionMode, (unsigned short)gResourceMemory[E_ID_RIGHT_MOTOR_TEMP_ADC]);
//*************************************************************
/*}
// Reverse mode
else if (fDirectionMode == -1)
{
command.tmWh3 = userCommand;
command.tmWh4 = userCommand;
}
// Park mode
else if (fDirectionMode == 0)
{
command.tmWh3 = 0;
command.tmWh4 = 0;
}*/
END_OF_EVERY
}
// --[ Method ]---------------------------------------------------------------
//
// - Class : CEffect
// - prototype : bool Parse(CAdvancedParser* pParser)
//
// - Purpose : Parses an effect. The parser has only the text data of
// the effect, not of the whole script.
//
// -----------------------------------------------------------------------------
bool CEffect::Parse(CAdvancedParser* pParser)
{
assert(pParser);
assert(pParser->Ready());
// General info
pParser->ResetPos();
if(!pParser->ReadKeyFloat("#Begin", '=', &m_fBegin, CParser::SEARCH_WHOLEDATA)) return false;
if(!pParser->ReadKeyFloat("#End", '=', &m_fEnd, CParser::SEARCH_WHOLEDATA)) return false;
if(!pParser->ReadKeyInt ("#Layer", '=', &m_nLayer, CParser::SEARCH_WHOLEDATA)) return false;
// Resources
int nError = 0;
pParser->ResetPos();
while(-1 != pParser->Find("#Resource", CParser::SEARCH_FROMCURPOS))
{
std::string strClass, strName, strValue;
if(!pParser->ReadKeyString("#Name", '=', &strName, CParser::SEARCH_INCURLINE)) nError++;
if(!pParser->ReadKeyString("#Class", '=', &strClass, CParser::SEARCH_INCURLINE)) nError++;
if(!pParser->ReadKeyString("#Value", '=', &strValue, CParser::SEARCH_INCURLINE)) nError++;
if(nError)
{
LOG.Write("\nERROR - CEffect::Parse(): Error parsing resource.");
return false;
}
int i = GetResourceIndex(strName);
if(i == -1)
{
LOG.Write("\nERROR - CEffect::Parse(): Resource \"%s\" doesn't belong to this effect.", strName.data());
return false;
}
std::string strClassAux;
GetResourceClassName(i, &strClassAux);
if(strClassAux != strClass)
{
LOG.Write("\nERROR - CEffect::Parse(): Error parsing resource \"%s\". Class=%s (expected %s)", strName.data(), strClass.data(), strClassAux.data());
return false;
}
if(!SetResourceValue(i, strValue))
{
LOG.Write("\nERROR - CEffect::Parse(): Error setting resource \"%s\" value.", strName.data());
return false;
}
}
// Variables
pParser->ResetPos();
while(-1 != pParser->Find("#Var", CParser::SEARCH_FROMCURPOS))
{
std::string strClass, strName;
if(!pParser->ReadKeyString("#Name", '=', &strName, CParser::SEARCH_INCURLINE)) nError++;
if(!pParser->ReadKeyString("#Class", '=', &strClass, CParser::SEARCH_INCURLINE)) nError++;
if(nError)
{
LOG.Write("\nERROR - CEffect::Parse(): Error parsing var.");
return false;
}
CVar* pVar = GetVar(strName);
if(!pVar)
{
LOG.Write("\nERROR - CEffect::Parse(): Error parsing var. \"%s\" doesn't belong to this effect", strName.data());
return false;
}
if(pVar->GetClassName() != strClass)
{
LOG.Write("\nERROR - CEffect::Parse(): Error parsing var \"%s\". Class=%s (expected %s)", strName.data(), strClass.data(), pVar->GetClassName().data());
return false;
}
if(!pVar->Parse(pParser))
{
LOG.Write("\nERROR - CEffect::Parse(): Error parsing var \"%s\" (CVar::Parse() failed).", strName.data());
return false;
}
}
// Commands
pParser->ResetPos();
while(-1 != pParser->Find("#Command", CParser::SEARCH_FROMCURPOS))
{
std::string strCommand;
float fTime;
if(!pParser->ReadStringBetweenChars('<', '>', &strCommand, CParser::SEARCH_INCURLINE, true)) nError++;
if(!pParser->ReadKeyFloat ("#Time", '=', &fTime, CParser::SEARCH_INCURLINE)) nError++;
if(nError)
{
LOG.Write("\nERROR - CEffect::Parse(): Error parsing command.");
return false;
}
AddCommand(fTime, strCommand);
}
return true;
}
/*
* State Machine Processing
*/
void ImplVUE32_3(void)
{
if(flag_1ms_b)
{
flag_1ms_b = 0;
asm volatile ("di"); //Disable int
filter_wheel();
asm volatile ("ei"); //Enable int
gResourceMemory[E_ID_WHEELVELOCITYSSENSOR_FR] = wheel_period_to_kph(spdo1_mean, spd1_moving);
gResourceMemory[E_ID_WHEELVELOCITYSSENSOR_FL] = wheel_period_to_kph(spdo2_mean, spd2_moving);
}
/* Manage motor command sign and drive enabler acconding to DPR switch state*/
//Car state is on Park, then drives have to be disabled
if(unDPRPreviousState != gResourceMemory[E_ID_DPR] && gResourceMemory[E_ID_DPR] == PARK)
{
unDPRPreviousState = gResourceMemory[E_ID_DPR];
DriveDisable(gDrivesVUE32_3, LeftDrive);
DriveDisable(gDrivesVUE32_3, RightDrive);
fDirectionMode = 0.0;
}
//Car state is on Drive or Reverse, then the drives have to be enabled
if(unDPRPreviousState != gResourceMemory[E_ID_DPR] && (gResourceMemory[E_ID_DPR] == REVERSE || gResourceMemory[E_ID_DPR] == DRIVE))
{
unDPRPreviousState = gResourceMemory[E_ID_DPR];
DriveEnable(gDrivesVUE32_3, LeftDrive);
DriveEnable(gDrivesVUE32_3, RightDrive);
//Negative sign
if(gResourceMemory[E_ID_DPR] == REVERSE)
fDirectionMode = -1.0;
//Positive sign
else if(gResourceMemory[E_ID_DPR] == DRIVE)
fDirectionMode = 1.0;
}
EVERY_X_MS(250)
//Copy drives informations to the VUE32 application layer
ReturnDriveInformation(gDrivesVUE32_3, LeftDrive, &gResourceMemory[E_ID_LEFT_MOTOR_SPEED], &gResourceMemory[E_ID_LEFT_MOTOR_CURRENT], &gResourceMemory[E_ID_LEFT_MOTOR_TEMP], &gResourceMemory[E_ID_LEFT_CONTROLLER_TEMP], &gResourceMemory[E_ID_LEFT_DRIVE_BATTERY_CURRENT], &gResourceMemory[E_ID_LEFT_DRIVE_BATTERY_VOLTAGE], &gResourceMemory[E_ID_LEFT_DRIVE_STATUS]);
ReturnDriveInformation(gDrivesVUE32_3, RightDrive, &gResourceMemory[E_ID_RIGHT_MOTOR_SPEED], &gResourceMemory[E_ID_RIGHT_MOTOR_CURRENT], &gResourceMemory[E_ID_RIGHT_MOTOR_TEMP], &gResourceMemory[E_ID_RIGHT_CONTROLLER_TEMP], &gResourceMemory[E_ID_RIGHT_DRIVE_BATTERY_CURRENT], &gResourceMemory[E_ID_RIGHT_DRIVE_BATTERY_VOLTAGE], &gResourceMemory[E_ID_RIGHT_DRIVE_STATUS]);
//TODO Add the transfert function for to get the exact speed car in Kph
gResourceMemory[E_ID_GLOBAL_CAR_SPEED] = (gDrivesVUE32_3[RightDrive].nMotorSpeed+gDrivesVUE32_3[LeftDrive].nMotorSpeed)/2;
SetResourceValue(E_ID_GLOBAL_CAR_SPEED, VUE32_1, sizeof(unsigned short), gResourceMemory[E_ID_GLOBAL_CAR_SPEED]);
END_OF_EVERY
// Sending synchronization messages for simplified compensation
EVERY_X_MS(50)
NETV_MESSAGE msg = {{0}};
msg.msg_priority = NETV_PRIORITY_HIGHEST;
msg.msg_type = NETV_TYPE_SYNCHRONIZE;
msg.msg_cmd = E_ID_VERSION;
msg.msg_source = GetMyAddr(); //GetMyAddr();
msg.msg_dest = 0x05;
msg.msg_comm_iface = NETV_COMM_IFACE_ALL;
msg.msg_data_length = 0;
msg.msg_remote = 1;
netv_send_message(&msg);
END_OF_EVERY
// Calculating compensated torque commands and sending them to the drives
EVERY_X_MS(50)
//Getting accelerator pedal command
userCommand = (float)gResourceMemory[E_ID_ACCELERATOR]*0.31;
//Getting rear wheel speeds (rpm to km/h)
carState.w3 = (((float)abs(gResourceMemory[E_ID_RIGHT_MOTOR_SPEED])/3.0)*60.0)*(2.0*3.1416*(0.55/2.0))/1000.0;
carState.w4 = (((float)abs(gResourceMemory[E_ID_LEFT_MOTOR_SPEED])/3.0)*60.0)*(2.0*3.1416*(0.55/2.0))/1000.0;
//Sending rear wheels speed to the user interface
FloatToInt conv;
conv.val = carState.w3;
gResourceMemory[E_ID_TEST_ALEX_MOTORSPEED1] = conv.raw;
conv.val = carState.w4;
gResourceMemory[E_ID_TEST_ALEX_MOTORSPEED2] = conv.raw;
//Calculating the compensated values
command = comp(carState, userCommand, gainCorrection, threshold);
gResourceMemory[E_ID_COMP_MOTOR_COMMAND_1] = command.tmWh3;
gResourceMemory[E_ID_COMP_MOTOR_COMMAND_2] = command.tmWh4;
// Sending the calculated commands to the drives
DriveStateMachine(gDrivesVUE32_3, LeftDrive, command.tmWh3*fDirectionMode, (unsigned short)gResourceMemory[E_ID_LEFT_MOTOR_TEMP_ADC]);
DriveStateMachine(gDrivesVUE32_3, RightDrive, command.tmWh4*fDirectionMode, (unsigned short)gResourceMemory[E_ID_RIGHT_MOTOR_TEMP_ADC]);
END_OF_EVERY
}
