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;
}
}
int ReconocerComando()
{
/*
#define LENGH_RESPUESTAS_BLUETOOTH_DATOS_COCHE 26
#define LENGH_RESPUESTAS_BLUETOOTH_DATOS_IMU 65
#define LENGH_RESPUESTAS_BLUETOOTH_VALORES_CALIBRA 63
#define LENGH_RESPUESTAS_BLUETOOTH_VALORESMANUAL 13
#define LENGH_RESPUESTAS_BLUETOOTH_ESTADO 9*/
if (bytesRecibidos <= 3)
{
return(50 - 1); // aun no se ha recibido el ID de comando(NOTA al recibirlo en el array se suma por tanto el indice ha de ser 4 o mas para q este leido), suponer longitud maxima
}
RespuestasBluetooth comando = (RespuestasBluetooth)bufferEntradaSerie[2];
//todos los comandos basicos tienen un solo byte de datos
/*
* if (comando == RespuestasBluetooth.e_res_msg || comando == RespuestasBluetooth.e_res_auto_imu || comando == RespuestasBluetooth.e_res_auto_parametros || comando == RespuestasBluetooth.e_res_cambiar_modo || comando == RespuestasBluetooth.e_res_cambio_ok || comando == RespuestasBluetooth.e_res_control_remoto) // recepcion estado
* {
* return 6;
* }
*/
if (comando == RespuestasBluetooth.e_res_datos_coche) //recepcion datos
{
return(26);
}
if (comando == RespuestasBluetooth.e_res_datosn_imu) //recepcion msg
{
return(65);
}
if (comando == RespuestasBluetooth.e_res_datos_potencia) //recepcion msg
{
return(15);
}
if (comando == RespuestasBluetooth.e_res_datos_ESC) //recepcion msg
{
return(14);
}
if (comando == RespuestasBluetooth.e_res_valores_calibracion) //recepcion calibracion
{
return(63);
}
if (comando == RespuestasBluetooth.e_res_valores_manual) //recepcion msg
{
return(13);
}
if (comando == RespuestasBluetooth.e_res_estado) //recepcion msg
{
return(9);
}
//todos los comandos basicos tienen un solo byte de datos
return(6);
}
