private void ProcesarComandoSerie()
{
int dato;
int posBuffer = 3;
RespuestasBluetooth comandoChar = (RespuestasBluetooth)bufferEntradaSerie[2];
switch (comandoChar)
{
case RespuestasBluetooth.e_res_pong:
tiempoRespuesta = (DateTime.Now - initPing).TotalMilliseconds;
break;
case RespuestasBluetooth.e_res_estado: //recepcion nuevo estado
modo_motor_actual = (E_modo)bufferEntradaSerie[3];
tipo_control_actual = (E_tipo_control)bufferEntradaSerie[4];
tipo_regulacion_direccion = (E_regulacion_direccion)bufferEntradaSerie[5];
tipo_regulacion_potencia = (E_regulacion_potencia)bufferEntradaSerie[6];
OnRecividosEstado(modo_motor_actual, tipo_control_actual, tipo_regulacion_direccion, tipo_regulacion_potencia);
LogearMSG(String.Format(string.Format("Recibido nuevo estado del sistema: motor: {0} control: {1} direccion: {2} potencia {3}", modo_motor_actual, tipo_control_actual, tipo_regulacion_direccion, tipo_regulacion_potencia)));
break;
case RespuestasBluetooth.e_res_datos_coche: //recepcion medida
posBuffer = 3;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
ConsignaMotor = (dato) / 100.0f;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
ConsignaRuedaDerecha = dato / 100.0f;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
ConsignaRuedaIzquierda = dato / 100.0f;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
ConsignaMarcha = dato / 100.0f;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
ESC_VoltajeEntrada = dato / 10.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 8);
ESC_rpmActual = dato * 10.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 8);
ESC_avgMotorCurrent = dato / 100.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 8);
ESC_avgInputCurrent = dato / 100.0f;
dato = (bufferEntradaSerie[posBuffer++] & 0xff);
ESC_Dutycycle = dato;
dato = (bufferEntradaSerie[posBuffer++] & 0xff);
modo_motor_actual = (E_modo)dato;
dato = (bufferEntradaSerie[posBuffer++] & 0xff);
tipo_control_actual = (E_tipo_control)dato;
dato = (bufferEntradaSerie[posBuffer++] & 0xff);
tipo_regulacion_direccion = (E_regulacion_direccion)dato;
dato = (bufferEntradaSerie[posBuffer++] & 0xff);
tipo_regulacion_potencia = (E_regulacion_potencia)dato;
OnRecividosDatosCoche(ConsignaMarcha, ConsignaMotor, ConsignaRuedaDerecha, ConsignaRuedaIzquierda, ESC_VoltajeEntrada, ESC_rpmActual, ESC_avgInputCurrent, ESC_avgInputCurrent, ESC_Dutycycle, modo_motor_actual, tipo_control_actual, tipo_regulacion_direccion, tipo_regulacion_potencia);
// LogearMSG(String.Format(string.Format(@"RPM: {0} RuedaDer {1} RuedaIzq {2} Marcha {3} ESC_Voltaje {4} ESC_rpm {5} ESC_avgMotorCurrent {6} ESC_avgInputCurrent {7} ESC_Dutycycle {8}",modo_motor_actual = dato;
// ConsignaRPMActual, ConsignaDireccionActual, AnguloRuedaDerecha, AnguloRuedaIzquierda, ESC_VoltajeEntrada, ESC_rpmActual, ESC_avgMotorCurrent, ESC_avgInputCurrent, ESC_Dutycycle)));
break;
case RespuestasBluetooth.e_res_datos_ESC: //recepcion medida
posBuffer = 3;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
ESC_VoltajeEntrada = dato / 10.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 8);
ESC_rpmActual = dato * 10.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 8);
ESC_avgMotorCurrent = dato / 100.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 8);
ESC_avgInputCurrent = dato / 100.0f;
dato = (bufferEntradaSerie[posBuffer++] & 0xff);
ESC_Dutycycle = dato;
OnRecividosDatoESC(ESC_VoltajeEntrada, ESC_rpmActual, ESC_avgInputCurrent, ESC_avgInputCurrent, ESC_Dutycycle);
break;
case RespuestasBluetooth.e_res_datos_potencia: //recepcion medida
posBuffer = 3;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
Ina_busvoltage = dato / 1000.0f;
//dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 8);
//Ina_loadvoltage = dato / 1000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
Ina_current_mA = dato / 10.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
Ina_power_mW = dato / 10.0f;
OnRecividosDatosPotencia(Ina_busvoltage, Ina_current_mA, Ina_loadvoltage, Ina_power_mW);
break;
case RespuestasBluetooth.e_res_datosn_imu: //recepcion medida
posBuffer = 3;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
pitch = (dato) / 1000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
roll = dato / 1000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
yaw = dato / 1000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
gyro[0] = dato;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
gyro[1] = dato;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
gyro[2] = dato;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
aceleracion[0] = dato;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
aceleracion[1] = dato;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
aceleracion[2] = dato;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
angulos[0] = dato / 10000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
angulos[1] = dato / 10000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
angulos[2] = dato / 10000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
angulos[3] = dato / 10000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
bar_temperatura = dato / 1000.0f;
dato = (bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8) | ((bufferEntradaSerie[posBuffer++]) << 16) | ((bufferEntradaSerie[posBuffer++]) << 24);
bar_presion = dato / 1000.0f;
OnRecividosDatosIMU(pitch, roll, yaw, gyro, aceleracion, angulos, imu_temp, bar_temperatura, bar_presion);
//LogearMSG(String.Format(string.Format("medida: {0} {1} {2} {3} {4}", pitch, roll, yaw, velocidad[0], tipo_control)));
break;
case RespuestasBluetooth.e_res_msg: // Mensajes
dato = bufferEntradaSerie[3];
if (dato == 1)
{
LogearMSG(String.Format(string.Format("{0} {1}", "Respuesta de error 'comando no reconocido' de Bluetooth", comandoChar)));
}
else if (dato == 2)
{
LogearMSG(String.Format(string.Format("{0} {1}", "Respuesta de error, recibido fin de cadena no esperado", comandoChar)));
}
else if (dato == 3)
{
LogearMSG(String.Format(string.Format("{0} {1}", "Respuesta de error 'comando demasiado largo' de Bluetooth", comandoChar)));
}
else if (dato == 4)
{
LogearMSG(String.Format(string.Format("{0} {1}", "-", comandoChar)));
}
else if (dato == 5)
{
LogearMSG(String.Format(string.Format("{0} {1}", "-", comandoChar)));
}
else if (dato == 6)
{
LogearMSG(String.Format(string.Format("{0} {1}", "Iniciado ciclo rapido", comandoChar)));
}
else if (dato == 7)
{
LogearMSG(String.Format(string.Format("{0} {1}", "Nueva configuracion de calibracion recibida", comandoChar)));
}
else if (dato == 8)
{
LogearMSG(String.Format(string.Format("{0} {1}", "Activado envio automatico de datos del coche", comandoChar)));
}
else if (dato == 9)
{
LogearMSG(String.Format(string.Format("{0} {1}", "Activado envio automatico de telemetria", comandoChar)));
}
else if (dato == 10)
{
// LogearMSG(String.Format(string.Format("{0} {1}", "Nuevos valores manuales OK", comandoChar)));
}
else if (dato == 11)
{
LogearMSG(String.Format(string.Format("{0} {1}", "Cambio modo OK", comandoChar)));
}
else
{
LogearMSG(String.Format(string.Format("{0} {1}", "Recibido msg: ", dato)));
}
break;
case RespuestasBluetooth.e_res_valores_calibracion: //Valores de calibracion recibidos
float[] parRecibido = new float[19];
posBuffer = 3;
for (int indiPar = 1; indiPar <= 14; indiPar++)
{
dato = (bufferEntradaSerie[posBuffer++] & 0xff) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 8) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 16) | ((bufferEntradaSerie[posBuffer++] & 0xff) << 24);
parRecibido[indiPar] = (dato) / 1000.0f;
}
int x, verHight, verLow;
x = (bufferEntradaSerie[posBuffer++] & 0xff);
verHight = (bufferEntradaSerie[posBuffer++] & 0xff);
verLow = (bufferEntradaSerie[posBuffer++] & 0xff);
OnRecividosDatosCalibracion(parRecibido, x, verHight, verLow);
break;
case RespuestasBluetooth.e_res_valores_manual: //Valores de manual recibidos
posBuffer = 3;
float[] valores = new float[4];
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
valores[0] = (dato) / 10.0f;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
valores[1] = (dato) / 10.0f;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
valores[2] = (dato) / 10.0f;
dato = (short)((bufferEntradaSerie[posBuffer++]) | ((bufferEntradaSerie[posBuffer++]) << 8));
valores[3] = (dato) / 10.0f;
OnRecividosDatosManual(valores);
break;
default:
LogearMSG(String.Format(string.Format("{0} {1}", "Error comando no reconocido recibido", comandoChar)));
break;
}
}
/******************************************************************/
private void OnRecividosDatosCoche(float p_ConsignaMarcha, float p_ConsignaRPMActual, float p_AnguloRuedaDerecha, float p_AnguloRuedaIzquierda, float p_ESC_VoltajeEntrada, float p_ESC_rpmActual, float p_ESC_avgMotorCurrent, float p_ESC_avgInputCurrent, float p_ESC_Dutycycle, E_modo p_modo_motor_actual, E_tipo_control p_tipo_control_actual, E_regulacion_direccion p_tipo_regulacion_direccion, E_regulacion_potencia p_tipo_regulacion_potencia)
{
DatosCocheEventArgs parametros = new DatosCocheEventArgs()
{
ConsignaMarcha = p_ConsignaMarcha,
ConsignaMotor = p_ConsignaRPMActual,
ConsignaRuedaDerecha = p_AnguloRuedaDerecha,
ConsignaRuedaIzquierda = p_AnguloRuedaIzquierda,
ESC_VoltajeEntrada = p_ESC_VoltajeEntrada,
ESC_avgInputCurrent = p_ESC_avgInputCurrent,
ESC_avgMotorCurrent = p_ESC_avgMotorCurrent,
ESC_Dutycycle = p_ESC_Dutycycle,
ESC_rpmActual = p_ESC_rpmActual,
Modo_motor_actual = p_modo_motor_actual,
Tipo_control_actual = p_tipo_control_actual,
Tipo_regulacion_direccion = p_tipo_regulacion_direccion,
Tipo_regulacion_potencia = p_tipo_regulacion_potencia,
Time = DateTime.UtcNow
};
RecividosDatosCocheEvent?.Invoke(this, parametros);
}
/******************************************************************/
private void OnRecividosEstado(E_modo p_modo_motor_actual, E_tipo_control p_tipo_control_actual, E_regulacion_direccion p_tipo_regulacion_direccion, E_regulacion_potencia p_tipo_regulacion_potencia)
{
DatosEstadoEventArgs parametros = new DatosEstadoEventArgs()
{
Modo_motor_actual = p_modo_motor_actual,
Tipo_control_actual = p_tipo_control_actual,
Tipo_regulacion_direccion = p_tipo_regulacion_direccion,
Tipo_regulacion_potencia = p_tipo_regulacion_potencia
};
RecividosEstado?.Invoke(this, parametros);
}
public void CambiarControlDireccion(E_regulacion_direccion modo)
{
Enviar_comando_Bluetooth(ComandosBluetooth.e_com_control_dirrecion, (int)modo);
}
