diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index b0c110a..abd8bdf 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -30,12 +30,20 @@ /* Includes ------------------------------------------------------------------*/ #include "stm32f4xx_it.h" #include "main.h" - +#include "gimbal_task.h" #include "USART_comms.h" +#include +#include -volatile char buffer_rx[100]; +volatile char buffer_rx[BUFFER_SIZE]; //holds 100 bytes int count_rx = 0; +volatile char UART8_buffer_rx[100]; +int UART8_count_rx = 0; + +char rando[] = "hello\n\r"; +char new_line[] = "\n\r"; +int atoi(const char *str); /** @addtogroup Template_Project * @{ */ @@ -171,10 +179,11 @@ void SysTick_Handler(void) /** * @} */ - + // Interrupt handler for USART 6. This is called on the reception of every // byte on USART6 +// Note: USART_Data only holds 1 byte of data despite being a uint16_t variable void USART6_IRQHandler(void) { // make sure USART6 was intended to be called for this interrupt @@ -206,4 +215,48 @@ void USART6_IRQHandler(void) } +void clear_gimbal_buffer(void) { + Gimbal_Angles *gimbal_angles = get_gimbal_angle_struct(); + + int angle = atoi(gimbal_angles->buffer_rx); + + if (angle<-180) angle=-180; + else if (angle>180) angle=180; + + gimbal_angles->yaw_angle = angle; + gimbal_angles->new_angle_flag = 1; + gimbal_angles->count_rx = 0; + + for (int i = 0; i < BUFFER_SIZE; i++) { + gimbal_angles->buffer_rx[i] = 0; + } +} + +// Interrupt handler for UART 8. This is called on the reception of every +// byte on UART8 +// Note: USART_Data only holds 1 byte of data despite being a uint16_t variable +void UART8_IRQHandler(void) +{ + // make sure UART8 was intended to be called for this interrupt + if(USART_GetITStatus(UART8, USART_IT_RXNE) != RESET) { + uint16_t UART_Data = USART_ReceiveData(UART8); + Gimbal_Angles *gimbal_angles = get_gimbal_angle_struct(); + + // Once a carriage return is read or the buffer full, move + // data into struct, and clear the buffer + if (UART_Data == 13 || gimbal_angles->count_rx >= BUFFER_SIZE-1) { + //vision_send_string(gimbal_angles->buffer_rx); //echo + //vision_send_string(new_line); + clear_gimbal_buffer(); + //vision_send_string(gimbal_angles->buffer_rx); //echo + //vision_send_string(new_line); + } + else { + gimbal_angles->buffer_rx[gimbal_angles->count_rx++] = UART_Data; + } + } +} + + + /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/CMSIS/system_stm32f4xx.c b/CMSIS/system_stm32f4xx.c index 8fe6888..6076ea4 100644 --- a/CMSIS/system_stm32f4xx.c +++ b/CMSIS/system_stm32f4xx.c @@ -34,9 +34,9 @@ through STLINK MCO pin of STM32F103 microcontroller. The frequency cannot be changed and is fixed at 8 MHz. Hardware configuration needed for Nucleo Board: - – SB54, SB55 OFF - – R35 removed - – SB16, SB50 ON */ + „1¤7 SB54, SB55 OFF + „1¤7 R35 removed + „1¤7 SB16, SB50 ON */ /* #define USE_HSE_BYPASS */ #if defined(USE_HSE_BYPASS) @@ -180,7 +180,7 @@ void SystemInit(void) * frequency in the chip. It is calculated based on the predefined * constant and the selected clock source: * - * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * - If source is HSI, SystemCoreClock will contain the HSI_VALUE(*) * * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) * diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index 393bc45..fd6731a 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -11,6 +11,7 @@ 0x4 ARM-ADS 5060422::V5.06 update 4 (build 422)::ARMCC + 0 STM32F427IIHx @@ -183,6 +184,7 @@ 0 0 2 + 0 1 0 8 @@ -323,6 +325,7 @@ 0 0 1 + 0 0 3 3 @@ -335,7 +338,7 @@ STM32F427_437xx,USE_STDPERIPH_DRIVER,__FPU_USED,__FPU_PRESENT,ARM_MATH_CM4,__CC_ARM,ARM_MATH_MATRIX_CHECK,ARM_MATH_ROUNDING - ..\CMSIS;..\FWLIB\inc;..\User;..\User\AHRS;..\User\DSP\Include;..\User\FreeRTOS\include;..\User\FreeRTOS\portable;..\User\FreeRTOS\portable\RVDS\ARM_CM4F;..\user\user_lib;..\User\hardware\ADC;..\User\hardware\BUZZER;..\User\hardware\delay;..\User\hardware\EXIT_Init;..\User\hardware\CAN;..\User\hardware\LED;..\User\hardware\FLASH;..\User\hardware\FRIC;..\User\hardware\LASER;..\User\hardware\POWER_CTRL;..\User\hardware\RC;..\User\hardware\RNG;..\User\hardware\SPI;..\User\hardware\SYS;..\user\hardware\timer;..\User\APP\CAN_Receive;..\User\APP\FreeRTOS_Middleware;..\User\APP\pid;..\User\APP\Remote_Control;..\User\TASK\start_task;..\User\APP\USART_comms;..\User\hardware\usart;..\User\TASK\revolver_task;..\User\TASK\INS_task;..\User\TASK\chassis_task;..\User\TASK\gimbal_task;..\User\TASK\shoot_task + ..\CMSIS;..\FWLIB\inc;..\User;..\User\AHRS;..\User\DSP\Include;..\User\FreeRTOS\include;..\User\FreeRTOS\portable;..\User\FreeRTOS\portable\RVDS\ARM_CM4F;..\user\user_lib;..\User\hardware\ADC;..\User\hardware\BUZZER;..\User\hardware\delay;..\User\hardware\EXIT_Init;..\User\hardware\CAN;..\User\hardware\LED;..\User\hardware\FLASH;..\User\hardware\FRIC;..\User\hardware\LASER;..\User\hardware\POWER_CTRL;..\User\hardware\RC;..\User\hardware\RNG;..\User\hardware\SPI;..\User\hardware\SYS;..\user\hardware\timer;..\User\APP\CAN_Receive;..\User\APP\FreeRTOS_Middleware;..\User\APP\pid;..\User\APP\Remote_Control;..\User\TASK\start_task;..\User\APP\USART_comms;..\User\hardware\usart;..\User\TASK\revolver_task;..\User\TASK\INS_task;..\User\TASK\chassis_task;..\User\TASK\gimbal_task;..\User\TASK\shoot_task;..\User\APP\vision diff --git a/Project/vision.c b/Project/vision.c new file mode 100644 index 0000000..e69de29 diff --git a/Project/vision.h b/Project/vision.h new file mode 100644 index 0000000..e69de29 diff --git a/user/APP/CAN_receive/CAN_receive.c b/user/APP/CAN_receive/CAN_receive.c index 8fe62cf..8307ffc 100644 --- a/user/APP/CAN_receive/CAN_receive.c +++ b/user/APP/CAN_receive/CAN_receive.c @@ -35,17 +35,26 @@ /******************** Private Defines for Receiving ********************/ -#define get_motor_measure(ptr, rx_message) \ +#define fill_motor_readings(ptr, rx_message) \ { \ (ptr)->last_ecd = (ptr)->ecd; \ (ptr)->ecd = (uint16_t)((rx_message)->Data[0] << 8 | (rx_message)->Data[1]); \ (ptr)->speed_rpm = (uint16_t)((rx_message)->Data[2] << 8 | (rx_message)->Data[3]); \ - (ptr)->given_current = (uint16_t)((rx_message)->Data[4] << 8 | (rx_message)->Data[5]); \ + (ptr)->current_read = (uint16_t)((rx_message)->Data[4] << 8 | (rx_message)->Data[5]); \ (ptr)->temperate = (rx_message)->Data[6]; \ } - - /// TODO: Make similar get_motor_measure for M3508 and P36 +#define fill_gimbal_motor_readings(ptr, rx_message) \ + { \ + (ptr)->last_ecd = (ptr)->ecd; \ + (ptr)->ecd = (uint16_t)((rx_message)->Data[0] << 8 | (rx_message)->Data[1]); \ + (ptr)->current_read = (uint16_t)((rx_message)->Data[2] << 8 | (rx_message)->Data[3]); \ + (ptr)->speed_rpm = (uint16_t)((rx_message)->Data[4] << 8 | (rx_message)->Data[5]); \ + (ptr)->temperate = (rx_message)->Data[6]; \ + } + // TODO: Check real message format -> most likely delete the gimbal version + // The ordering of current_read and speed_rpm in the documentation and the dji code do not match + @@ -254,22 +263,22 @@ static void CAN_hook(CanRxMsg *rx_message) { case CAN_YAW_MOTOR_ID: { - get_motor_measure(&motor_yaw, rx_message); + fill_gimbal_motor_readings(&motor_yaw, rx_message); break; } case CAN_PIT_MOTOR_ID: { - get_motor_measure(&motor_pit, rx_message); + fill_gimbal_motor_readings(&motor_pit, rx_message); break; } case CAN_TRIGGER_MOTOR_ID: { - get_motor_measure(&motor_trigger, rx_message); + fill_motor_readings(&motor_trigger, rx_message); break; } case CAN_HOPPER_MOTOR_ID: { - get_motor_measure(&motor_hopper, rx_message); + fill_motor_readings(&motor_hopper, rx_message); break; } case CAN_3508_M1_ID: @@ -279,7 +288,7 @@ static void CAN_hook(CanRxMsg *rx_message) { static uint8_t i = 0; i = rx_message->StdId - CAN_3508_M1_ID; //get motor ID - get_motor_measure(&motor_chassis[i], rx_message); //Use motor ID as index of array + fill_motor_readings(&motor_chassis[i], rx_message); //Use motor ID as index of array break; } diff --git a/user/APP/CAN_receive/CAN_receive.h b/user/APP/CAN_receive/CAN_receive.h index 661fb2d..c4c313b 100644 --- a/user/APP/CAN_receive/CAN_receive.h +++ b/user/APP/CAN_receive/CAN_receive.h @@ -51,7 +51,7 @@ typedef struct { uint16_t ecd; int16_t speed_rpm; - int16_t given_current; + int16_t current_read; uint8_t temperate; int16_t last_ecd; } motor_measure_t; diff --git a/user/APP/USART_comms/USART_comms.c b/user/APP/USART_comms/USART_comms.c index 8684d4c..e81da3f 100644 --- a/user/APP/USART_comms/USART_comms.c +++ b/user/APP/USART_comms/USART_comms.c @@ -2,6 +2,8 @@ #include "USART_comms.h" #include +// Debugging + // Sends one string over USART // Example usage: serial_send_string("Nando eats Nando's at Nando's"); void serial_send_string(volatile char *str) @@ -48,3 +50,16 @@ int num_digits(int n) { return count; } + +// Vision + +// Sends one string over UART8 +void vision_send_string(volatile char *str) +{ + while (*str) { + // Once previous byte is finished being transmitted, transmit next byte + while (USART_GetFlagStatus(UART8, USART_FLAG_TC) == RESET); + USART_SendData(UART8, *str); + str++; + } +} diff --git a/user/APP/USART_comms/USART_comms.h b/user/APP/USART_comms/USART_comms.h index c4f5535..c7d993d 100644 --- a/user/APP/USART_comms/USART_comms.h +++ b/user/APP/USART_comms/USART_comms.h @@ -1,9 +1,11 @@ #ifndef USART_COMMS_H #define USART_COMMS_H +#include "main.h" extern void serial_send_string(volatile char *str); extern void serial_send_int_array(volatile int *arr, int length); extern void serial_send_int(int num); extern int num_digits(int n); +extern void vision_send_string(volatile char *str); #endif diff --git a/user/APP/remote_control/remote_control.c b/user/APP/remote_control/remote_control.c index 027126a..96d5dc7 100644 --- a/user/APP/remote_control/remote_control.c +++ b/user/APP/remote_control/remote_control.c @@ -1,13 +1,13 @@ /** ****************************(C) COPYRIGHT 2016 DJI**************************** * @file remote_control.c/h - * @brief Ò£¿ØÆ÷´¦Àí£¬Ò£¿ØÆ÷ÊÇͨ¹ýÀàËÆSBUSµÄЭÒé´«Ê䣬ÀûÓÃDMA´«Ê䷽ʽ½ÚÔ¼CPU - * ×ÊÔ´£¬ÀûÓô®¿Ú¿ÕÏÐÖжÏÀ´À­Æð´¦Àíº¯Êý£¬Í¬Ê±ÌṩһЩµôÏßÖØÆôDMA£¬´®¿Ú - * µÄ·½Ê½±£Ö¤ÈȲå°ÎµÄÎȶ¨ÐÔ¡£ - * @note ¸ÃÈÎÎñÊÇͨ¹ý´®¿ÚÖжÏÆô¶¯£¬²»ÊÇfreeRTOSÈÎÎñ + * @brief ?????????????????????????SBUS???????????DMA????????CPU + * ????????ô???????????????????????????h?????????DMA?????? + * ?k????????e??????? + * @note ????????????????????????????freeRTOS???? * @history * Version Date Author Modification - * V1.0.0 Dec-26-2018 RM 1. Íê³É + * V1.0.0 Dec-26-2018 RM 1. ??? * @verbatim ============================================================================== @@ -27,7 +27,7 @@ #include // #include "Detect_Task.h" // see todo l.134 -//Ò£¿ØÆ÷³ö´íÊý¾ÝÉÏÏÞ +//????????????????? #define RC_CHANNAL_ERROR_VALUE 700 // Function prototypes @@ -123,38 +123,38 @@ void USART1_IRQHandler(void) if(DMA_GetCurrentMemoryTarget(DMA2_Stream2) == 0) { - //ÖØÐÂÉèÖÃDMA + //????????DMA DMA_Cmd(DMA2_Stream2, DISABLE); this_time_rx_len = SBUS_RX_BUF_NUM - DMA_GetCurrDataCounter(DMA2_Stream2); DMA_SetCurrDataCounter(DMA2_Stream2, SBUS_RX_BUF_NUM); DMA2_Stream2->CR |= DMA_SxCR_CT; - //ÇåDMAÖжϱêÖ¾ + //??DMA????? DMA_ClearFlag(DMA2_Stream2, DMA_FLAG_TCIF2 | DMA_FLAG_HTIF2); DMA_Cmd(DMA2_Stream2, ENABLE); if(this_time_rx_len == RC_FRAME_LENGTH) { - //´¦ÀíÒ£¿ØÆ÷Êý¾Ý + //????????????? SBUS_TO_RC(SBUS_rx_buf[0], &rc_ctrl); - //¼Ç¼Êý¾Ý½ÓÊÕʱ¼ä + //??¼?????????? //TODO - need to implement this // DetectHook(DBUSTOE); } } else { - //ÖØÐÂÉèÖÃDMA + //????????DMA DMA_Cmd(DMA2_Stream2, DISABLE); this_time_rx_len = SBUS_RX_BUF_NUM - DMA_GetCurrDataCounter(DMA2_Stream2); DMA_SetCurrDataCounter(DMA2_Stream2, SBUS_RX_BUF_NUM); DMA2_Stream2->CR &= ~(DMA_SxCR_CT); - //ÇåDMAÖжϱêÖ¾ + //??DMA????? DMA_ClearFlag(DMA2_Stream2, DMA_FLAG_TCIF2 | DMA_FLAG_HTIF2); DMA_Cmd(DMA2_Stream2, ENABLE); if(this_time_rx_len == RC_FRAME_LENGTH) { - //´¦ÀíÒ£¿ØÆ÷Êý¾Ý + //????????????? SBUS_TO_RC(SBUS_rx_buf[1], &rc_ctrl); - //¼Ç¼Êý¾Ý½ÓÊÕʱ¼ä + //??¼?????????? // DetectHook(DBUSTOE); } diff --git a/user/TASK/INS_task/INS_task.c b/user/TASK/INS_task/INS_task.c index 92a7f6a..0eeea21 100644 --- a/user/TASK/INS_task/INS_task.c +++ b/user/TASK/INS_task/INS_task.c @@ -373,7 +373,8 @@ void INS_task(void *pvParameters) extern Gimbal_t gimbal; extern Gimbal_Motor_t gimbal_pitch_motor; -extern char str[]; + +static char message[32]; /** * @brief Reading angle, gyro, and accelerometer data and printing to serial @@ -390,34 +391,34 @@ void test_imu_readings(uint8_t angle, uint8_t gyro, uint8_t accel){ if (angle == TRUE) { //Sending angle data via UART - sprintf(str, "Angle yaw: %f\n\r", gimbal.angle_update[INS_YAW_ADDRESS_OFFSET]); - serial_send_string(str); - sprintf(str, "Angle pitch: %f\n\r", gimbal.angle_update[INS_PITCH_ADDRESS_OFFSET]); - serial_send_string(str); - sprintf(str, "Angle roll: %f\n\r", gimbal.angle_update[INS_ROLL_ADDRESS_OFFSET]); - serial_send_string(str); + sprintf(message, "Angle yaw: %f\n\r", gimbal.angle_update[INS_YAW_ADDRESS_OFFSET]); + serial_send_string(message); + sprintf(message, "Angle pitch: %f\n\r", gimbal.angle_update[INS_PITCH_ADDRESS_OFFSET]); + serial_send_string(message); + sprintf(message, "Angle roll: %f\n\r", gimbal.angle_update[INS_ROLL_ADDRESS_OFFSET]); + serial_send_string(message); serial_send_string("\n"); } if (gyro == TRUE) { //Sending gyro data via UART - sprintf(str, "Gyro X: %f\n\r", gimbal.gyro_update[INS_GYRO_X_ADDRESS_OFFSET]); - serial_send_string(str); - sprintf(str, "Gyro Y: %f\n\r", gimbal.gyro_update[INS_GYRO_Y_ADDRESS_OFFSET]); - serial_send_string(str); - sprintf(str, "Gyro Z: %f\n\r", gimbal.gyro_update[INS_GYRO_Z_ADDRESS_OFFSET]); - serial_send_string(str); + sprintf(message, "Gyro X: %f\n\r", gimbal.gyro_update[INS_GYRO_X_ADDRESS_OFFSET]); + serial_send_string(message); + sprintf(message, "Gyro Y: %f\n\r", gimbal.gyro_update[INS_GYRO_Y_ADDRESS_OFFSET]); + serial_send_string(message); + sprintf(message, "Gyro Z: %f\n\r", gimbal.gyro_update[INS_GYRO_Z_ADDRESS_OFFSET]); + serial_send_string(message); serial_send_string("\n"); } if (accel == TRUE) { //Sending accelerometer data via UART - sprintf(str, "Acce X: %f\n\r", gimbal.accel_update[INS_ACCEL_X_ADDRESS_OFFSET]); - serial_send_string(str); - sprintf(str, "Acce Y: %f\n\r", gimbal.accel_update[INS_ACCEL_Y_ADDRESS_OFFSET]); - serial_send_string(str); - sprintf(str, "Acce Z: %f\n\r", gimbal.accel_update[INS_ACCEL_Z_ADDRESS_OFFSET]); - serial_send_string(str); + sprintf(message, "Acce X: %f\n\r", gimbal.accel_update[INS_ACCEL_X_ADDRESS_OFFSET]); + serial_send_string(message); + sprintf(message, "Acce Y: %f\n\r", gimbal.accel_update[INS_ACCEL_Y_ADDRESS_OFFSET]); + serial_send_string(message); + sprintf(message, "Acce Z: %f\n\r", gimbal.accel_update[INS_ACCEL_Z_ADDRESS_OFFSET]); + serial_send_string(message); serial_send_string("\n"); } } diff --git a/user/TASK/INS_task/INS_task.h b/user/TASK/INS_task/INS_task.h index d792c00..8430381 100644 --- a/user/TASK/INS_task/INS_task.h +++ b/user/TASK/INS_task/INS_task.h @@ -18,6 +18,8 @@ #define MPU6500_USE_SPI_DMA //ÊÇ·ñʹÓÃSPIµÄDMA´«Ê䣬²»Ê¹ÓÃ×¢ÊͶ¨Òå +#define INS_TASK_DELAY 10 + //Èç¹ûÓÃÁËIST8310£¬DMA´«Êä23¸ö×Ö½Ú£¬Èç¹û²»Óã¬ÉÙ7¸ö×Ö½Ú£¬Îª16¸ö×Ö½Ú #if defined(USE_IST8310) #define DMA_RX_NUM 23 diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 067af4a..f3a87b4 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -23,7 +23,7 @@ #include "remote_control.h" #include "INS_task.h" #include "pid.h" - +#include /******************** Private User Declarations ********************/ static void chassis_init(Chassis_t *chassis_init); @@ -32,9 +32,17 @@ static void set_control_mode(Chassis_t *chassis); static void calculate_chassis_motion_setpoints(Chassis_t *chassis_set); static void calculate_motor_setpoints(Chassis_t *chassis_motors); static void increment_PID(Chassis_t *chassis_pid); +static void send_feedback_to_uart(Chassis_t *chassis); +static void check_allowed_current(Chassis_t *chassis_feedback); +static void limit_current(Chassis_Motor_t *motor); static Chassis_t chassis; + + +static char message[64] = {0}; +static int counter = 0; + #define DEBUG 0 /******************** Main Task/Functions Called from Outside ********************/ @@ -59,6 +67,8 @@ void chassis_task(void *pvParameters){ calculate_chassis_motion_setpoints(&chassis); calculate_motor_setpoints(&chassis); increment_PID(&chassis); + check_allowed_current(&chassis); + send_feedback_to_uart(&chassis); //output CAN_CMD_CHASSIS(chassis.motor[FRONT_RIGHT].current_out, chassis.motor[FRONT_LEFT].current_out, @@ -97,9 +107,11 @@ static void chassis_init(Chassis_t *chassis_init){ chassis_init->motor[i].motor_feedback = get_Chassis_Motor_Measure_Point(i); chassis_init->motor[i].speed_read = chassis_init->motor[i].motor_feedback->speed_rpm; chassis_init->motor[i].pos_read = chassis_init->motor[i].motor_feedback->ecd; - chassis_init->motor[i].current_read = chassis_init->motor[i].motor_feedback->given_current; + chassis_init->motor[i].current_read = chassis_init->motor[i].motor_feedback->current_read; - PID_Init(&chassis_init->motor[i].pid_controller, PID_DELTA, def_pid_constants, M3508_MAX_OUT, M3508_MIN_OUT); + chassis_init->motor[i].limiter = FULL_CURRENT; + chassis_init->motor[i].limiter_counter = 0; + PID_Init(&chassis_init->motor[i].pid_controller, PID_POSITION, def_pid_constants, M3508_MAX_OUT, M3508_MIN_OUT); } //Init yaw and front vector @@ -120,7 +132,7 @@ static void get_new_data(Chassis_t *chassis_update){ for (int i = 0; i < 4; i++) { chassis_update->motor[i].speed_read = chassis_update->motor[i].motor_feedback->speed_rpm; chassis_update->motor[i].pos_read = chassis_update->motor[i].motor_feedback->ecd; - chassis_update->motor[i].current_read = chassis_update->motor[i].motor_feedback->given_current; + chassis_update->motor[i].current_read = chassis_update->motor[i].motor_feedback->current_read; } } @@ -199,12 +211,19 @@ char pid_out[64]; * @retval None */ static void increment_PID(Chassis_t *chassis_pid){ - //translation + + for(int i = 0; i < 4; i++){ + chassis_pid->motor[i].current_out = chassis_pid->motor[i].speed_set; + } + + //translation //rotation fp32 front_right; fp32 back_right; fp32 front_left; fp32 back_left; + + // TODO: eliminate in place declaration & cast of floats front_right = PID_Calc(&chassis_pid->motor[FRONT_RIGHT].pid_controller, chassis_pid->motor[FRONT_RIGHT].speed_read, @@ -250,6 +269,91 @@ static void increment_PID(Chassis_t *chassis_pid){ chassis_pid->motor[BACK_LEFT].speed_read, chassis_pid->motor[BACK_LEFT].current_out); serial_send_string(pid_out); + } - +} + + + + +/** +* Current limiter has 4 states, +* If over move to half +* If over still for longer than hysteresis time, move to quarter, +* If still over for longer than hysteresis time, move to off. +* for all of these cases: +* If current stays below limit for hysteresis time, reduce limitation by 1 level. +*/ + +static void check_allowed_current(Chassis_t *chassis_feedback){ + for(int i = 0; i < 4; i++){ + if(abs(chassis_feedback->motor[i].motor_feedback->current_read) > CURRENT_LIMIT){ + sprintf(message, "current limit reached, limiter state is %d", chassis_feedback->motor[i].limiter); + serial_send_string(message); + if(chassis_feedback->motor[i].limiter == FULL_CURRENT){ + chassis_feedback->motor[i].limiter = HALF_CURRENT; + } + + chassis_feedback->motor[i].limiter_counter++; + if(chassis_feedback->motor[i].limiter_counter > HYSTERESIS_PERIOD){ + if(chassis_feedback->motor[i].limiter != NO_CURRENT){ + chassis_feedback->motor[i].limiter++; + } + + chassis_feedback->motor[i].limiter_counter = 0; + } + } + else{ + chassis_feedback->motor[i].limiter_counter--; + if(chassis_feedback->motor[i].limiter_counter < -HYSTERESIS_PERIOD){ + if(chassis_feedback->motor[i].limiter != FULL_CURRENT){ + chassis_feedback->motor[i].limiter--; + } + + chassis_feedback->motor[i].limiter_counter = 0; + } + } + + limit_current(&chassis_feedback->motor[i]); + } +} + + +static void limit_current(Chassis_Motor_t *motor){ + sprintf(message, "limiter state is changing from %d", motor->limiter); + switch(motor->limiter){ + case FULL_CURRENT: + break; + case HALF_CURRENT: + motor->current_out *= 0.5f; + break; + case QUARTER_CURRENT: + motor->current_out *= 0.25f; + break; + case NO_CURRENT: + motor->current_out = 0; + break; + } + +} + +//static void under_current_limit(Chassis_Motor_t motor + + + + + + +static void send_feedback_to_uart(Chassis_t *chassis){ + if(counter == 0){ + sprintf(message, "chassis rpm is: %i \n\r", chassis->motor[0].motor_feedback->speed_rpm); + serial_send_string(message); + sprintf(message, "chassis torque current is: %i \n\r", chassis->motor[0].motor_feedback->current_read); + serial_send_string(message); + sprintf(message, "chassis limiter state: %d", chassis->motor[0].limiter); + serial_send_string(message); + } + + counter = (counter + 1) % 1000; + } diff --git a/user/TASK/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index ef01d6d..98419f5 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -17,7 +17,7 @@ #include "pid.h" /******************************* Task Delays *********************************/ -#define CHASSIS_TASK_DELAY 1 +#define CHASSIS_TASK_DELAY 5 #define CHASSIS_INIT_DELAY 20 /******************** User Definitions ********************/ @@ -28,21 +28,34 @@ #define BACK_LEFT 2 #define BACK_RIGHT 3 -#define MULTIPLIER 3 +#define MULTIPLIER 5 -// RC channels -#define RC_X 2 +// RC channels -- this indicated strafe drive +// left stick: rotation +// right stick: fwd/rev, left/right +#define RC_X 0 #define RC_Y 1 -#define RC_Z 0 +#define RC_Z 2 +// For //M3508 motors max and min CAN output -#define M3508_MAX_OUT 1000 +#define M3508_MAX_OUT 10000 #define M3508_MIN_OUT 50.0 +#define M3508_MAX_IOUT 40 //M3508 speed PID constants -#define M3508_KP 0.02 -#define M3508_KI 0.00 -#define M3508_KD 0.1 +#define M3508_KP 0.002f +#define M3508_KI 0.00f +#define M3508_KD 0.0f +// Current Limiting Constants +#define HYSTERESIS_PERIOD 5 +#define CURRENT_LIMIT 25000 +typedef enum{ + FULL_CURRENT, + HALF_CURRENT, + QUARTER_CURRENT, + NO_CURRENT, +} current_limiter_state; typedef enum{ CHASSIS_VECTOR_RAW, @@ -56,16 +69,21 @@ typedef struct const motor_measure_t *motor_feedback; //Current speed read from motors - int16_t pos_read; + uint16_t pos_read; int16_t speed_read; int16_t current_read; //Target speed set by user/remote control int16_t pos_set; int16_t speed_set; + // TODO: check if these should be unsigned /exist at all //Final output speed int16_t current_out; + + // Current limiting parameters + int8_t limiter_counter; + current_limiter_state limiter; //Control PidTypeDef pid_controller; diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 91834c0..688f726 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -20,7 +20,6 @@ #include "task.h" #include "start_task.h" - /******************** User Includes ********************/ #include "CAN_Receive.h" #include "user_lib.h" @@ -29,21 +28,31 @@ #include #include "pid.h" #include "shoot_task.h" +#include -#define DEADBAND 10 +#define DEADBAND 1 // This is accessbile globally and some data is loaded from INS_task Gimbal_t gimbal; +Gimbal_Angles gimbal_angles; //UART mailbox -char str[32] = {0}; +char str[32] = {0}; // TODO: figure out why ins_task is using this... delete and compile to reproduce +//static char message[64] = {0}; +static int loop_counter = 0; /******************** Functions ********************/ -static void initialization(Gimbal_t *gimbal); -static void get_new_data(Gimbal_t *gimbal); +static void initialization(Gimbal_t *gimbal, Gimbal_Angles *gimbal_angles); +static void get_new_encoder_data(Gimbal_t *gimbal); static void update_setpoints(Gimbal_t *gimbal); static void increment_PID(Gimbal_t *gimbal); +static void fill_complex_equivalent(fp32 position[2], uint16_t ecd_value); + +Gimbal_Angles* get_gimbal_angle_struct(void) +{ + return &gimbal_angles; +} /** * @brief Initializes PID and fetches Gimbal motor data to ensure @@ -54,16 +63,23 @@ static void increment_PID(Gimbal_t *gimbal); void gimbal_task(void* parameters){ vTaskDelay(GIMBAL_INIT_DELAY); - initialization(&gimbal); + initialization(&gimbal, &gimbal_angles); while(1){ //send_to_uart(&gimbal); + //send_vision_angle(); /* For now using strictly encoder feedback for position */ - get_new_data(&gimbal); + get_new_encoder_data(&gimbal); //send_to_uart(&gimbal); - update_setpoints(&gimbal); + + if (gimbal_angles.new_angle_flag) { + sprintf(str, "delta angle: %d\n\r", gimbal_angles.yaw_angle); + serial_send_string(str); + update_setpoints(&gimbal); + gimbal_angles.new_angle_flag = 0; + } //send_to_uart(&gimbal); increment_PID(&gimbal); //send_to_uart(&gimbal); @@ -75,6 +91,7 @@ void gimbal_task(void* parameters){ //Sending data via UART vTaskDelay(GIMBAL_TASK_DELAY); + send_to_uart(&gimbal); } } @@ -83,20 +100,32 @@ void gimbal_task(void* parameters){ * @param None * @retval None */ -static void initialization(Gimbal_t *gimbal_ptr){ +static void initialization(Gimbal_t *gimbal_ptr, Gimbal_Angles *gimbal_angles){ gimbal_ptr->pitch_motor.motor_feedback = get_Pitch_Gimbal_Motor_Measure_Point(); gimbal_ptr->yaw_motor.motor_feedback = get_Yaw_Gimbal_Motor_Measure_Point(); gimbal_ptr->launcher = get_launcher_pointer(); fp32 pid_constants_yaw[3] = {pid_kp_yaw, pid_ki_yaw, pid_kd_yaw}; fp32 pid_constants_pitch[3] = {pid_kp_pitch, pid_ki_pitch, pid_kd_pitch}; - + + gimbal_ptr->yaw_setpoint[0] = 0.0; + gimbal_ptr->yaw_setpoint[1] = -1.0; + gimbal_ptr->yaw_position[0] = 0.0; + gimbal_ptr->yaw_position[1] = -1.0; + gimbal_ptr->yaw_error = 0.0; + PID_Init(&(gimbal_ptr->pitch_motor.pid_controller), PID_POSITION, pid_constants_pitch, max_out_pitch, max_i_term_out_pitch); PID_Init(&(gimbal_ptr->yaw_motor.pid_controller), PID_POSITION, pid_constants_yaw, max_out_yaw, max_i_term_out_yaw); gimbal_ptr->rc_update = get_remote_control_point(); - gimbal_ptr->pitch_motor.pos_set = 5500; + gimbal_ptr->pitch_motor.pos_set = 3000; gimbal_ptr->yaw_motor.pos_set = 6000; + + gimbal_angles->count_rx = 0; + gimbal_angles->new_angle_flag = 0; + gimbal_angles->pitch_angle = 0; + gimbal_angles->yaw_angle = 0; + // TODO: find better bounds on Pitch } /** @@ -104,14 +133,36 @@ static void initialization(Gimbal_t *gimbal_ptr){ * @param None * @retval None */ -static void get_new_data(Gimbal_t *gimbal_data){ - // Get CAN received data - gimbal_data->pitch_motor.pos_read = gimbal_data->pitch_motor.motor_feedback->ecd; - gimbal_data->pitch_motor.speed_read = gimbal_data->pitch_motor.motor_feedback->speed_rpm; +static void get_new_encoder_data(Gimbal_t *gimbal_data){ + + // Get CAN received data + gimbal_data->pitch_motor.pos_read = gimbal_data->pitch_motor.motor_feedback->ecd; + gimbal_data->pitch_motor.speed_read = gimbal_data->pitch_motor.motor_feedback->speed_rpm; - gimbal_data->yaw_motor.pos_read = gimbal_data->yaw_motor.motor_feedback->ecd; - gimbal_data->yaw_motor.speed_read = gimbal_data->yaw_motor.motor_feedback->speed_rpm; + gimbal_data->yaw_motor.pos_read = gimbal_data->yaw_motor.motor_feedback->ecd; + gimbal_data->yaw_motor.speed_read = gimbal_data->yaw_motor.motor_feedback->speed_rpm; + fill_complex_equivalent(gimbal_data->yaw_position, gimbal_data->yaw_motor.pos_read); +} + + +static void fill_complex_equivalent(fp32 position[2], uint16_t ecd_value){ + fp32 theta = ecd_value * Motor_Ecd_to_Rad; + position[0] = cos(theta); + position[1] = sin(theta); +} + +static void multiply_complex_a_by_b(fp32 a[2], fp32 b[2]){ + fp32 real = a[0] * b[0] - a[1] * b[1]; + fp32 imaj = a[0] * b[1] + a[1] * b[0]; + a[0] = real; + a[1] = imaj; +} + +static void make_unit_length(fp32 n[2]){ + fp32 length = sqrt(n[0] * n[0] + n[1] * n[1]); + n[0] = n[0] / length; + n[1] = n[1] / length; } /** @@ -120,43 +171,52 @@ static void get_new_data(Gimbal_t *gimbal_data){ * @retval None */ static void update_setpoints(Gimbal_t *gimbal_set){ + if(1){ + fp32 theta = gimbal_angles.yaw_angle * Degree_To_Rad; + fp32 rotation[2] = {cos(theta), sin(theta)}; + multiply_complex_a_by_b(gimbal_set->yaw_setpoint, rotation); + make_unit_length(gimbal_set->yaw_setpoint); + + gimbal_set->pitch_motor.pos_set += int16_deadzone(gimbal_angles.pitch_angle, -DEADBAND, DEADBAND) / 80.0f; + } + gimbal_set->pitch_motor.pos_set = int16_constrain(gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); - gimbal_set->yaw_motor.pos_set += (-1) * int16_deadzone(gimbal_set->rc_update->rc.ch[2], -DEADBAND, DEADBAND) / 10; - gimbal_set->pitch_motor.pos_set += int16_deadzone(gimbal_set->rc_update->rc.ch[3], -DEADBAND, DEADBAND) / 10; - - int16_constrain(gimbal_set->yaw_motor.pos_set, YAW_MIN, YAW_MAX); - int16_constrain(gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); + char print[30]; + sprintf(print, "Yaw setpoint x: %.4f y: %.4f \n\r", gimbal_set->yaw_setpoint[0], gimbal_set->yaw_setpoint[1]); + serial_send_string(print); + sprintf(print, "Pitch setpoint: %d (encoder position 0-8191)\n\r", gimbal_set->pitch_motor.pos_set); + serial_send_string(print); +} + +static fp32 a_dot_b(fp32 a[2], fp32 b[2]){ + return a[0] * b[0] + a[1] * b[1]; +} - //TODO: worry about the case where pos_set is unsigned and rc channel manages to push it - // negative for a moment, causing the position to wrap from below 0 to a maxvalue. +static fp32 length_of_a_cross_b(fp32 a[2], fp32 b[2]){ + return a[0] * b[1] - a[1] * b[0]; } +static int16_t get_error_sign(fp32 actual_position[2], fp32 set_position[2]){ + return length_of_a_cross_b(actual_position, set_position) >= 0 ? 1 : -1; +} + + /** * @brief Increments PID loop based on latest setpoints and latest positions * @param None * @retval None */ static void increment_PID(Gimbal_t *gimbal_pid){ + fp32 error = get_error_sign(gimbal_pid->yaw_position, gimbal_pid->yaw_setpoint) + * (1 - a_dot_b(gimbal_pid->yaw_position,gimbal_pid->yaw_setpoint)) + * ERROR_MULTIPLIER; + + gimbal_pid->yaw_motor.current_out = PID_Calc(&gimbal_pid->yaw_motor.pid_controller, 0.0f, error); + gimbal_pid->pitch_motor.current_out = PID_Calc(&gimbal_pid->pitch_motor.pid_controller, gimbal_pid->pitch_motor.pos_read, gimbal_pid->pitch_motor.pos_set); - gimbal_pid->yaw_motor.current_out = PID_Calc(&gimbal_pid->yaw_motor.pid_controller, gimbal_pid->yaw_motor.pos_read, gimbal_pid->yaw_motor.pos_set); } -/** - * @brief Reads vision instruction from UART and cap to certin values - * @param None - * @retval Vision signal in range of 0 and 8191 - */ -int get_vision_signal(void) { - int vision_signal = 5700; // TODO: Get real values from vision - - while (vision_signal > 8191) { - vision_signal -= 8191; - } - while (vision_signal < 0) { - vision_signal+= 8192; - } - return vision_signal; -} + /** * @brief Updates Uart with position information on the yaw motor and the PID settings @@ -168,20 +228,44 @@ int get_vision_signal(void) { */ void send_to_uart(Gimbal_t *gimbal_msg) { - //char str[20]; //uart data buffer //TODO - fix below / fill as needed - + + if(loop_counter == 0){ + /*sprintf(str, "yaw position re: %.2f im: %.2f \n\r", gimbal_msg->yaw_position[0], gimbal_msg->yaw_position[1]); + serial_send_string(str); + sprintf(message, "yaw setpoint re: %.2f im: %.2f \n\r", gimbal_msg->yaw_setpoint[0], gimbal_msg->yaw_setpoint[1]); + serial_send_string(message); + sprintf(message, "--- \n\r"); + serial_send_string(message); + sprintf(message, "pitch: %i", gimbal_msg->pitch_motor.pos_read); + serial_send_string(message);*/ + } + loop_counter = (loop_counter + 1) % 1000; + + + /* + sprintf(str, "we are not infected"); + serial_send_string(str);*/ /* sprintf(str, "yaw speed read: %d\n\r", gimbal->yaw_motor->speed_read); serial_send_string(str); - sprintf(str, "yaw current read: %d\n\r", gimbal->yaw_motor->current_read); - serial_send_string(str); -*/ - //sprintf(str, "yaw setpoint: %d\n\r", gimbal->yaw_motor->pos_set); - //serial_send_string(str); +static void send_feedback_to_uart(Gimbal_t *gimbal){ + if(counter == 0){ + sprintf(message, "(2,3) yaw rpm is: %i \n\r", gimbal->yaw_motor.motor_feedback->speed_rpm); + serial_send_string(message); + sprintf(message, "(4,5) yaw torque current is: %i \n\r", gimbal->yaw_motor.motor_feedback->current_read); + serial_send_string(message); + } + + counter = (counter + 1) % 1000; */ - //sprintf(str, "yaw current out: %d\n\r", gimbal->yaw_motor->current_out); - //serial_send_string(str); } +/* +void send_vision_angle() { + char hex[5]; + sprintf(hex, "%x", gimbal_angles.yaw_angle); + vision_send_string(hex); +} +*/ diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index c8db9cb..9d1a4bc 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -10,6 +10,7 @@ #include "pid.h" #include "shoot_task.h" #include "remote_control.h" +#include "USART_comms.h" /******************************* Task Delays *********************************/ #define GIMBAL_TASK_DELAY 1 @@ -17,16 +18,17 @@ /*************** Converts between motor position and degrees *****************/ -#define Motor_Ecd_to_Rad 0.000766990394f +#define Motor_Ecd_to_Rad 0.000766990394f /* 2PI / 8191 */ +#define Degree_To_Rad 0.0087266389; #define FALSE 0 #define TRUE 1 /****************************** PID Constants ********************************/ -#define pid_kp_yaw 40.0f +#define pid_kp_yaw 250.0f #define pid_ki_yaw 0.0f -#define pid_kd_yaw 0.0f -#define max_out_yaw 5000.0f +#define pid_kd_yaw 250.0f +#define max_out_yaw 15000.0f #define max_i_term_out_yaw 1000.0f #define pid_kp_pitch 40.0f @@ -44,20 +46,22 @@ #define ENCODER_MAX 8191 #define YAW_MIN 2359 #define YAW_MAX 6576 -#define PITCH_MIN 5500 -#define PITCH_MAX 7000 +#define PITCH_MIN 2020 +#define PITCH_MAX 3000 +#define ERROR_MULTIPLIER 2048 +#define BUFFER_SIZE 100 /************************** Gimbal Data Structures ***************************/ typedef struct { const motor_measure_t *motor_feedback; - int16_t pos_read; + uint16_t pos_read; int16_t speed_read; int16_t current_read; int16_t pos_set; - int16_t current_out; + int16_t current_out; // TODO: update to voltage out PidTypeDef pid_controller; } Gimbal_Motor_t; @@ -70,18 +74,36 @@ typedef struct const fp32 *angle_update; const fp32 *gyro_update; const fp32 *accel_update; + const uint32_t current_angle; // TODO: Add gimbal angles when we care about orientation of robot in 3-d space + fp32 yaw_setpoint[2]; // {real, imaj} + fp32 yaw_position[2]; // {real, imaj} + fp32 yaw_error; + Shoot_t *launcher; } Gimbal_t; +///////////////////////// +// Degrees relative +typedef struct +{ + uint8_t new_angle_flag; + int yaw_angle; + int pitch_angle; + uint8_t count_rx; + char buffer_rx[BUFFER_SIZE]; +} Gimbal_Angles; + /******************************* Function Declarations ***********************/ int get_vision_signal(void); extern void gimbal_task(void *pvParameters); extern void send_to_uart(Gimbal_t *gimbal); +//void send_vision_angle(); - +// get a pointer to the gimbal struct +extern Gimbal_Angles* get_gimbal_angle_struct(void); /******************************* Variable Declarations ***********************/ // These will later be produced from RC diff --git a/user/TASK/shoot_task/shoot_task.c b/user/TASK/shoot_task/shoot_task.c index a479b5d..7d3eab1 100644 --- a/user/TASK/shoot_task/shoot_task.c +++ b/user/TASK/shoot_task/shoot_task.c @@ -122,7 +122,7 @@ static void shoot_init(Shoot_t *shoot_init) { */ static void set_control_mode(void) { //Gets outcome of rc - if (shoot.rc->rc.s[POWER_SWITCH] == RC_SW_UP || shoot.rc->rc.s[POWER_SWITCH] == RC_SW_DOWN) { + if (shoot.rc->rc.s[POWER_SWITCH] == RC_SW_UP) { if (shoot.rc->rc.s[SHOOT_SWITCH] == RC_SW_MID) { pwm_target = Fric_DOWN; // no shoot @@ -210,6 +210,10 @@ static void shoot_reversed_control(Shoot_Motor_t *trigger_motor, Shoot_Motor_t * } +static uint16_t shoot_count = 0; +#define SHOOT_COUNT_MAX 1250 +#define SHOOT_COUNT_SWTICH 750 + /** * @brief Trigger motor and hopper motor continuously spinning to allow for rapid firing * @param Trigger motor and hopper motor structs @@ -217,8 +221,20 @@ static void shoot_reversed_control(Shoot_Motor_t *trigger_motor, Shoot_Motor_t * */ static void shoot_rapid_control(Shoot_Motor_t *trigger_motor, Shoot_Motor_t *hopper_motor) { - //Hopper and trigger motor both spinning, used for clearing bullets - hopper_motor->speed_set = HOPPER_SPEED; + shoot_count++; + + if(shoot_count > SHOOT_COUNT_MAX){ + shoot_count = 0; + hopper_motor->speed_set = HOPPER_SPEED; + + } + if(shoot_count > SHOOT_COUNT_SWTICH){ + hopper_motor->speed_set = HOPPER_SPEED * -0.5f; + } + else { + hopper_motor->speed_set = HOPPER_SPEED;; + } + trigger_motor->speed_set = TRIGGER_SPEED; } diff --git a/user/TASK/shoot_task/shoot_task.h b/user/TASK/shoot_task/shoot_task.h index a6b1615..6895ead 100644 --- a/user/TASK/shoot_task/shoot_task.h +++ b/user/TASK/shoot_task/shoot_task.h @@ -15,7 +15,10 @@ #include "CAN_receive.h" #ifndef SHOOT_TASK_H -#define SHOOT_TASK_H +#define SHOOT_TASK_H + +#define SHOOT_TASK_DELAY 1 +#define SHOOT_INIT_DELAY 2000 //Time between ramp steps, in seconds #define RAMP_PRD 0.001 @@ -33,7 +36,7 @@ #define HOPPER_OFF 0 //Trigger motor PID -#define TRIGGER_MAX_OUT 5000.0f +#define TRIGGER_MAX_OUT 8000.0f #define TRIGGER_MAX_IOUT 2500.0f #define TRIGGER_ANGLE_PID_KP 800.0f #define TRIGGER_ANGLE_PID_KI 0.5f diff --git a/user/TASK/start_task/start_task.h b/user/TASK/start_task/start_task.h index 0b439ee..77db577 100644 --- a/user/TASK/start_task/start_task.h +++ b/user/TASK/start_task/start_task.h @@ -19,9 +19,4 @@ #include "main.h" void startTask(void); -#define INS_TASK_DELAY 10 -#define GIMBAL_TASK_DELAY 1 -#define SHOOT_TASK_DELAY 1 -#define SHOOT_INIT_DELAY 2000 - #endif diff --git a/user/hardware/usart/usart.c b/user/hardware/usart/usart.c index 41eea4f..8fa5cbc 100644 --- a/user/hardware/usart/usart.c +++ b/user/hardware/usart/usart.c @@ -51,6 +51,63 @@ void USART_6_INIT(void) // configure NVIC for interrupts - only on Rx NVIC_InitStructure.NVIC_IRQChannel = USART6_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; // set priority group to highest priority achieveable by sw (hw can go negative) + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; // set priority highest in subgroup + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + +} + +void USART_8_INIT(void) +{ + GPIO_InitTypeDef GPIO_InitStructure; + USART_InitTypeDef USART_InitStructure; + NVIC_InitTypeDef NVIC_InitStructure; + + // Enable the GPIOG clock + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE); + // Enable peripheral clock for USART6 + RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART8, ENABLE); + + // Force USART6 clock to reset + RCC_APB1PeriphResetCmd(RCC_APB1Periph_UART8, ENABLE); + RCC_APB1PeriphResetCmd(RCC_APB1Periph_UART8, DISABLE); + + // Setup GPIO pins for UART + + GPIO_PinAFConfig(GPIOE, GPIO_PinSource1, GPIO_AF_UART8); // Tx + GPIO_PinAFConfig(GPIOE, GPIO_PinSource0, GPIO_AF_UART8); // Rx + + // Configure GPIO pin settings + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_0; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; // rc USART uses GPIO_PuPd_NOPULL + GPIO_Init(GPIOE, &GPIO_InitStructure); + + USART_DeInit(UART8); + + // Configure UART - this all needs to match the config on the Pi + USART_InitStructure.USART_BaudRate = 115200; // idk what max is so 9600 to be safe + USART_InitStructure.USART_WordLength = USART_WordLength_8b; // These are the default settings + USART_InitStructure.USART_StopBits = USART_StopBits_1; // All set by USART_StructInit() + USART_InitStructure.USART_Parity = USART_Parity_No; // These details are here for clarity + USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; + + + // USART_StructInit(&USART_InitStructure); // all the settings above are the default settings, this sets them all + USART_Init(UART8, &USART_InitStructure); + + // enable interrupts + USART_ITConfig(UART8, USART_IT_RXNE, ENABLE); + + // enable USART8 peripheral + USART_Cmd(UART8, ENABLE); + + // configure NVIC for interrupts - only on Rx + NVIC_InitStructure.NVIC_IRQChannel = UART8_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; // set priority group to highest priority achieveable by sw (hw can go negative) NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; // set priority highest in subgroup NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; diff --git a/user/hardware/usart/usart.h b/user/hardware/usart/usart.h index 503ff87..6a6f201 100644 --- a/user/hardware/usart/usart.h +++ b/user/hardware/usart/usart.h @@ -2,5 +2,6 @@ #define USART_H extern void USART_6_INIT(void); +extern void USART_8_INIT(void); #endif diff --git a/user/main.c b/user/main.c index 76d7f90..c1c67bc 100644 --- a/user/main.c +++ b/user/main.c @@ -88,7 +88,8 @@ void BSP_init(void) CAN1_mode_init(CAN_SJW_1tq, CAN_BS2_2tq, CAN_BS1_6tq, 5, CAN_Mode_Normal); CAN2_mode_init(CAN_SJW_1tq, CAN_BS2_2tq, CAN_BS1_6tq, 5, CAN_Mode_Normal); - USART_6_INIT(); + USART_6_INIT(); // debugging + USART_8_INIT(); // vision remote_control_init(); //24v power output on