/******************************************************************/ 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); }
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); }
public void CambiarModo(E_modo modo) { Enviar_comando_Bluetooth(ComandosBluetooth.e_com_cambiar_modo, (int)modo); }