public virtual void OnAsservissementModeStatusFromRobot(AsservissementMode asservMode)
{
var handler = OnAsservissementModeStatusFromRobotGeneratedEvent;
if (handler != null)
{
handler(this, new AsservissementModeEventArgs {
mode = asservMode
});
}
}
public void SetAsservissementMode(AsservissementMode mode)
{
asservissementMode = mode;
switch (asservissementMode)
{
case AsservissementMode.Disabled:
LabelConsigneX.Visibility = Visibility.Hidden;
LabelConsigneY.Visibility = Visibility.Hidden;
LabelConsigneTheta.Visibility = Visibility.Hidden;
LabelErreurX.Visibility = Visibility.Hidden;
LabelErreurY.Visibility = Visibility.Hidden;
LabelErreurTheta.Visibility = Visibility.Hidden;
LabelCommandX.Visibility = Visibility.Hidden;
LabelCommandY.Visibility = Visibility.Hidden;
LabelCommandTheta.Visibility = Visibility.Hidden;
LabelConsigneM1.Visibility = Visibility.Hidden;
LabelConsigneM2.Visibility = Visibility.Hidden;
LabelConsigneM3.Visibility = Visibility.Hidden;
LabelConsigneM4.Visibility = Visibility.Hidden;
LabelErreurM1.Visibility = Visibility.Hidden;
LabelErreurM2.Visibility = Visibility.Hidden;
LabelErreurM3.Visibility = Visibility.Hidden;
LabelErreurM4.Visibility = Visibility.Hidden;
LabelCommandM1.Visibility = Visibility.Hidden;
LabelCommandM2.Visibility = Visibility.Hidden;
LabelCommandM3.Visibility = Visibility.Hidden;
LabelCommandM4.Visibility = Visibility.Hidden;
LabelCorrPX.Visibility = Visibility.Hidden;
LabelCorrPY.Visibility = Visibility.Hidden;
LabelCorrPTheta.Visibility = Visibility.Hidden;
LabelCorrIX.Visibility = Visibility.Hidden;
LabelCorrIY.Visibility = Visibility.Hidden;
LabelCorrITheta.Visibility = Visibility.Hidden;
LabelCorrDX.Visibility = Visibility.Hidden;
LabelCorrDY.Visibility = Visibility.Hidden;
LabelCorrDTheta.Visibility = Visibility.Hidden;
LabelCorrPM1.Visibility = Visibility.Hidden;
LabelCorrPM2.Visibility = Visibility.Hidden;
LabelCorrPM3.Visibility = Visibility.Hidden;
LabelCorrPM4.Visibility = Visibility.Hidden;
LabelCorrIM1.Visibility = Visibility.Hidden;
LabelCorrIM2.Visibility = Visibility.Hidden;
LabelCorrIM3.Visibility = Visibility.Hidden;
LabelCorrIM4.Visibility = Visibility.Hidden;
LabelCorrDM1.Visibility = Visibility.Hidden;
LabelCorrDM2.Visibility = Visibility.Hidden;
LabelCorrDM3.Visibility = Visibility.Hidden;
LabelCorrDM4.Visibility = Visibility.Hidden;
break;
case AsservissementMode.Polar:
LabelConsigneX.Visibility = Visibility.Visible;
LabelConsigneY.Visibility = Visibility.Visible;
LabelConsigneTheta.Visibility = Visibility.Visible;
LabelErreurX.Visibility = Visibility.Visible;
LabelErreurY.Visibility = Visibility.Visible;
LabelErreurTheta.Visibility = Visibility.Visible;
LabelCommandX.Visibility = Visibility.Visible;
LabelCommandY.Visibility = Visibility.Visible;
LabelCommandTheta.Visibility = Visibility.Visible;
LabelConsigneM1.Visibility = Visibility.Hidden;
LabelConsigneM2.Visibility = Visibility.Hidden;
LabelConsigneM3.Visibility = Visibility.Hidden;
LabelConsigneM4.Visibility = Visibility.Hidden;
LabelErreurM1.Visibility = Visibility.Hidden;
LabelErreurM2.Visibility = Visibility.Hidden;
LabelErreurM3.Visibility = Visibility.Hidden;
LabelErreurM4.Visibility = Visibility.Hidden;
LabelCommandM1.Visibility = Visibility.Hidden;
LabelCommandM2.Visibility = Visibility.Hidden;
LabelCommandM3.Visibility = Visibility.Hidden;
LabelCommandM4.Visibility = Visibility.Hidden;
LabelCorrPX.Visibility = Visibility.Visible;
LabelCorrPY.Visibility = Visibility.Visible;
LabelCorrPTheta.Visibility = Visibility.Visible;
LabelCorrIX.Visibility = Visibility.Visible;
LabelCorrIY.Visibility = Visibility.Visible;
LabelCorrITheta.Visibility = Visibility.Visible;
LabelCorrDX.Visibility = Visibility.Visible;
LabelCorrDY.Visibility = Visibility.Visible;
LabelCorrDTheta.Visibility = Visibility.Visible;
LabelCorrPM1.Visibility = Visibility.Hidden;
LabelCorrPM2.Visibility = Visibility.Hidden;
LabelCorrPM3.Visibility = Visibility.Hidden;
LabelCorrPM4.Visibility = Visibility.Hidden;
LabelCorrIM1.Visibility = Visibility.Hidden;
LabelCorrIM2.Visibility = Visibility.Hidden;
LabelCorrIM3.Visibility = Visibility.Hidden;
LabelCorrIM4.Visibility = Visibility.Hidden;
LabelCorrDM1.Visibility = Visibility.Hidden;
LabelCorrDM2.Visibility = Visibility.Hidden;
LabelCorrDM3.Visibility = Visibility.Hidden;
LabelCorrDM4.Visibility = Visibility.Hidden;
break;
case AsservissementMode.Independant:
LabelConsigneX.Visibility = Visibility.Hidden;
LabelConsigneY.Visibility = Visibility.Hidden;
LabelConsigneTheta.Visibility = Visibility.Hidden;
LabelErreurX.Visibility = Visibility.Hidden;
LabelErreurY.Visibility = Visibility.Hidden;
LabelErreurTheta.Visibility = Visibility.Hidden;
LabelCommandX.Visibility = Visibility.Hidden;
LabelCommandY.Visibility = Visibility.Hidden;
LabelCommandTheta.Visibility = Visibility.Hidden;
LabelConsigneM1.Visibility = Visibility.Visible;
LabelConsigneM2.Visibility = Visibility.Visible;
LabelConsigneM3.Visibility = Visibility.Visible;
LabelConsigneM4.Visibility = Visibility.Visible;
LabelErreurM1.Visibility = Visibility.Visible;
LabelErreurM2.Visibility = Visibility.Visible;
LabelErreurM3.Visibility = Visibility.Visible;
LabelErreurM4.Visibility = Visibility.Visible;
LabelCommandM1.Visibility = Visibility.Visible;
LabelCommandM2.Visibility = Visibility.Visible;
LabelCommandM3.Visibility = Visibility.Visible;
LabelCommandM4.Visibility = Visibility.Visible;
LabelCorrPX.Visibility = Visibility.Hidden;
LabelCorrPY.Visibility = Visibility.Hidden;
LabelCorrPTheta.Visibility = Visibility.Hidden;
LabelCorrIX.Visibility = Visibility.Hidden;
LabelCorrIY.Visibility = Visibility.Hidden;
LabelCorrITheta.Visibility = Visibility.Hidden;
LabelCorrDX.Visibility = Visibility.Hidden;
LabelCorrDY.Visibility = Visibility.Hidden;
LabelCorrDTheta.Visibility = Visibility.Hidden;
LabelCorrPM1.Visibility = Visibility.Visible;
LabelCorrPM2.Visibility = Visibility.Visible;
LabelCorrPM3.Visibility = Visibility.Visible;
LabelCorrPM4.Visibility = Visibility.Visible;
LabelCorrIM1.Visibility = Visibility.Visible;
LabelCorrIM2.Visibility = Visibility.Visible;
LabelCorrIM3.Visibility = Visibility.Visible;
LabelCorrIM4.Visibility = Visibility.Visible;
LabelCorrDM1.Visibility = Visibility.Visible;
LabelCorrDM2.Visibility = Visibility.Visible;
LabelCorrDM3.Visibility = Visibility.Visible;
LabelCorrDM4.Visibility = Visibility.Visible;
break;
}
}
//Processeur de message en provenance du robot...
//Une fois processé, le message sera transformé en event sortant
public void ProcessDecodedMessage(Int16 command, Int16 payloadLength, byte[] payload)
{
byte[] tab;
uint timeStamp;
switch (command)
{
case (short)Commands.R2PC_WelcomeMessage:
{
OnWelcomeMessageFromRobot();
}
break;
case (short)Commands.R2PC_SpeedPolarAndIndependantOdometry:
{
nbMessageSpeedReceived++;
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float vX = tab.GetFloat();
tab = payload.GetRange(8, 4);
float vY = tab.GetFloat();
tab = payload.GetRange(12, 4);
float vTheta = tab.GetFloat();
tab = payload.GetRange(16, 4);
float vM1 = tab.GetFloat();
tab = payload.GetRange(20, 4);
float vM2 = tab.GetFloat();
tab = payload.GetRange(24, 4);
float vM3 = tab.GetFloat();
tab = payload.GetRange(28, 4);
float vM4 = tab.GetFloat();
OnPolarOdometrySpeedFromRobot(robotID, timeStamp, vX, vY, vTheta);
OnIndependantOdometrySpeedFromRobot(timeStamp, vM1, vM2, vM3, vM4);
}
break;
case (short)Commands.R2PC_IMUData:
{
float accelX = 0, accelY = 0, accelZ = 0, gyroX = 0, gyroY = 0, gyroZ = 0;
timeStamp = 0;
switch (competition)
{
case GameMode.RoboCup:
nbMessageIMUReceived++;
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
accelX = tab.GetFloat();
tab = payload.GetRange(8, 4);
accelY = tab.GetFloat();
tab = payload.GetRange(12, 4);
accelZ = tab.GetFloat();
tab = payload.GetRange(16, 4);
gyroX = tab.GetFloat();
tab = payload.GetRange(20, 4);
gyroY = tab.GetFloat();
tab = payload.GetRange(24, 4);
gyroZ = tab.GetFloat();
break;
case GameMode.Eurobot: //La carte de mesure est placée verticalement
nbMessageIMUReceived++;
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
accelY = -tab.GetFloat();
tab = payload.GetRange(8, 4);
accelZ = tab.GetFloat();
tab = payload.GetRange(12, 4);
accelX = -tab.GetFloat();
tab = payload.GetRange(16, 4);
gyroY = -tab.GetFloat();
tab = payload.GetRange(20, 4);
gyroZ = tab.GetFloat();
tab = payload.GetRange(24, 4);
gyroX = -tab.GetFloat();
break;
}
Point3D accelXYZ = new Point3D(accelX, accelY, accelZ);
Point3D gyroXYZ = new Point3D(gyroX, gyroY, gyroZ);
//On envois l'event aux abonnés
OnIMUDataFromRobot(timeStamp, accelXYZ, gyroXYZ);
}
break;
case (short)Commands.R2PC_MotorCurrentsMonitoring:
{
uint time2 = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
byte[] tab2 = payload.GetRange(4, 4);
float motor1Current = tab2.GetFloat();
tab2 = payload.GetRange(8, 4);
float motor2Current = tab2.GetFloat();
tab2 = payload.GetRange(12, 4);
float motor3Current = tab2.GetFloat();
tab2 = payload.GetRange(16, 4);
float motor4Current = tab2.GetFloat();
tab2 = payload.GetRange(20, 4);
float motor5Current = tab2.GetFloat();
tab2 = payload.GetRange(24, 4);
float motor6Current = tab2.GetFloat();
tab2 = payload.GetRange(28, 4);
float motor7Current = tab2.GetFloat();
//On envois l'event aux abonnés
OnMotorsCurrentsFromRobot(time2, motor1Current, motor2Current, motor3Current, motor4Current, motor5Current, motor6Current, motor7Current);
}
break;
case (short)Commands.R2PC_SpeedAuxiliaryOdometry:
uint time = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float vitesseMotor5 = tab.GetFloat();
tab = payload.GetRange(8, 4);
float vitesseMotor6 = tab.GetFloat();
tab = payload.GetRange(12, 4);
float vitesseMotor7 = tab.GetFloat();
//On envois l'event aux abonnés
OnAuxiliaryOdometrySpeedFromRobot(time, vitesseMotor5, vitesseMotor6, vitesseMotor7);
break;
case (short)Commands.R2PC_SpeedAuxiliaryMotorsConsignes:
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float consigneMotor5 = tab.GetFloat();
tab = payload.GetRange(8, 4);
float consigneMotor6 = tab.GetFloat();
tab = payload.GetRange(12, 4);
float consigneMotor7 = tab.GetFloat();
//On envois l'event aux abonnés
OnAuxiliarySpeedConsigneDataFromRobot(timeStamp, consigneMotor5, consigneMotor6, consigneMotor7);
break;
case (short)Commands.R2PC_EncoderRawData:
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
int enc1RawVal = (int)(payload[7] | payload[6] << 8 | payload[5] << 16 | payload[4] << 24);
int enc2RawVal = (int)(payload[11] | payload[10] << 8 | payload[9] << 16 | payload[8] << 24);
int enc3RawVal = (int)(payload[15] | payload[14] << 8 | payload[13] << 16 | payload[12] << 24);
int enc4RawVal = (int)(payload[19] | payload[18] << 8 | payload[17] << 16 | payload[16] << 24);
int enc5RawVal = (int)(payload[23] | payload[22] << 8 | payload[21] << 16 | payload[20] << 24);
int enc6RawVal = (int)(payload[27] | payload[26] << 8 | payload[25] << 16 | payload[24] << 24);
int enc7RawVal = (int)(payload[31] | payload[30] << 8 | payload[29] << 16 | payload[28] << 24);
//On envois l'event aux abonnés
OnEncoderRawDataFromRobot(timeStamp, enc1RawVal, enc2RawVal, enc3RawVal, enc4RawVal, enc5RawVal, enc6RawVal, enc7RawVal);
break;
case (short)Commands.R2PC_IOMonitoring:
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
byte ioValue = payload[4];
OnIOValuesFromRobot(timeStamp, ioValue);
break;
case (short)Commands.R2PC_PowerMonitoring:
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float battCMDVoltage = tab.GetFloat();
tab = payload.GetRange(8, 4);
float battCMDCurrent = tab.GetFloat();
tab = payload.GetRange(12, 4);
float battPWRVoltage = tab.GetFloat();
tab = payload.GetRange(16, 4);
float battPWRCurrent = tab.GetFloat();
OnPowerMonitoringValuesFromRobot(timeStamp, battCMDVoltage, battCMDCurrent, battPWRVoltage, battPWRCurrent);
break;
case (short)Commands.R2PC_4WheelsSpeedPolarPidCommandErrorCorrectionConsigne:
{
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float xError = tab.GetFloat();
tab = payload.GetRange(8, 4);
float yError = tab.GetFloat();
tab = payload.GetRange(12, 4);
float thetaError = tab.GetFloat();
tab = payload.GetRange(16, 4);
float xCorrection = tab.GetFloat();
tab = payload.GetRange(20, 4);
float yCorrection = tab.GetFloat();
tab = payload.GetRange(24, 4);
float thetaCorrection = tab.GetFloat();
tab = payload.GetRange(28, 4);
float xconsigne = tab.GetFloat();
tab = payload.GetRange(32, 4);
float yConsigne = tab.GetFloat();
tab = payload.GetRange(36, 4);
float thetaConsigne = tab.GetFloat();
//On envois l'event aux abonnés
On4WheelsPolarPidErrorCorrectionConsigneDataFromRobot(timeStamp, xError, yError, thetaError, xCorrection, yCorrection, thetaCorrection, xconsigne, yConsigne, thetaConsigne);
}
break;
case (short)Commands.R2PC_4WheelsSpeedIndependantPidCommandErrorCorrectionConsigne:
{
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float M1Error = tab.GetFloat();
tab = payload.GetRange(8, 4);
float M2Error = tab.GetFloat();
tab = payload.GetRange(12, 4);
float M3Error = tab.GetFloat();
tab = payload.GetRange(16, 4);
float M4Error = tab.GetFloat();
tab = payload.GetRange(20, 4);
float M1Correction = tab.GetFloat();
tab = payload.GetRange(24, 4);
float M2Correction = tab.GetFloat();
tab = payload.GetRange(28, 4);
float M3Correction = tab.GetFloat();
tab = payload.GetRange(32, 4);
float M4Correction = tab.GetFloat();
tab = payload.GetRange(36, 4);
float M1Consigne = tab.GetFloat();
tab = payload.GetRange(40, 4);
float M2Consigne = tab.GetFloat();
tab = payload.GetRange(44, 4);
float M3Consigne = tab.GetFloat();
tab = payload.GetRange(48, 4);
float M4Consigne = tab.GetFloat();
//On envois l'event aux abonnés
On4WheelsSpeedIndependantPidDebugDataFromRobot(timeStamp,
M1Error, M2Error, M3Error, M4Error,
M1Correction, M2Correction, M3Correction, M4Correction,
M1Consigne, M2Consigne, M3Consigne, M4Consigne);
}
break;
case (short)Commands.R2PC_2WheelsSpeedPolarPidCommandErrorCorrectionConsigne:
{
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float xError = tab.GetFloat();
tab = payload.GetRange(8, 4);
float thetaError = tab.GetFloat();
tab = payload.GetRange(12, 4);
float xCorrection = tab.GetFloat();
tab = payload.GetRange(16, 4);
float thetaCorrection = tab.GetFloat();
tab = payload.GetRange(20, 4);
float xconsigne = tab.GetFloat();
tab = payload.GetRange(24, 4);
float thetaConsigne = tab.GetFloat();
//On envois l'event aux abonnés
On2WheelsPolarPidErrorCorrectionConsigneDataFromRobot(timeStamp, xError, thetaError, xCorrection, thetaCorrection, xconsigne, thetaConsigne);
}
break;
case (short)Commands.R2PC_2WheelsSpeedIndependantPidCommandErrorCorrectionConsigne:
{
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float M1Error = tab.GetFloat();
tab = payload.GetRange(8, 4);
float M2Error = tab.GetFloat();
tab = payload.GetRange(12, 4);
float M1Correction = tab.GetFloat();
tab = payload.GetRange(16, 4);
float M2Correction = tab.GetFloat();
tab = payload.GetRange(20, 4);
float M1Consigne = tab.GetFloat();
tab = payload.GetRange(24, 4);
float M2Consigne = tab.GetFloat();
//On envois l'event aux abonnés
On2WheelsSpeedIndependantPidDebugDataFromRobot(timeStamp,
M1Error, M2Error,
M1Correction, M2Correction,
M1Consigne, M2Consigne);
}
break;
case (short)Commands.R2PC_4WheelsSpeedPolarPidCorrections:
{
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float CorrPx = tab.GetFloat();
tab = payload.GetRange(8, 4);
float CorrIx = tab.GetFloat();
tab = payload.GetRange(12, 4);
float CorrDx = tab.GetFloat();
tab = payload.GetRange(16, 4);
float CorrPy = tab.GetFloat();
tab = payload.GetRange(20, 4);
float CorrIy = tab.GetFloat();
tab = payload.GetRange(24, 4);
float CorrDy = tab.GetFloat();
tab = payload.GetRange(28, 4);
float CorrPTheta = tab.GetFloat();
tab = payload.GetRange(32, 4);
float CorrITheta = tab.GetFloat();
tab = payload.GetRange(36, 4);
float CorrDTheta = tab.GetFloat();
//On envois l'event aux abonnés
On4WheelsSpeedPolarPidCorrectionDataFromRobot(CorrPx, CorrIx, CorrDx, CorrPy, CorrIy, CorrDy, CorrPTheta, CorrITheta, CorrDTheta);
}
break;
case (short)Commands.R2PC_4WheelsSpeedIndependantPidCorrections:
{
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float CorrPM1 = tab.GetFloat();
tab = payload.GetRange(8, 4);
float CorrIM1 = tab.GetFloat();
tab = payload.GetRange(12, 4);
float CorrDM1 = tab.GetFloat();
tab = payload.GetRange(16, 4);
float CorrPM2 = tab.GetFloat();
tab = payload.GetRange(20, 4);
float CorrIM2 = tab.GetFloat();
tab = payload.GetRange(24, 4);
float CorrDM2 = tab.GetFloat();
tab = payload.GetRange(28, 4);
float CorrPM3 = tab.GetFloat();
tab = payload.GetRange(32, 4);
float CorrIM3 = tab.GetFloat();
tab = payload.GetRange(36, 4);
float CorrDM3 = tab.GetFloat();
tab = payload.GetRange(40, 4);
float CorrPM4 = tab.GetFloat();
tab = payload.GetRange(44, 4);
float CorrIM4 = tab.GetFloat();
tab = payload.GetRange(48, 4);
float CorrDM4 = tab.GetFloat();
//On envois l'event aux abonnés
On4WheelsSpeedIndependantPidCorrectionDataFromRobot(CorrPM1, CorrIM1, CorrDM1, CorrPM2, CorrIM2, CorrDM2, CorrPM3, CorrIM3, CorrDM3, CorrPM4, CorrIM4, CorrDM4);
}
break;
case (short)Commands.R2PC_2WheelsSpeedPolarPidCorrections:
{
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float CorrPx = tab.GetFloat();
tab = payload.GetRange(8, 4);
float CorrIx = tab.GetFloat();
tab = payload.GetRange(12, 4);
float CorrDx = tab.GetFloat();
tab = payload.GetRange(16, 4);
float CorrPTheta = tab.GetFloat();
tab = payload.GetRange(20, 4);
float CorrITheta = tab.GetFloat();
tab = payload.GetRange(24, 4);
float CorrDTheta = tab.GetFloat();
//On envois l'event aux abonnés
On2WheelsSpeedPolarPidCorrectionDataFromRobot(CorrPx, CorrIx, CorrDx, CorrPTheta, CorrITheta, CorrDTheta);
}
break;
case (short)Commands.R2PC_2WheelsSpeedIndependantPidCorrections:
{
timeStamp = (uint)(payload[3] | payload[2] << 8 | payload[1] << 16 | payload[0] << 24);
tab = payload.GetRange(4, 4);
float CorrPM1 = tab.GetFloat();
tab = payload.GetRange(8, 4);
float CorrIM1 = tab.GetFloat();
tab = payload.GetRange(12, 4);
float CorrDM1 = tab.GetFloat();
tab = payload.GetRange(16, 4);
float CorrPM2 = tab.GetFloat();
tab = payload.GetRange(20, 4);
float CorrIM2 = tab.GetFloat();
tab = payload.GetRange(24, 4);
float CorrDM2 = tab.GetFloat();
//On envois l'event aux abonnés
On2WheelsSpeedIndependantPidCorrectionDataFromRobot(CorrPM1, CorrIM1, CorrDM1, CorrPM2, CorrIM2, CorrDM2);
}
break;
case (short)Commands.R2PC_MotorsEnableDisableStatus:
bool value = Convert.ToBoolean(payload[0]);
OnEnableDisableMotorsACKFromRobot(value);
break;
case (short)Commands.R2PC_TirEnableDisableStatus:
value = Convert.ToBoolean(payload[0]);
OnEnableDisableTirACKFromRobot(value);
break;
case (short)Commands.R2PC_AsservissementModeStatus:
AsservissementMode asservMode = (AsservissementMode)payload[0];
OnAsservissementModeStatusFromRobot(asservMode);
break;
case (short)Commands.R2PC_SpeedPIDEnableDebugInternalStatus:
value = Convert.ToBoolean(payload[0]);
OnEnableAsservissementDebugDataACKFromRobot(value);
break;
case (short)Commands.R2PC_MotorCurrentMonitoringEnableStatus:
value = Convert.ToBoolean(payload[0]);
OnEnableMotorCurrentACKFromRobot(value);
break;
case (short)Commands.R2PC_EncoderRawMonitoringEnableStatus:
value = Convert.ToBoolean(payload[0]);
OnEnableEncoderRawDataACKFromRobot(value);
break;
case (short)Commands.R2PC_SpeedConsigneMonitoringEnableStatus:
value = Convert.ToBoolean(payload[0]);
OnEnableMotorSpeedConsigneDataACKFromRobot(value);
break;
case (short)Commands.R2PC_PowerMonitoringEnableStatus:
value = Convert.ToBoolean(payload[0]);
OnEnablePowerMonitoringDataACKFromRobot(value);
break;
case (short)Commands.R2PC_ErrorMessage:
string errorMsg = Encoding.UTF8.GetString(payload);
OnErrorTextFromRobot(errorMsg);
break;
default: break;
}
}
public void SetAsservissementMode(AsservissementMode mode)
{
asservissementMode = mode;
switch (asservissementMode)
{
case AsservissementMode.Off2Wheels:
LabelConsigneX.Visibility = Visibility.Hidden;
LabelConsigneTheta.Visibility = Visibility.Hidden;
LabelErreurX.Visibility = Visibility.Hidden;
LabelErreurTheta.Visibility = Visibility.Hidden;
LabelCommandX.Visibility = Visibility.Hidden;
LabelCommandTheta.Visibility = Visibility.Hidden;
LabelConsigneM1.Visibility = Visibility.Hidden;
LabelConsigneM2.Visibility = Visibility.Hidden;
LabelErreurM1.Visibility = Visibility.Hidden;
LabelErreurM2.Visibility = Visibility.Hidden;
LabelCommandM1.Visibility = Visibility.Hidden;
LabelCommandM2.Visibility = Visibility.Hidden;
LabelCorrPX.Visibility = Visibility.Hidden;
LabelCorrPTheta.Visibility = Visibility.Hidden;
LabelCorrIX.Visibility = Visibility.Hidden;
LabelCorrITheta.Visibility = Visibility.Hidden;
LabelCorrDX.Visibility = Visibility.Hidden;
LabelCorrDTheta.Visibility = Visibility.Hidden;
LabelCorrPM1.Visibility = Visibility.Hidden;
LabelCorrPM2.Visibility = Visibility.Hidden;
LabelCorrIM1.Visibility = Visibility.Hidden;
LabelCorrIM2.Visibility = Visibility.Hidden;
LabelCorrDM1.Visibility = Visibility.Hidden;
LabelCorrDM2.Visibility = Visibility.Hidden;
break;
case AsservissementMode.Polar2Wheels:
LabelConsigneX.Visibility = Visibility.Visible;
LabelConsigneTheta.Visibility = Visibility.Visible;
LabelErreurX.Visibility = Visibility.Visible;
LabelErreurTheta.Visibility = Visibility.Visible;
LabelCommandX.Visibility = Visibility.Visible;
LabelCommandTheta.Visibility = Visibility.Visible;
LabelConsigneM1.Visibility = Visibility.Hidden;
LabelConsigneM2.Visibility = Visibility.Hidden;
LabelErreurM1.Visibility = Visibility.Hidden;
LabelErreurM2.Visibility = Visibility.Hidden;
LabelCommandM1.Visibility = Visibility.Hidden;
LabelCommandM2.Visibility = Visibility.Hidden;
LabelCorrPX.Visibility = Visibility.Visible;
LabelCorrPTheta.Visibility = Visibility.Visible;
LabelCorrIX.Visibility = Visibility.Visible;
LabelCorrITheta.Visibility = Visibility.Visible;
LabelCorrDX.Visibility = Visibility.Visible;
LabelCorrDTheta.Visibility = Visibility.Visible;
LabelCorrPM1.Visibility = Visibility.Hidden;
LabelCorrPM2.Visibility = Visibility.Hidden;
LabelCorrIM1.Visibility = Visibility.Hidden;
LabelCorrIM2.Visibility = Visibility.Hidden;
LabelCorrDM1.Visibility = Visibility.Hidden;
LabelCorrDM2.Visibility = Visibility.Hidden;
break;
case AsservissementMode.Independant2Wheels:
LabelConsigneX.Visibility = Visibility.Hidden;
LabelConsigneTheta.Visibility = Visibility.Hidden;
LabelErreurX.Visibility = Visibility.Hidden;
LabelErreurTheta.Visibility = Visibility.Hidden;
LabelCommandX.Visibility = Visibility.Hidden;
LabelCommandTheta.Visibility = Visibility.Hidden;
LabelConsigneM1.Visibility = Visibility.Visible;
LabelConsigneM2.Visibility = Visibility.Visible;
LabelErreurM1.Visibility = Visibility.Visible;
LabelErreurM2.Visibility = Visibility.Visible;
LabelCommandM1.Visibility = Visibility.Visible;
LabelCommandM2.Visibility = Visibility.Visible;
LabelCorrPX.Visibility = Visibility.Hidden;
LabelCorrPTheta.Visibility = Visibility.Hidden;
LabelCorrIX.Visibility = Visibility.Hidden;
LabelCorrITheta.Visibility = Visibility.Hidden;
LabelCorrDX.Visibility = Visibility.Hidden;
LabelCorrDTheta.Visibility = Visibility.Hidden;
LabelCorrPM1.Visibility = Visibility.Visible;
LabelCorrPM2.Visibility = Visibility.Visible;
LabelCorrIM1.Visibility = Visibility.Visible;
LabelCorrIM2.Visibility = Visibility.Visible;
LabelCorrDM1.Visibility = Visibility.Visible;
LabelCorrDM2.Visibility = Visibility.Visible;
break;
default:
break;
}
}
