diff --git a/Middlewares/owlware b/Middlewares/owlware index e5a6928..ee55f02 160000 --- a/Middlewares/owlware +++ b/Middlewares/owlware @@ -1 +1 @@ -Subproject commit e5a69282aba578d68f7fb33fe6973eef60d6f9e8 +Subproject commit ee55f02e9db76e4344b55dd508d086d25c9fe6e6 diff --git a/app/inc/SPI_task.h b/app/inc/SPI_task.h new file mode 100644 index 0000000..a15fb01 --- /dev/null +++ b/app/inc/SPI_task.h @@ -0,0 +1,71 @@ +#include "cmsis_os.h" +#include "spi.h" +#ifndef __SPI_task.h__ + #define __SPI_task .h__ + #ifdef __cplusplus +extern "C" { + #endif + +extern SPI_HandleTypeDef hspi2; // esta en el archivo de spi + +typedef struct { + SPI_HandleTypeDef* spiHandle; + GPIO_TypeDef* csPort; + uint16_t csPin; +} SPI_Config; +uint8_t buffer[40]; +float vel_x, vel_y, vel_z; // velocidad variables +float acce_x, acce_y, acce_z; // aceleracion variables +float gir_x, gir_y, gir_z; // giroscopio variable + +/*---------------------------------------------------------------*/ +/*la cosa del semaforo que debo implementar*/ +void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef* hi2c); + +void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef* hi2c); + +static void Task1(void const* argument); + +//*-----------------------------------------------------------------------*/ + +// Funciones para manejar SPI y datos del robot +void SPI_Init(SPI_HandleTypeDef* hspi); // Inicializar SPI +void SPI_ReadData(SPI_HandleTypeDef* hspi, uint8_t* buffer, + uint16_t len); // Leer datos SPI (le puse el nombre de read data) +void SPI_WriteSensor(SPI_HandleTypeDef* hspi, uint8_t* data, uint16_t len); // Escribir datos SPI + +// Declaracion de funciones de SPI +int16_t INU_ReadResgister16(uint8_t reg); + +// Calcular velocidad y aceleración + +// a lo que invesige el metodo de conseguir los datos +void ReadData(float* x1, float* y1, float* z1, float* x2, float* y2, float* z2) { + // correccion de las cosas del buffer + uint8_t command = 0x02; + uint8_t buffer[40]; + + // Enviar comando + uint8_t txData = 0x02; + uint8_t rxData; + HAL_SPI_TransmitReceive(&hspi2, &txData, &rxData, 1, HAL_MAX_DELAY); + + // Leer 40 bytes de datos + for (int i = 0; i < 40; i++) { + txData = 0x00; // Enviar datos vacíos para recibir respuesta + HAL_SPI_TransmitReceive(&hspi2, &txData, &buffer[i], 1, HAL_MAX_DELAY); + } + + // Convertir los datos en valores float + float* x1 = (float*)&buffer[0]; + float* y1 = (float*)&buffer[4]; + float* z1 = (float*)&buffer[8]; + float* x2 = (float*)&buffer[12]; + float* y2 = (float*)&buffer[16]; + float* z2 = (float*)&buffer[20]; + + #ifdef __cplusplus +} + #endif + +#endif /* __SPI_task.h__ */ \ No newline at end of file diff --git a/app/src/SPI_task.c b/app/src/SPI_task.c new file mode 100644 index 0000000..8a54987 --- /dev/null +++ b/app/src/SPI_task.c @@ -0,0 +1,135 @@ + + +#ifndef SPI_task_C +#define SPI_task_C + +#ifdef __cplusplus +extern "C" { +#endif + +#include "SPI_task.h" + +#include +#include + +#include "main.h" + +// unsigned char buffer_spi[40]; // esta en bytes este buffer solo sera para el spi ya tenemos uno en el task.h +// es un array para guardar los datos del acelerometro y giroscopio en memoria + +SPI_Config SPI1_Config = {.spiHandle = &hspi2, .csPort = SPI1_CS_GPIO_Port, .csPin = SPI1_CS_Pin}; + +// funcion para leer un registro de 16 bits +uint16_t todos_addres() { + for (int i = 0; i < 128; i++) { + uint16_t data = IMU_ReadResgister16(i); + if (data != 0) { + // printf("0x %02X: 0x%02X\n", i, data); + } + } + return 0; +} + +// las cosas del semaforo +osSemaphoreId I2C_semaphore; // define el semaforo +osSemaphoreDef(I2C_semaphore); // define la estructura del semaforo + +void initSemaphore(void) { + I2C_semaphore = osSemaphoreCreate(osSemaphore(I2C_semaphore), 1); // Crear semáforo con 1 permiso + if (I2C_semaphore == NULL) { + } +} + +I2C_HandleTypeDef hi2c1; + +void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef* hi2c) { + if (hi2c->State == HAL_I2C_STATE_READY) { + osSemaphoreRelease(I2C_semaphore); + } +} +// proceso de empaquetados de datos con los datos del acelerometro y giroscopio que recibimos +// creacion strucct +typedef struct { + float vel_x, vel_y, vel_z; // velocidad variables + float ace_x, ace_y, ace_z; // aceleracion variables + float gir_x, gir_y, gir_z; // giroscopio variable +} IMU_Data_t; + +// Definir la cola +osMessageQId imuQueue; +#define QUEUE_SIZE 10 + +// Definir la estructura de la cola +osMessageQDef(imuQueue, QUEUE_SIZE, IMU_Data_t); + +// Pool de memoria para los datos del IMU +IMU_Data_t imuDataPool[QUEUE_SIZE]; +uint8_t poolIndex = 0; + +// Función para inicializar la cola +void initQueue(void) { + imuQueue = osMessageCreate(osMessageQ(imuQueue), NULL); + if (imuQueue == NULL) { + // Manejar el error si la cola no se pudo crear + } +} + +// funcion para inicializar el SPI +void ProcesarDatos(void) { + // Leer datos del acelerómetro + int16_t ax = IMU_ReadResgister16(0x3B); // Eje X + int16_t ay = IMU_ReadResgister16(0x3D); // Eje Y + int16_t az = IMU_ReadResgister16(0x3F); // Eje Z + + // Leer datos del giroscopio + int16_t gx = IMU_ReadResgister16(0x43); // Eje X + int16_t gy = IMU_ReadResgister16(0x45); // Eje Y + int16_t gz = IMU_ReadResgister16(0x47); // Eje Z + + // convertir los datos a unidade fisicas + float x1g = ax * (9.81f / 16384.0f); + float y1g = ay * (9.81f / 16384.0f); + float z1g = az * (9.81f / 16384.0f); + // quedan pendiente de asignacion para su uso + float x2g = gx * (9.81f / 16384.0f); + float y2g = gy * (9.81f / 16384.0f); + float z2g = gz * (9.81f / 16384.0f); + + // Almacenar los datos en el struct + IMU_Data_t* imuData = &imuDataPool[poolIndex]; + imuData->ace_x = x1g; + imuData->ace_y = y1g; + imuData->ace_z = z1g; + imuData->gir_x = x2g; + imuData->gir_y = y2g; + imuData->gir_z = z2g; + + // Enviar los datos a la cola + osStatus status = osMessagePut(imuQueue, (uint32_t)imuData, 0); + if (status != osOK) { + // Manejar el error si no se pudo enviar el mensaje + } + + // Actualizar el índice del pool de memoria + poolIndex = (poolIndex + 1) % QUEUE_SIZE; +} +void Task_ProcessIMUData(void const* argument) { + while (1) { + // Esperar a recibir un mensaje de la cola + osEvent event = osMessageGet(imuQueue, osWaitForever); + if (event.status == osEventMessage) { + // Obtener el puntero al struct + IMU_Data_t* imuData = (IMU_Data_t*)event.value.v; + + // Procesar los datos recibidos + // printf("Accel X: %f, Accel Y: %f, Accel Z: %f\n", imuData->ace_x, imuData->ace_y, imuData->ace_z); + // printf("Gyro X: %f, Gyro Y: %f, Gyro Z: %f\n", imuData->gir_x, imuData->gir_y, imuData->gir_z); + } + } +} + +#ifdef __cplusplus +} +#endif + +#endif /* SPI_task.C */ diff --git a/app/src/main.c b/app/src/main.c index 5e2955a..34beac2 100644 --- a/app/src/main.c +++ b/app/src/main.c @@ -26,8 +26,9 @@ // #include // #include // CMSIS Include +#include "SPI_task.h" #include "cmsis_os.h" - +#include "spi.h" /* Standard includes. */ #include @@ -40,15 +41,25 @@ /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ +osMessageQId imuQueue; // Declare the imuQueue variable /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); - -typedef enum { THREAD_1 = 0, THREAD_2 } Thread_TypeDef; +typedef enum { THREAD_ID_1 = 0, THREAD_ID_2 = 1, TASK1_ID = 2 } Thread_TypeDef; +void Task_ProcessIMUData(void const* argument); // Add this line to declare the function osThreadId LEDThread1Handle; +osThreadId tid_thread1; +osThreadId tid_thread2; +osThreadId tid_task1; +osThreadId id1; static void BlinkyThread(void const* argument); +static void thread1(void const* argument); +static void thread2(void const* argument); +// static void Task1(void const* argument); // por si acaso + +// Definición de la macro osThreadDef /*-----------------------------------------------------------*/ // osThreadId_t defaultTaskHandle; @@ -59,6 +70,7 @@ static void BlinkyThread(void const* argument); // }; // void BlinkyThread(void* argument); /*-----------------------------------------------------------*/ + static void BlinkyThread(void const* argument) { HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_RESET); while (1) { @@ -87,6 +99,45 @@ static void BlinkyThread(void const* argument) { * @brief The application entry point. * @retval int */ + +/* Definición de los hilos */ +// el compilador no reconoce la macro +osThreadDef(THREAD_ID_1, thread1, osPriorityHigh, 1, 128); // Define el hilo thread1 +osThreadDef(THREAD_ID_2, thread2, osPriorityNormal, 1, 128); // Define el hilo thread2 +osThreadDef(TASK1_ID, Task1, osPriorityNormal, 1, 128); // Define el hilo Task1 + +osSemaphoreId semaphore; // Semaphore ID +osSemaphoreDef(semaphore); // Semaphore definition + +void thread1(void const* argument) { + int32_t value; + while (1) { + osDelay(3); // Pass control to other tasks for 3ms + value = osSemaphoreWait(semaphore, 1); // Wait 1ms for the free semaphore + if (value > 0) { + // Si no hubo timeout, el semáforo fue adquirido + // Usar el recurso aquí + osSemaphoreRelease(semaphore); // Liberar el semáforo + } + } +} + +// thread 2 - Normal Priority - looks for a free semaphore and uses the resource whenever it is available +void thread2(void const* argument) { + while (1) { + osSemaphoreWait(semaphore, osWaitForever); // Esperar indefinidamente por el semáforo + // Usar el recurso aquí + osSemaphoreRelease(semaphore); // Liberar el semáforo + } +} +void Task_ProcessIMUData(void const* argument) { + // Implement the task to process IMU data here + while (1) { + // Process IMU data + osDelay(100); // Adjust the delay as necessary + } +} + int main(void) { /* MCU Configuration--------------------------------------------------------*/ @@ -103,18 +154,29 @@ int main(void) { MX_TIM3_Init(); MX_TIM4_Init(); MX_USART2_UART_Init(); + MX_SPI1_Init(); // lo copie directo spi.c esta declarado como una funcion ahi pero dice que no esta definida + + MX_SPI1_Init(); + osKernelInitialize(); + + /* Creación de los hilos */ + tid_thread1 = osThreadCreate(osThread(THREAD_ID_1), NULL); // Crea el hilo thread1 + tid_thread2 = osThreadCreate(osThread(THREAD_ID_2), NULL); // Crea el hilo thread2 + tid_task1 = osThreadCreate(osThread(TASK1_ID), NULL); // Crea el hilo Task1 + + // ahora toca la cola + // Inicializar la cola + osMessageQDef(QUEUE_SIZE, 16, uint32_t); // Define the message queue + imuQueue = osMessageCreate(osMessageQ(QUEUE_SIZE), NULL); + if (imuQueue == NULL) { + // Manejar el error si la cola no se pudo crear + } - /* Infinite loop */ - // osKernelInitialize(); - - // Create the Blinky thread - osThreadDef(THREAD_1, BlinkyThread, osPriorityNormal, 0, configMINIMAL_STACK_SIZE); - - LEDThread1Handle = osThreadCreate(osThread(THREAD_1), NULL); - - // Start the RTOS kernel - osKernelStart(); + // Crear la tarea para procesar los datos del IMU + osThreadDef(Task_ProcessIMUData, Task_ProcessIMUData, osPriorityNormal, 0, 128); + osThreadCreate(osThread(Task_ProcessIMUData), NULL); + osKernelStart(); // Iniciar el scheduler del RTOS v1 // This is a fake comment, delete for (;;) { /* Should not reach here. */ @@ -140,8 +202,8 @@ void SystemClock_Config(void) { RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { /* Initialization Error */ - while (1) - ; + while (1) { + }; } /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 @@ -154,8 +216,8 @@ void SystemClock_Config(void) { RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { /* Initialization Error */ - while (1) - ; + while (1) { + }; } } diff --git a/robotConfig b/robotConfig index 4eb10cf..764c826 160000 --- a/robotConfig +++ b/robotConfig @@ -1 +1 @@ -Subproject commit 4eb10cf505c67e5eef39c2dd317c92f00899dd89 +Subproject commit 764c826e2f26e1d07ee84755f5f9cb2b6011264f