void CLocomotion::lStop()
{
Serial.println("STOP PROCEDURE");
while(m_speedConsigne > 5){
updatePulses();
m_speedConsigne -= 5;
updateEtat();
delay(20);
// printLPulses();
}
Timer3.stop();
updatePower(0);
}
void CLocomotion::lSpeedControl()
{
int speedError = m_speedConsigne - m_etat.speed; //error
Serial.print("Error : ");
Serial.println(speedError);
m_speedErrorSum += speedError; //integration
int speedErrorDerivative = speedError - m_speedErrorPrev;// action derivee
m_speedErrorPrev = speedError;
int command = KP*speedError + KI*m_speedErrorSum + KD*m_speedErrorPrev;
int commandD = command;
int commandG = command;
if(abs(m_etat.dir)==1){
commandD -= (int)(KPP*m_dPulses);
commandG += (int)(KPP*m_dPulses);
// Serial.print("Droite 1: ");
// Serial.println(commandD);
// Serial.print("Gauche 1: ");
// Serial.println(commandG);
}
else {
commandD -= (int)KPP*m_dPulses;
commandG += (int)KPP*m_dPulses;
}
// Serial.print("commandD :");
// Serial.println(commandD);
// Serial.print("commandG :");
// Serial.println(commandG);
// command = 32*sqrt(command);
commandD = 32*sqrt(commandD);
commandG = 32*sqrt(commandG);
Serial.print("Droite : ");
Serial.println(commandD);
Serial.print("Gauche : ");
Serial.println(commandG);
updatePower(commandD,commandG);
}
int main(void) {
initializePlatform();
initializeLogging();
initializeTimers();
initializePower();
initializeUsb(listener.usb);
initializeSerial(listener.serial);
initializeEthernet(listener.ethernet);
initializeLights();
initializeBluetooth();
debug("Initializing as %s", getMessageSet());
setup();
for (;;) {
loop();
processListenerQueues(&listener);
updateInterfaceLight();
updatePower();
}
return 0;
}
void SkillController::Update()
{
if (game_obj_ev != nullptr)
{
updateRun();
updateHide();
updateCoolDownTimer(false);
if (*game_obj_ev == SKILL)
{
if (!game_obj_info->skill_list.empty())
{
updateGlobalVibration();
unsigned int skill_size = game_obj_info->skill_list.size();
if (current_skill >= 0 && current_skill < skill_size)
{
skill_info* skill_iter = getCurrentSkill();
const char* skill_type = skill_iter->skill_type;
if (!strcmp(skill_type, "Charge"))
updateCharge();
else if (!strcmp(skill_type, "Power"))
updatePower();
else if (!strcmp(skill_type, "Chain"))
updateChain();
}
if (isSkillFinish)
{
*game_obj_ev = SEARCHING_RECOGNIZE_AREA;
chain_index = 0;
}
}
}
else
onVibration = false;
}
}
