From 43582f030af4512c3f739d2186722aa4d4fea598 Mon Sep 17 00:00:00 2001 From: kricha02 Date: Tue, 10 Mar 2020 21:45:50 -0700 Subject: [PATCH 01/22] Updated gimbal and shoot task to follow switch controls correctly. Cleaned up INS task and Start task seperation. --- Project/embed-infantry.uvprojx | 5 ++++- user/TASK/INS_task/INS_task.h | 2 ++ user/TASK/chassis_task/chassis_task.h | 5 ++--- user/TASK/gimbal_task/gimbal_task.c | 7 ++++--- user/TASK/shoot_task/shoot_task.c | 2 +- user/TASK/shoot_task/shoot_task.h | 5 ++++- user/TASK/start_task/start_task.h | 5 ----- 7 files changed, 17 insertions(+), 14 deletions(-) diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index 393bc45..904038f 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -10,7 +10,8 @@ embed-infantry 0x4 ARM-ADS - 5060422::V5.06 update 4 (build 422)::ARMCC + 5060750::V5.06 update 6 (build 750)::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 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.h b/user/TASK/chassis_task/chassis_task.h index ef01d6d..ed9066c 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 ********************/ @@ -41,8 +41,7 @@ //M3508 speed PID constants #define M3508_KP 0.02 #define M3508_KI 0.00 -#define M3508_KD 0.1 - +#define M3508_KD 0.25 typedef enum{ CHASSIS_VECTOR_RAW, diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 91834c0..0d16828 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -120,9 +120,10 @@ static void get_new_data(Gimbal_t *gimbal_data){ * @retval None */ static void update_setpoints(Gimbal_t *gimbal_set){ - - 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; + if(gimbal_set->rc_update->rc.s[0] == RC_SW_MID || gimbal_set->rc_update->rc.s[0] == RC_SW_UP){ + 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); diff --git a/user/TASK/shoot_task/shoot_task.c b/user/TASK/shoot_task/shoot_task.c index a479b5d..93848b0 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 diff --git a/user/TASK/shoot_task/shoot_task.h b/user/TASK/shoot_task/shoot_task.h index a6b1615..c1087b8 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 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 From 9acef781945e4a24cd4fb0160d03946b48db24bd Mon Sep 17 00:00:00 2001 From: Benji Date: Thu, 12 Mar 2020 21:10:43 -0700 Subject: [PATCH 02/22] working 360 gimbal (verified) --- Project/embed-infantry.uvprojx | 5 +- user/TASK/gimbal_task/gimbal_task.c | 123 ++++++++++++++++++++++++---- user/TASK/gimbal_task/gimbal_task.h | 21 +++-- 3 files changed, 124 insertions(+), 25 deletions(-) diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index 393bc45..904038f 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -10,7 +10,8 @@ embed-infantry 0x4 ARM-ADS - 5060422::V5.06 update 4 (build 422)::ARMCC + 5060750::V5.06 update 6 (build 750)::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 diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 91834c0..7b4b1b5 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,6 +28,7 @@ #include #include "pid.h" #include "shoot_task.h" +#include #define DEADBAND 10 @@ -36,7 +36,9 @@ Gimbal_t gimbal; //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); @@ -44,6 +46,14 @@ static void get_new_data(Gimbal_t *gimbal); static void update_setpoints(Gimbal_t *gimbal); static void increment_PID(Gimbal_t *gimbal); +/******* Complex Arithmatic Functions ***********s***/ +static void fill_complex_equivalent(fp32 position[2], uint16_t ecd_value); +static void multiply_complex_a_by_b(fp32 a[2], fp32 b[2]); +static fp32 a_dot_b(fp32 a[2], fp32 b[2]); +static fp32 length_of_a_cross_b(fp32 a[2], fp32 b[2]); +static int16_t get_error_sign(fp32 actual_position[2], fp32 set_position[2]); +static void make_unit_length(fp32 vector[2]); + /** * @brief Initializes PID and fetches Gimbal motor data to ensure @@ -75,6 +85,7 @@ void gimbal_task(void* parameters){ //Sending data via UART vTaskDelay(GIMBAL_TASK_DELAY); + send_to_uart(&gimbal); } } @@ -89,14 +100,21 @@ static void initialization(Gimbal_t *gimbal_ptr){ 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] = 1.0; + gimbal_ptr->yaw_setpoint[1] = 0.0; + gimbal_ptr->yaw_position[0] = 1.0; + gimbal_ptr->yaw_position[1] = 0.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; + // TODO: find better bounds on Pitch } /** @@ -104,14 +122,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_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; } /** @@ -121,24 +161,59 @@ static void get_new_data(Gimbal_t *gimbal_data){ */ static void update_setpoints(Gimbal_t *gimbal_set){ - 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; + //yaw: + //rc -> theta -> complex rotation -> new setpoint + fp32 theta = int16_deadzone(gimbal_set->rc_update->rc.ch[2], -DEADBAND, DEADBAND) + * Motor_Ecd_to_Rad / 40.0f; + fp32 rotation[2] = {cos(theta), sin(theta)}; // TODO: consider alternative {cos{theta}, sin{theta}} + multiply_complex_a_by_b(gimbal_set->yaw_setpoint, rotation); + make_unit_length(gimbal_set->yaw_setpoint); - int16_constrain(gimbal_set->yaw_motor.pos_set, YAW_MIN, YAW_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) / 40; + + //int16_constrain(gimbal_set->yaw_motor.pos_set, YAW_MIN, YAW_MAX); int16_constrain(gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); //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 a_dot_b(fp32 a[2], fp32 b[2]){ + return a[0] * b[0] + a[1] * b[1]; +} + +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){ + /* Get error sign + get error val + insert into pid*/ + // error magnitude 1 * ERROR_MULTIPLIER corresponds to gimbal being 90 degrees out of place --> jump action observed by pedram + // TODO: consider revising PID to eliminate the ERROR_MULTIPLIER constant + // TODO: retune PID currently very slow + 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; + // TODO: Modify pid to reject massive d term spike this could create. + // TODO: Consider how this isn't quite a linear error (dot product) + 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); + } /** @@ -158,6 +233,9 @@ int get_vision_signal(void) { return vision_signal; } + + + /** * @brief Updates Uart with position information on the yaw motor and the PID settings * This will block for several milliseconds @@ -168,10 +246,23 @@ 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); + } + 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); diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index c8db9cb..d2f7312 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -17,7 +17,7 @@ /*************** Converts between motor position and degrees *****************/ -#define Motor_Ecd_to_Rad 0.000766990394f +#define Motor_Ecd_to_Rad 0.000766990394f /* 2PI / 8191 */ #define FALSE 0 #define TRUE 1 @@ -44,19 +44,20 @@ #define ENCODER_MAX 8191 #define YAW_MIN 2359 #define YAW_MAX 6576 -#define PITCH_MIN 5500 -#define PITCH_MAX 7000 +#define PITCH_MIN 5800 +#define PITCH_MAX 5000 +#define ERROR_MULTIPLIER 2048 /************************** Gimbal Data Structures ***************************/ typedef struct { const motor_measure_t *motor_feedback; - int16_t pos_read; - int16_t speed_read; - int16_t current_read; - - int16_t pos_set; + uint16_t pos_read; + uint16_t speed_read; + uint16_t current_read; + // TODO: Check types -- especially speed_read/current_read...?? + uint16_t pos_set; int16_t current_out; PidTypeDef pid_controller; @@ -72,6 +73,10 @@ typedef struct const fp32 *accel_update; // 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; From 2739b63af3ae15689e1096c93923421fda001eb1 Mon Sep 17 00:00:00 2001 From: Benji Date: Sat, 14 Mar 2020 09:54:40 -0700 Subject: [PATCH 03/22] print feedback message --- Project/embed-infantry.uvprojx | 3 +++ user/APP/CAN_receive/CAN_receive.c | 25 ++++++++++------- user/APP/CAN_receive/CAN_receive.h | 2 +- user/TASK/INS_task/INS_task.c | 39 ++++++++++++++------------- user/TASK/chassis_task/chassis_task.c | 21 +++++++++++++-- user/TASK/gimbal_task/gimbal_task.c | 35 +++++++++--------------- 6 files changed, 71 insertions(+), 54 deletions(-) diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index 393bc45..80a5b57 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 diff --git a/user/APP/CAN_receive/CAN_receive.c b/user/APP/CAN_receive/CAN_receive.c index 8fe62cf..166c802 100644 --- a/user/APP/CAN_receive/CAN_receive.c +++ b/user/APP/CAN_receive/CAN_receive.c @@ -35,17 +35,24 @@ /******************** 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, delete one of these if unecessary... @@ -254,22 +261,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 +286,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/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/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 067af4a..94c654c 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -32,6 +32,7 @@ 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 Chassis_t chassis; @@ -97,7 +98,7 @@ 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); } @@ -120,7 +121,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; } } @@ -253,3 +254,19 @@ static void increment_PID(Chassis_t *chassis_pid){ } } + + +static char message[64] = {0}; +static int counter = 0; + +static void send_feedback_to_uart(Chassis_t *chassis){ + if(counter == 0){ + sprintf(message, "(2,3) rpm is: %i \n\r", chassis->motor[0].motor_feedback->speed_rpm); + serial_send_string(message); + sprintf(message, "(4,5) torque feedback is: %i \n\r", chassis->motor[0].motor_feedback->current_read); + serial_send_string(message); + } + + counter = (counter + 1) % 1000; + +} diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 91834c0..6a31e44 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -158,30 +158,19 @@ int get_vision_signal(void) { return vision_signal; } -/** - * @brief Updates Uart with position information on the yaw motor and the PID settings - * This will block for several milliseconds - * @param gimbal_yaw_motor struct containing information about the gimbal yaw motor - * @param pid struct containing pid coefficients - * @param pitch_signal signal to pitch motor - * @retval None - */ -void send_to_uart(Gimbal_t *gimbal_msg) -{ - //char str[20]; //uart data buffer - //TODO - fix below / fill as needed -/* - sprintf(str, "yaw speed read: %d\n\r", gimbal->yaw_motor->speed_read); - serial_send_string(str); +static char message[64] = {0}; +static int counter = 0; - 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) rpm is: %i \n\r", gimbal->yaw_motor.motor_feedback->speed_rpm); + serial_send_string(message); + sprintf(message, "(4,5) torque feedback is: %i \n\r", gimbal->yaw_motor.motor_feedback->current_read); + serial_send_string(message); + } - //sprintf(str, "yaw current out: %d\n\r", gimbal->yaw_motor->current_out); - //serial_send_string(str); -} + counter = (counter + 1) % 1000; + +} \ No newline at end of file From 2d0421f4470c0d96b1410057c8953aa167cdebe5 Mon Sep 17 00:00:00 2001 From: Benji Date: Sat, 14 Mar 2020 18:35:07 -0700 Subject: [PATCH 04/22] current limiter: --- Project/embed-infantry.uvprojx | 2 +- user/APP/CAN_receive/CAN_receive.c | 4 +- user/TASK/chassis_task/chassis_task.c | 93 +++++++++++++++++++++++++-- user/TASK/chassis_task/chassis_task.h | 21 +++++- user/TASK/gimbal_task/gimbal_task.c | 25 ++++++- user/TASK/gimbal_task/gimbal_task.h | 2 +- 6 files changed, 133 insertions(+), 14 deletions(-) diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index 80a5b57..904038f 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -10,7 +10,7 @@ embed-infantry 0x4 ARM-ADS - 5060422::V5.06 update 4 (build 422)::ARMCC + 5060750::V5.06 update 6 (build 750)::ARMCC 0 diff --git a/user/APP/CAN_receive/CAN_receive.c b/user/APP/CAN_receive/CAN_receive.c index 166c802..8307ffc 100644 --- a/user/APP/CAN_receive/CAN_receive.c +++ b/user/APP/CAN_receive/CAN_receive.c @@ -52,7 +52,9 @@ (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, delete one of these if unecessary... + // 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 + diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 94c654c..61b656e 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); @@ -33,9 +33,16 @@ 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 ********************/ @@ -60,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, @@ -100,7 +109,9 @@ static void chassis_init(Chassis_t *chassis_init){ 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->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 @@ -206,6 +217,8 @@ static void increment_PID(Chassis_t *chassis_pid){ 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, @@ -256,14 +269,82 @@ static void increment_PID(Chassis_t *chassis_pid){ } -static char message[64] = {0}; -static int counter = 0; + + +/** +* 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); + 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, "(2,3) rpm is: %i \n\r", chassis->motor[0].motor_feedback->speed_rpm); + 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, "(4,5) torque feedback is: %i \n\r", chassis->motor[0].motor_feedback->current_read); + sprintf(message, "chassis limiter state: %d", chassis->motor[0].limiter); serial_send_string(message); } diff --git a/user/TASK/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index ef01d6d..cb0b40a 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -36,13 +36,23 @@ #define RC_Z 0 //M3508 motors max and min CAN output -#define M3508_MAX_OUT 1000 +#define M3508_MAX_OUT 1500 #define M3508_MIN_OUT 50.0 //M3508 speed PID constants -#define M3508_KP 0.02 +#define M3508_KP 0.04 #define M3508_KI 0.00 #define M3508_KD 0.1 +// Current Limiting Constants +#define HYSTERESIS_PERIOD 5 +#define CURRENT_LIMIT 1000 + +typedef enum{ + FULL_CURRENT, + HALF_CURRENT, + QUARTER_CURRENT, + NO_CURRENT, +} current_limiter_state; typedef enum{ CHASSIS_VECTOR_RAW, @@ -56,16 +66,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 6a31e44..409c0bd 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -44,6 +44,27 @@ static void get_new_data(Gimbal_t *gimbal); static void update_setpoints(Gimbal_t *gimbal); static void increment_PID(Gimbal_t *gimbal); +/** +* Slew Control +* While the current is over a limiting constant: +* Reduce control signal by 0.5 every 5 ms +* When current returns the acceptable range, double the +* allowed current every 5 ms until it returns to full output. +* +* Disable/reset I-term at this time whenever current limiting is active.\ +* +* Do we want reductions to follow : 1/2, 1/4, 1/8, ... +* or 1/2, 1/4 , OFF +* Or mutliply out put constant +* Tunable Parameters +* redutions factor: e.g. reduce current by 0.5 +* reduction time: e.g. reduce current every 5 ms +* increase factor +* increase time +* I-term off/reset +* hysteresis time: how long should the current limiter +* wait before starting to increase/decrease current. +*/ /** * @brief Initializes PID and fetches Gimbal motor data to ensure @@ -165,9 +186,9 @@ static int counter = 0; static void send_feedback_to_uart(Gimbal_t *gimbal){ if(counter == 0){ - sprintf(message, "(2,3) rpm is: %i \n\r", gimbal->yaw_motor.motor_feedback->speed_rpm); + 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) torque feedback is: %i \n\r", gimbal->yaw_motor.motor_feedback->current_read); + sprintf(message, "(4,5) yaw torque current is: %i \n\r", gimbal->yaw_motor.motor_feedback->current_read); serial_send_string(message); } diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index c8db9cb..bce143d 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -57,7 +57,7 @@ typedef struct 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; From 85638014a4a4913b21b71ec2d0348a2ec6feafa3 Mon Sep 17 00:00:00 2001 From: Benji Date: Sat, 14 Mar 2020 18:44:52 -0700 Subject: [PATCH 05/22] soft limit pitch --- user/TASK/gimbal_task/gimbal_task.c | 2 ++ user/TASK/gimbal_task/gimbal_task.h | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 7b4b1b5..dc629cf 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -256,6 +256,8 @@ void send_to_uart(Gimbal_t *gimbal_msg) 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; diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index d2f7312..d6e5286 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -44,8 +44,8 @@ #define ENCODER_MAX 8191 #define YAW_MIN 2359 #define YAW_MAX 6576 -#define PITCH_MIN 5800 -#define PITCH_MAX 5000 +#define PITCH_MIN 2020 +#define PITCH_MAX 3000 #define ERROR_MULTIPLIER 2048 From e04ba346a0439365a79149ddb406f321941bed30 Mon Sep 17 00:00:00 2001 From: Benji Date: Sat, 14 Mar 2020 19:04:52 -0700 Subject: [PATCH 06/22] verified current limit triggering --- user/TASK/chassis_task/chassis_task.c | 3 ++- user/TASK/chassis_task/chassis_task.h | 12 ++++++------ user/TASK/gimbal_task/gimbal_task.c | 2 +- user/TASK/gimbal_task/gimbal_task.h | 4 ++-- 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 61b656e..6972b4a 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -43,7 +43,7 @@ static Chassis_t chassis; static char message[64] = {0}; static int counter = 0; -#define DEBUG 0 +#define DEBUG 1 /******************** Main Task/Functions Called from Outside ********************/ @@ -284,6 +284,7 @@ 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; } diff --git a/user/TASK/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index cb0b40a..0b817bb 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -28,7 +28,7 @@ #define BACK_LEFT 2 #define BACK_RIGHT 3 -#define MULTIPLIER 3 +#define MULTIPLIER 6 // RC channels #define RC_X 2 @@ -36,13 +36,13 @@ #define RC_Z 0 //M3508 motors max and min CAN output -#define M3508_MAX_OUT 1500 +#define M3508_MAX_OUT 2000 #define M3508_MIN_OUT 50.0 +#define M3508_MAX_IOUT 40 //M3508 speed PID constants -#define M3508_KP 0.04 -#define M3508_KI 0.00 -#define M3508_KD 0.1 - +#define M3508_KP 4.0f +#define M3508_KI 0.0001f +#define M3508_KD 0.01f // Current Limiting Constants #define HYSTERESIS_PERIOD 5 #define CURRENT_LIMIT 1000 diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 409c0bd..9964379 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -116,7 +116,7 @@ static void initialization(Gimbal_t *gimbal_ptr){ gimbal_ptr->rc_update = get_remote_control_point(); - gimbal_ptr->pitch_motor.pos_set = 5500; + gimbal_ptr->pitch_motor.pos_set = 2700; gimbal_ptr->yaw_motor.pos_set = 6000; } diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index bce143d..2402b19 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -44,8 +44,8 @@ #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 /************************** Gimbal Data Structures ***************************/ From e8c29e7cdbe93887f70163307a34a3a10c50f271 Mon Sep 17 00:00:00 2001 From: kricha02 Date: Sat, 14 Mar 2020 20:34:10 -0700 Subject: [PATCH 07/22] Tuned pid constants and fixed pitch limiting. --- user/TASK/gimbal_task/gimbal_task.c | 14 +++++++++----- user/TASK/gimbal_task/gimbal_task.h | 6 +++--- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index dc629cf..ed39425 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -163,7 +163,7 @@ static void update_setpoints(Gimbal_t *gimbal_set){ //yaw: //rc -> theta -> complex rotation -> new setpoint - fp32 theta = int16_deadzone(gimbal_set->rc_update->rc.ch[2], -DEADBAND, DEADBAND) + fp32 theta = -1 * int16_deadzone(gimbal_set->rc_update->rc.ch[2], -DEADBAND, DEADBAND) * Motor_Ecd_to_Rad / 40.0f; fp32 rotation[2] = {cos(theta), sin(theta)}; // TODO: consider alternative {cos{theta}, sin{theta}} multiply_complex_a_by_b(gimbal_set->yaw_setpoint, rotation); @@ -173,9 +173,13 @@ static void update_setpoints(Gimbal_t *gimbal_set){ //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) / 40; + char print[30]; + sprintf(print, "Constrain pitch %d between %d, %d \n\r", gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); + serial_send_string(print); //int16_constrain(gimbal_set->yaw_motor.pos_set, YAW_MIN, YAW_MAX); - int16_constrain(gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); - + gimbal_set->pitch_motor.pos_set = int16_constrain(gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); + sprintf(print, "Constrained to %d \n\r", gimbal_set->pitch_motor.pos_set); + serial_send_string(print); //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. } @@ -250,14 +254,14 @@ void send_to_uart(Gimbal_t *gimbal_msg) //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]); + /*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); + serial_send_string(message);*/ } loop_counter = (loop_counter + 1) % 1000; diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index d6e5286..3d70308 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -23,10 +23,10 @@ /****************************** 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 From 2576661025595819ec8a5dc88e69c77506b9359a Mon Sep 17 00:00:00 2001 From: Benji Date: Sun, 15 Mar 2020 10:24:52 -0700 Subject: [PATCH 08/22] updates --- user/TASK/chassis_task/chassis_task.c | 10 ++++++++-- user/TASK/chassis_task/chassis_task.h | 10 +++++----- user/TASK/gimbal_task/gimbal_task.c | 3 ++- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 6972b4a..9d65d36 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -211,7 +211,12 @@ 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; @@ -264,7 +269,8 @@ 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); - } + + }*/ } diff --git a/user/TASK/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index 0b817bb..8f54de0 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -28,7 +28,7 @@ #define BACK_LEFT 2 #define BACK_RIGHT 3 -#define MULTIPLIER 6 +#define MULTIPLIER 10 // RC channels #define RC_X 2 @@ -40,12 +40,12 @@ #define M3508_MIN_OUT 50.0 #define M3508_MAX_IOUT 40 //M3508 speed PID constants -#define M3508_KP 4.0f -#define M3508_KI 0.0001f -#define M3508_KD 0.01f +#define M3508_KP 1.0f +#define M3508_KI 0.0f +#define M3508_KD 0.0f // Current Limiting Constants #define HYSTERESIS_PERIOD 5 -#define CURRENT_LIMIT 1000 +#define CURRENT_LIMIT 25000 typedef enum{ FULL_CURRENT, diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 938c9e8..cd7ffe1 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -45,6 +45,7 @@ static void initialization(Gimbal_t *gimbal); static void get_new_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); /** * Slew Control @@ -290,6 +291,6 @@ static void send_feedback_to_uart(Gimbal_t *gimbal){ serial_send_string(message); } - counter = (counter + 1) % 1000; + counter = (counter + 1) % 1000; */ } \ No newline at end of file From d8a98b3b2e9087c53c44bfc5c1b2d80c686692f5 Mon Sep 17 00:00:00 2001 From: Benji Date: Sun, 15 Mar 2020 10:51:01 -0700 Subject: [PATCH 09/22] remove warning --- user/TASK/gimbal_task/gimbal_task.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 8b12c63..3b35d40 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -194,8 +194,6 @@ static void update_setpoints(Gimbal_t *gimbal_set){ gimbal_set->pitch_motor.pos_set = int16_constrain(gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); sprintf(print, "Constrained to %d \n\r", gimbal_set->pitch_motor.pos_set); serial_send_string(print); - //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 a_dot_b(fp32 a[2], fp32 b[2]){ @@ -297,4 +295,4 @@ static void send_feedback_to_uart(Gimbal_t *gimbal){ counter = (counter + 1) % 1000; */ -} \ No newline at end of file +} From f66e61d6ddbae52670b9e7a8f8e572ba3ad783b7 Mon Sep 17 00:00:00 2001 From: JoshMarangoni Date: Sun, 15 Mar 2020 11:26:25 -0700 Subject: [PATCH 10/22] progress --- CMSIS/stm32f4xx_it.c | 66 +++++++++++++++++++++++- Project/embed-infantry.uvprojx | 7 +-- user/APP/USART_comms/USART_comms.h | 16 ++++++ user/APP/remote_control/remote_control.c | 28 +++++----- user/TASK/gimbal_task/gimbal_task.c | 5 ++ user/TASK/gimbal_task/gimbal_task.h | 1 + 6 files changed, 102 insertions(+), 21 deletions(-) diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index b0c110a..4e0888b 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -36,6 +36,7 @@ volatile char buffer_rx[100]; int count_rx = 0; + /** @addtogroup Template_Project * @{ */ @@ -171,7 +172,15 @@ void SysTick_Handler(void) /** * @} */ - + +READY = 1; +NOT_READY = 0; + +// get a pointer to the gimbal struct +const Gimbal_buffer* get_gimbal_angle_buffer(void) +{ + return &gimbal_buff; +} // Interrupt handler for USART 6. This is called on the reception of every // byte on USART6 @@ -180,7 +189,60 @@ void USART6_IRQHandler(void) // make sure USART6 was intended to be called for this interrupt if(USART_GetITStatus(USART6, USART_IT_RXNE) != RESET) { uint16_t USART_Data = USART_ReceiveData(USART6); - + serial_send_string(&NOT_READY); + + uint8_t mask = 0x01; + + // first 10 bits are decimal, next 9 integer, next 5 checksum + switch(gimbal_buff.num_filled) { + case 0: { + gimbal_buff.packets[0] = (uint8_t)USART_Data; + gimbal_buff.decimal = (uint8_t)USART_Data; + serial_send_string(&READY); + }; + case 1: { + gimbal_buff.packets[1] = (uint8_t)USART_Data; + uint8_t last_decimal_bits; + uint8_t first_int_bits; + + for (int i=0; i < 8; i++) { + if (i < 2) { + last_decimal_bits += (uint8_t)USART_Data & mask; + last_decimal_bits = last_decimal_bits << 1; + } + else { + first_int_bits += (uint8_t)USART_Data & mask; + first_int_bits = first_int_bits << 1; + } + mask = mask << 1; + } + gimbal_buff.integer = first_int_bits; + gimbal_buff.decimal += last_decimal_bits; + serial_send_string(&READY); + }; + case 2: { + gimbal_buff.packets[2] = (uint8_t)USART_Data; + uint8_t last_int_bits; + uint8_t checksum_bits; + + for (int i=0; i < 8; i++) { + if (i < 3) { + last_int_bits += (uint8_t)USART_Data & mask; + last_int_bits = last_int_bits << 1; + } + else { + checksum_bits += (uint8_t)USART_Data & mask; + checksum_bits = checksum_bits << 1; + } + mask = mask << 1; + } + gimbal_buff.checksum = checksum_bits; + serial_send_string(&READY); + } + default: + serial_send_string(&READY); + } + // This just echos everything back once a CR (carriage return) // character (13 in ascii) is received. Windows (or Putty, idk) // appends a CR to enter/return so it should work diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index d5ac67a..0e64021 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -11,13 +11,12 @@ 0x4 ARM-ADS 5060750::V5.06 update 6 (build 750)::ARMCC - 0 STM32F427IIHx STMicroelectronics - Keil.STM32F4xx_DFP.2.14.0 - http://www.keil.com/pack/ + Keil.STM32F4xx_DFP.2.12.0 + http://www.keil.com/pack IRAM(0x20000000,0x30000) IRAM2(0x10000000,0x10000) IROM(0x08000000,0x200000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE @@ -184,7 +183,6 @@ 0 0 2 - 0 1 0 8 @@ -325,7 +323,6 @@ 0 0 1 - 0 0 3 3 diff --git a/user/APP/USART_comms/USART_comms.h b/user/APP/USART_comms/USART_comms.h index c4f5535..fde8b4c 100644 --- a/user/APP/USART_comms/USART_comms.h +++ b/user/APP/USART_comms/USART_comms.h @@ -1,9 +1,25 @@ #ifndef USART_COMMS_H #define USART_COMMS_H +static char READY; +static char NOT_READY; + 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); +typedef struct +{ + uint8_t num_filled; + uint8_t packets[3]; + uint16_t integer; + uint16_t decimal; + uint8_t checksum; + +} Gimbal_buffer; + +// A struct that holds all of the gimbal data +static Gimbal_buffer gimbal_buff; + #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/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 3286cb6..d0212a3 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -28,6 +28,7 @@ #include #include "pid.h" + #define DEADBAND 10 // This is accessbile globally and some data is loaded from INS_task @@ -55,6 +56,7 @@ void gimbal_task(void* parameters){ initialization(&gimbal); while(1){ + serial_send_string(READY); //sends 0 to Python //send_to_uart(&gimbal); /* For now using strictly encoder feedback for position */ @@ -97,6 +99,9 @@ static void initialization(Gimbal_t *gimbal_ptr){ gimbal_ptr->pitch_motor.pos_set = 6000; gimbal_ptr->yaw_motor.pos_set = 6000; + + gimbal_buff.ready_signal = 1; + gimbal_buff.num_filled = 0; } /** diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index 3a4fde4..08c4fa5 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -69,6 +69,7 @@ typedef struct const fp32 *angle_update; const fp32 *gyro_update; const fp32 *accel_update; + const uint24_t current_angle; // TODO: Add gimbal angles when we care about orientation of robot in 3-d space } Gimbal_t; From 0bdc292b2eff70423df8ebbc19f3e9005a15c70a Mon Sep 17 00:00:00 2001 From: jirm2019 Date: Sun, 15 Mar 2020 13:37:03 -0700 Subject: [PATCH 11/22] moved vision uart to UART8, use vision_send_string, moved vision uart files to vision.c and h files --- CMSIS/stm32f4xx_it.c | 61 ----------------------------- Project/embed-infantry.uvprojx | 21 ++++++++-- user/APP/USART_comms/USART_comms.c | 15 +++++++ user/APP/USART_comms/USART_comms.h | 10 ++--- user/TASK/gimbal_task/gimbal_task.c | 9 +++-- user/TASK/gimbal_task/gimbal_task.h | 2 +- user/hardware/usart/usart.c | 57 +++++++++++++++++++++++++++ user/hardware/usart/usart.h | 1 + user/main.c | 3 +- 9 files changed, 104 insertions(+), 75 deletions(-) diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index 4e0888b..a76afa4 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -173,14 +173,6 @@ void SysTick_Handler(void) * @} */ -READY = 1; -NOT_READY = 0; - -// get a pointer to the gimbal struct -const Gimbal_buffer* get_gimbal_angle_buffer(void) -{ - return &gimbal_buff; -} // Interrupt handler for USART 6. This is called on the reception of every // byte on USART6 @@ -189,59 +181,6 @@ void USART6_IRQHandler(void) // make sure USART6 was intended to be called for this interrupt if(USART_GetITStatus(USART6, USART_IT_RXNE) != RESET) { uint16_t USART_Data = USART_ReceiveData(USART6); - serial_send_string(&NOT_READY); - - uint8_t mask = 0x01; - - // first 10 bits are decimal, next 9 integer, next 5 checksum - switch(gimbal_buff.num_filled) { - case 0: { - gimbal_buff.packets[0] = (uint8_t)USART_Data; - gimbal_buff.decimal = (uint8_t)USART_Data; - serial_send_string(&READY); - }; - case 1: { - gimbal_buff.packets[1] = (uint8_t)USART_Data; - uint8_t last_decimal_bits; - uint8_t first_int_bits; - - for (int i=0; i < 8; i++) { - if (i < 2) { - last_decimal_bits += (uint8_t)USART_Data & mask; - last_decimal_bits = last_decimal_bits << 1; - } - else { - first_int_bits += (uint8_t)USART_Data & mask; - first_int_bits = first_int_bits << 1; - } - mask = mask << 1; - } - gimbal_buff.integer = first_int_bits; - gimbal_buff.decimal += last_decimal_bits; - serial_send_string(&READY); - }; - case 2: { - gimbal_buff.packets[2] = (uint8_t)USART_Data; - uint8_t last_int_bits; - uint8_t checksum_bits; - - for (int i=0; i < 8; i++) { - if (i < 3) { - last_int_bits += (uint8_t)USART_Data & mask; - last_int_bits = last_int_bits << 1; - } - else { - checksum_bits += (uint8_t)USART_Data & mask; - checksum_bits = checksum_bits << 1; - } - mask = mask << 1; - } - gimbal_buff.checksum = checksum_bits; - serial_send_string(&READY); - } - default: - serial_send_string(&READY); - } // This just echos everything back once a CR (carriage return) // character (13 in ascii) is received. Windows (or Putty, idk) diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index 7fd205e..a5186c1 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -10,13 +10,14 @@ embed-infantry 0x4 ARM-ADS - 5060422::V5.06 update 4 (build 422)::ARMCC + 5060750::V5.06 update 6 (build 750)::ARMCC + 0 STM32F427IIHx STMicroelectronics - Keil.STM32F4xx_DFP.2.12.0 - http://www.keil.com/pack + Keil.STM32F4xx_DFP.2.14.0 + http://www.keil.com/pack/ IRAM(0x20000000,0x30000) IRAM2(0x10000000,0x10000) IROM(0x08000000,0x200000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE @@ -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 @@ -859,6 +862,16 @@ 5 ..\user\APP\PID\pid.h + + vision.c + 1 + ..\user\APP\vision\vision.c + + + vision.h + 5 + ..\user\APP\vision\vision.h + 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 fde8b4c..c6e80b0 100644 --- a/user/APP/USART_comms/USART_comms.h +++ b/user/APP/USART_comms/USART_comms.h @@ -1,14 +1,17 @@ #ifndef USART_COMMS_H #define USART_COMMS_H +#include "main.h" -static char READY; -static char NOT_READY; +static volatile char READY = 1; +static volatile char NOT_READY = 0; 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); + typedef struct { uint8_t num_filled; @@ -19,7 +22,4 @@ typedef struct } Gimbal_buffer; -// A struct that holds all of the gimbal data -static Gimbal_buffer gimbal_buff; - #endif diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index cc75f1e..79d6f86 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -29,12 +29,14 @@ #include #include "pid.h" #include "shoot_task.h" +#include "vision.h" #define DEADBAND 10 // This is accessbile globally and some data is loaded from INS_task Gimbal_t gimbal; +Gimbal_buffer *gimbal_buff; //UART mailbox char str[32] = {0}; @@ -58,7 +60,7 @@ void gimbal_task(void* parameters){ initialization(&gimbal); while(1){ - serial_send_string(READY); //sends 0 to Python + serial_send_string(&READY); //sends 1 to Python //send_to_uart(&gimbal); /* For now using strictly encoder feedback for position */ @@ -100,8 +102,9 @@ static void initialization(Gimbal_t *gimbal_ptr){ gimbal_ptr->pitch_motor.pos_set = 5500; gimbal_ptr->yaw_motor.pos_set = 6000; - gimbal_buff.ready_signal = 1; - gimbal_buff.num_filled = 0; + gimbal_buff = get_gimbal_angle_buffer(); + // gimbal_buff.ready_signal = 1; + gimbal_buff->num_filled = 0; } /** diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index b09934f..dd0d07c 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -70,7 +70,7 @@ typedef struct const fp32 *angle_update; const fp32 *gyro_update; const fp32 *accel_update; - const uint24_t current_angle; + //const uint24_t current_angle; // TODO: Add gimbal angles when we care about orientation of robot in 3-d space Shoot_t *launcher; diff --git a/user/hardware/usart/usart.c b/user/hardware/usart/usart.c index 41eea4f..c1b4143 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 = 9600; // 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 USART6 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 From 22577669d22a8d4d6c296eb9dba56289c3189af4 Mon Sep 17 00:00:00 2001 From: Benji Date: Sun, 15 Mar 2020 22:02:57 -0700 Subject: [PATCH 12/22] filming shoot task build --- user/TASK/chassis_task/chassis_task.c | 7 +++---- user/TASK/chassis_task/chassis_task.h | 4 ++-- user/TASK/gimbal_task/gimbal_task.c | 14 +++++++------- user/TASK/shoot_task/shoot_task.c | 20 ++++++++++++++++++-- user/TASK/shoot_task/shoot_task.h | 2 +- 5 files changed, 31 insertions(+), 16 deletions(-) diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 9d65d36..f3a87b4 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -43,7 +43,7 @@ static Chassis_t chassis; static char message[64] = {0}; static int counter = 0; -#define DEBUG 1 +#define DEBUG 0 /******************** Main Task/Functions Called from Outside ********************/ @@ -215,7 +215,7 @@ static void increment_PID(Chassis_t *chassis_pid){ for(int i = 0; i < 4; i++){ chassis_pid->motor[i].current_out = chassis_pid->motor[i].speed_set; } - /* + //translation //rotation fp32 front_right; @@ -270,8 +270,7 @@ static void increment_PID(Chassis_t *chassis_pid){ chassis_pid->motor[BACK_LEFT].current_out); serial_send_string(pid_out); - }*/ - + } } diff --git a/user/TASK/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index 1485fab..732f802 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -28,7 +28,7 @@ #define BACK_LEFT 2 #define BACK_RIGHT 3 -#define MULTIPLIER 10 +#define MULTIPLIER 3 // RC channels #define RC_X 2 @@ -40,7 +40,7 @@ #define M3508_MIN_OUT 50.0 #define M3508_MAX_IOUT 40 //M3508 speed PID constants -#define M3508_KP 1.0f +#define M3508_KP 0.001f #define M3508_KI 0.0f #define M3508_KD 0.0f // Current Limiting Constants diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 3eadc96..b4096d8 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -30,7 +30,7 @@ #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; @@ -115,10 +115,10 @@ static void initialization(Gimbal_t *gimbal_ptr){ 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] = 1.0; - gimbal_ptr->yaw_setpoint[1] = 0.0; - gimbal_ptr->yaw_position[0] = 1.0; - gimbal_ptr->yaw_position[1] = 0.0; + 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); @@ -181,12 +181,12 @@ static void update_setpoints(Gimbal_t *gimbal_set){ // TODO: FIX if(gimbal_set->rc_update->rc.s[0] == RC_SW_MID || gimbal_set->rc_update->rc.s[0] == RC_SW_UP){ fp32 theta = -1 * int16_deadzone(gimbal_set->rc_update->rc.ch[2], -DEADBAND, DEADBAND) - * Motor_Ecd_to_Rad / 40.0f; + * Motor_Ecd_to_Rad / 80.0f; fp32 rotation[2] = {cos(theta), sin(theta)}; // TODO: consider alternative {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_set->rc_update->rc.ch[3], -DEADBAND, DEADBAND) / 40.0f; + gimbal_set->pitch_motor.pos_set += int16_deadzone(gimbal_set->rc_update->rc.ch[3], -DEADBAND, DEADBAND) / 80.0f; } char print[30]; diff --git a/user/TASK/shoot_task/shoot_task.c b/user/TASK/shoot_task/shoot_task.c index 93848b0..7d3eab1 100644 --- a/user/TASK/shoot_task/shoot_task.c +++ b/user/TASK/shoot_task/shoot_task.c @@ -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 c1087b8..6895ead 100644 --- a/user/TASK/shoot_task/shoot_task.h +++ b/user/TASK/shoot_task/shoot_task.h @@ -36,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 From edbbadc90079564d5594c2838d13e9e05fb209b6 Mon Sep 17 00:00:00 2001 From: JoshMarangoni Date: Tue, 17 Mar 2020 16:36:24 -0700 Subject: [PATCH 13/22] create uart 8 irq no test --- CMSIS/stm32f4xx_it.c | 87 ++++++++++++++++++++++++++++- Project/embed-infantry.uvprojx | 13 ++--- Project/vision.c | 0 Project/vision.h | 0 user/APP/USART_comms/USART_comms.h | 4 +- user/TASK/gimbal_task/gimbal_task.c | 9 ++- user/TASK/gimbal_task/gimbal_task.h | 6 +- user/hardware/usart/usart.c | 2 +- 8 files changed, 104 insertions(+), 17 deletions(-) create mode 100644 Project/vision.c create mode 100644 Project/vision.h diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index a76afa4..e8d3f32 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -30,7 +30,7 @@ /* Includes ------------------------------------------------------------------*/ #include "stm32f4xx_it.h" #include "main.h" - +#include "gimbal_task.h" #include "USART_comms.h" volatile char buffer_rx[100]; @@ -206,5 +206,90 @@ void USART6_IRQHandler(void) } } +// Interrupt handler for USART 8. This is called on the reception of every +// byte on USART8 +void USART8_IRQHandler(void) +{ + // make sure USART8 was intended to be called for this interrupt + if(USART_GetITStatus(UART8, USART_IT_RXNE) != RESET) { + uint16_t USART_Data = USART_ReceiveData(UART8); + serial_send_string(NOT_READY); + + uint8_t mask = 0x01; + Gimbal_buffer *gimbal_buff = get_gimbal_angle_buffer(); + // first 10 bits are decimal, next 9 integer, next 5 checksum + switch(gimbal_buff->num_filled) { + case 0: { + gimbal_buff->packets[0] = (uint8_t)USART_Data; + gimbal_buff->decimal = (uint8_t)USART_Data; + serial_send_string((char)READY); + }; + case 1: { + gimbal_buff->packets[1] = (uint8_t)USART_Data; + uint8_t last_decimal_bits; + uint8_t first_int_bits; + + for (int i=0; i < 8; i++) { + if (i < 2) { + last_decimal_bits += (uint8_t)USART_Data & mask; + last_decimal_bits = last_decimal_bits << 1; + } + else { + first_int_bits += (uint8_t)USART_Data & mask; + first_int_bits = first_int_bits << 1; + } + mask = mask << 1; + } + gimbal_buff->integer = first_int_bits; + gimbal_buff->decimal += last_decimal_bits; + serial_send_string((char)READY); + }; + case 2: { + gimbal_buff->packets[2] = (uint8_t)USART_Data; + uint8_t last_int_bits; + uint8_t checksum_bits; + + for (int i=0; i < 8; i++) { + if (i < 3) { + last_int_bits += (uint8_t)USART_Data & mask; + last_int_bits = last_int_bits << 1; + } + else { + checksum_bits += (uint8_t)USART_Data & mask; + checksum_bits = checksum_bits << 1; + } + mask = mask << 1; + } + gimbal_buff.checksum = checksum_bits; + serial_send_string(&READY); + } + default: + serial_send_string(&READY); + } + + // This just echos everything back once a CR (carriage return) + // character (13 in ascii) is received. Windows (or Putty, idk) + // appends a CR to enter/return so it should work + if (USART_Data == 13) { + buffer_rx[count_rx] = 0; + serial_send_string(buffer_rx); + + count_rx = 0; + for (int i = 0; i < 100; i++) { + buffer_rx[i] = 0; + } + + } else if (count_rx < 100 - 1) { + buffer_rx[count_rx++] = USART_Data; + } else { + count_rx = 0; + for (int i = 0; i < 100; i++) { + buffer_rx[i] = 0; + } + } + + } +} + /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index a5186c1..43b600f 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -10,14 +10,13 @@ embed-infantry 0x4 ARM-ADS - 5060750::V5.06 update 6 (build 750)::ARMCC - 0 + 5060422::V5.06 update 4 (build 422)::ARMCC STM32F427IIHx STMicroelectronics - Keil.STM32F4xx_DFP.2.14.0 - http://www.keil.com/pack/ + Keil.STM32F4xx_DFP.2.12.0 + http://www.keil.com/pack IRAM(0x20000000,0x30000) IRAM2(0x10000000,0x10000) IROM(0x08000000,0x200000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE @@ -184,7 +183,6 @@ 0 0 2 - 0 1 0 8 @@ -325,7 +323,6 @@ 0 0 1 - 0 0 3 3 @@ -865,12 +862,12 @@ vision.c 1 - ..\user\APP\vision\vision.c + .\vision.c vision.h 5 - ..\user\APP\vision\vision.h + .\vision.h 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/USART_comms/USART_comms.h b/user/APP/USART_comms/USART_comms.h index c6e80b0..f81fd96 100644 --- a/user/APP/USART_comms/USART_comms.h +++ b/user/APP/USART_comms/USART_comms.h @@ -2,8 +2,8 @@ #define USART_COMMS_H #include "main.h" -static volatile char READY = 1; -static volatile char NOT_READY = 0; +#define READY 1 +#define NOT_READY 0 extern void serial_send_string(volatile char *str); extern void serial_send_int_array(volatile int *arr, int length); diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 79d6f86..95dcb48 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -29,7 +29,6 @@ #include #include "pid.h" #include "shoot_task.h" -#include "vision.h" #define DEADBAND 10 @@ -47,6 +46,11 @@ static void get_new_data(Gimbal_t *gimbal); static void update_setpoints(Gimbal_t *gimbal); static void increment_PID(Gimbal_t *gimbal); +const Gimbal_buffer* get_gimbal_angle_buffer(void) +{ + return &gimbal_buff; +} + /** * @brief Initializes PID and fetches Gimbal motor data to ensure @@ -60,7 +64,7 @@ void gimbal_task(void* parameters){ initialization(&gimbal); while(1){ - serial_send_string(&READY); //sends 1 to Python + serial_send_string((char)READY); //sends 1 to Python //send_to_uart(&gimbal); /* For now using strictly encoder feedback for position */ @@ -103,7 +107,6 @@ static void initialization(Gimbal_t *gimbal_ptr){ gimbal_ptr->yaw_motor.pos_set = 6000; gimbal_buff = get_gimbal_angle_buffer(); - // gimbal_buff.ready_signal = 1; gimbal_buff->num_filled = 0; } diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index dd0d07c..bbdcf59 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 @@ -70,7 +71,7 @@ typedef struct const fp32 *angle_update; const fp32 *gyro_update; const fp32 *accel_update; - //const uint24_t current_angle; + const uint32_t current_angle; // TODO: Add gimbal angles when we care about orientation of robot in 3-d space Shoot_t *launcher; @@ -82,7 +83,8 @@ int get_vision_signal(void); extern void gimbal_task(void *pvParameters); extern void send_to_uart(Gimbal_t *gimbal); - +// get a pointer to the gimbal struct +extern const Gimbal_buffer* get_gimbal_angle_buffer(void); /******************************* Variable Declarations ***********************/ // These will later be produced from RC diff --git a/user/hardware/usart/usart.c b/user/hardware/usart/usart.c index c1b4143..5be1712 100644 --- a/user/hardware/usart/usart.c +++ b/user/hardware/usart/usart.c @@ -103,7 +103,7 @@ void USART_8_INIT(void) // enable interrupts USART_ITConfig(UART8, USART_IT_RXNE, ENABLE); - // enable USART6 peripheral + // enable USART8 peripheral USART_Cmd(UART8, ENABLE); // configure NVIC for interrupts - only on Rx From 62644e811588fc404bbb254c6228f9cf0ca7b772 Mon Sep 17 00:00:00 2001 From: Benji Date: Tue, 17 Mar 2020 20:55:26 -0700 Subject: [PATCH 14/22] latest --- CMSIS/system_stm32f4xx.c | 8 ++++---- user/TASK/chassis_task/chassis_task.h | 17 ++++++++++------- 2 files changed, 14 insertions(+), 11 deletions(-) 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/user/TASK/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index 732f802..98419f5 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -28,20 +28,23 @@ #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 2000 +#define M3508_MAX_OUT 10000 #define M3508_MIN_OUT 50.0 #define M3508_MAX_IOUT 40 //M3508 speed PID constants -#define M3508_KP 0.001f -#define M3508_KI 0.0f +#define M3508_KP 0.002f +#define M3508_KI 0.00f #define M3508_KD 0.0f // Current Limiting Constants #define HYSTERESIS_PERIOD 5 From 67d55c2d09e4ea15161666d538bbfc69cbb4914f Mon Sep 17 00:00:00 2001 From: JoshMarangoni Date: Tue, 17 Mar 2020 20:55:59 -0700 Subject: [PATCH 15/22] progress --- CMSIS/stm32f4xx_it.c | 80 ++++++++++++++++++++++++++--- Project/embed-infantry.uvprojx | 10 ---- user/APP/USART_comms/USART_comms.h | 14 ----- user/TASK/gimbal_task/gimbal_task.c | 24 +++++---- user/TASK/gimbal_task/gimbal_task.h | 14 ++++- 5 files changed, 101 insertions(+), 41 deletions(-) diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index e8d3f32..dfc724d 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -33,9 +33,10 @@ #include "gimbal_task.h" #include "USART_comms.h" -volatile char buffer_rx[100]; +volatile char buffer_rx[BUFFER_SIZE]; //holds 100 bytes int count_rx = 0; +Gimbal_Angles* gimbal_angles; /** @addtogroup Template_Project * @{ @@ -176,6 +177,7 @@ 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 @@ -183,22 +185,23 @@ void USART6_IRQHandler(void) uint16_t USART_Data = USART_ReceiveData(USART6); // This just echos everything back once a CR (carriage return) - // character (13 in ascii) is received. Windows (or Putty, idk) - // appends a CR to enter/return so it should work + // character (13 in ascii) is received. Useful if debugging + // from Putty which appends CR if enter key is pressed. if (USART_Data == 13) { buffer_rx[count_rx] = 0; serial_send_string(buffer_rx); + // clear the buffer and counter count_rx = 0; - for (int i = 0; i < 100; i++) { + for (int i = 0; i < BUFFER_SIZE; i++) { buffer_rx[i] = 0; } - } else if (count_rx < 100 - 1) { + } else if (count_rx < BUFFER_SIZE - 1) { buffer_rx[count_rx++] = USART_Data; } else { count_rx = 0; - for (int i = 0; i < 100; i++) { + for (int i = 0; i < BUFFER_SIZE; i++) { buffer_rx[i] = 0; } } @@ -206,6 +209,69 @@ void USART6_IRQHandler(void) } } +int hex_char_to_int(char c){ + switch(c) { + case '0': return 0b0000; + case '1': return 0b0001; + case '2': return 0b0010; + case '3': return 0b0011; + case '4': return 0b0100; + case '5': return 0b0101; + case '6': return 0b0110; + case '7': return 0b0111; + case '8': return 0b1000; + case '9': return 0b1001; + case 'a': return 0b1010; + case 'b': return 0b1011; + case 'c': return 0b1100; + case 'd': return 0b1101; + case 'e': return 0b1110; + case 'f': return 0b1111; + default: return 0; + } + return 0; +} + +void clear_gimbal_buffer(void) { + Gimbal_Angles *gimbal_angles = get_gimbal_angle_struct(); + uint16_t angle = 0; + + for (int i=0; i < gimbal_angles->count_rx; i++) { + angle = angle << 4; + angle += hex_char_to_int(gimbal_angles->buffer_rx[i]); + } + + 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 USART8_IRQHandler(void) +{ + // make sure USART8 was intended to be called for this interrupt + if(USART_GetITStatus(UART8, USART_IT_RXNE) != RESET) { + uint16_t USART_Data = USART_ReceiveData(UART8); + Gimbal_Angles *gimbal_angles = get_gimbal_angle_struct(); + + // Once a carriage return is read, move data into struct, and + // clear the buffer + if (USART_Data == 13 || gimbal_angles->count_rx >= BUFFER_SIZE-1) { + clear_gimbal_buffer(); + } + else { + buffer_rx[count_rx++] = USART_Data; + } + } +} + +/* // Interrupt handler for USART 8. This is called on the reception of every // byte on USART8 void USART8_IRQHandler(void) @@ -290,6 +356,6 @@ void USART8_IRQHandler(void) } } - +*/ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index 43b600f..8d95077 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -859,16 +859,6 @@ 5 ..\user\APP\PID\pid.h - - vision.c - 1 - .\vision.c - - - vision.h - 5 - .\vision.h - diff --git a/user/APP/USART_comms/USART_comms.h b/user/APP/USART_comms/USART_comms.h index f81fd96..c7d993d 100644 --- a/user/APP/USART_comms/USART_comms.h +++ b/user/APP/USART_comms/USART_comms.h @@ -2,24 +2,10 @@ #define USART_COMMS_H #include "main.h" -#define READY 1 -#define NOT_READY 0 - 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); -typedef struct -{ - uint8_t num_filled; - uint8_t packets[3]; - uint16_t integer; - uint16_t decimal; - uint8_t checksum; - -} Gimbal_buffer; - #endif diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 95dcb48..32ce492 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -35,20 +35,20 @@ // This is accessbile globally and some data is loaded from INS_task Gimbal_t gimbal; -Gimbal_buffer *gimbal_buff; +Gimbal_Angles gimbal_angles; //UART mailbox char str[32] = {0}; /******************** Functions ********************/ -static void initialization(Gimbal_t *gimbal); +static void initialization(Gimbal_t *gimbal, Gimbal_Angles *gimbal_angles); static void get_new_data(Gimbal_t *gimbal); static void update_setpoints(Gimbal_t *gimbal); static void increment_PID(Gimbal_t *gimbal); -const Gimbal_buffer* get_gimbal_angle_buffer(void) +extern const Gimbal_Angles* get_gimbal_angle_buffer(void) { - return &gimbal_buff; + return &gimbal_angles; } @@ -61,11 +61,11 @@ const Gimbal_buffer* get_gimbal_angle_buffer(void) void gimbal_task(void* parameters){ vTaskDelay(GIMBAL_INIT_DELAY); - initialization(&gimbal); + initialization(&gimbal, &gimbal_angles); while(1){ - serial_send_string((char)READY); //sends 1 to Python //send_to_uart(&gimbal); + send_vision_angle(); /* For now using strictly encoder feedback for position */ @@ -91,7 +91,7 @@ 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(); @@ -106,8 +106,8 @@ static void initialization(Gimbal_t *gimbal_ptr){ gimbal_ptr->pitch_motor.pos_set = 5500; gimbal_ptr->yaw_motor.pos_set = 6000; - gimbal_buff = get_gimbal_angle_buffer(); - gimbal_buff->num_filled = 0; + gimbal_angles->count_rx = 0; + gimbal_angles->new_angle_flag = 0; } /** @@ -196,3 +196,9 @@ void send_to_uart(Gimbal_t *gimbal_msg) //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 bbdcf59..752a435 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -48,6 +48,7 @@ #define PITCH_MIN 5500 #define PITCH_MAX 7000 +#define BUFFER_SIZE 100 /************************** Gimbal Data Structures ***************************/ typedef struct @@ -77,14 +78,25 @@ typedef struct Shoot_t *launcher; } Gimbal_t; +///////////////////////// +typedef struct +{ + uint8_t new_angle_flag; + uint16_t yaw_angle; + uint16_t 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 const Gimbal_buffer* get_gimbal_angle_buffer(void); +extern const Gimbal_Angles* get_gimbal_angle_struct(void); /******************************* Variable Declarations ***********************/ // These will later be produced from RC From 957b4d78cc1f35b9a2f8882cbcb6cfb117e85706 Mon Sep 17 00:00:00 2001 From: Benji Date: Tue, 17 Mar 2020 21:24:43 -0700 Subject: [PATCH 16/22] uses vision delta angles to set gimbal setpoint --- user/TASK/gimbal_task/gimbal_task.c | 8 +++----- user/TASK/gimbal_task/gimbal_task.h | 2 ++ 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 67f5287..0407ba5 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -187,16 +187,14 @@ static void update_setpoints(Gimbal_t *gimbal_set){ //yaw: //rc -> theta -> complex rotation -> new setpoint - + // TODO: FIX if(gimbal_set->rc_update->rc.s[0] == RC_SW_MID || gimbal_set->rc_update->rc.s[0] == RC_SW_UP){ - fp32 theta = -1 * int16_deadzone(gimbal_set->rc_update->rc.ch[2], -DEADBAND, DEADBAND) - * Motor_Ecd_to_Rad / 80.0f; + fp32 theta = gimbal_angles.yaw_angle * Degree_To_Rad; fp32 rotation[2] = {cos(theta), sin(theta)}; // TODO: consider alternative {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_set->rc_update->rc.ch[3], -DEADBAND, DEADBAND) / 80.0f; + gimbal_set->pitch_motor.pos_set += int16_deadzone(gimbal_angles.pitch_angle, -DEADBAND, DEADBAND) / 80.0f; } char print[30]; diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index a07ddd4..1149d19 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -19,6 +19,7 @@ /*************** Converts between motor position and degrees *****************/ #define Motor_Ecd_to_Rad 0.000766990394f /* 2PI / 8191 */ +#define Degree_To_Rad 0.0087266389; #define FALSE 0 #define TRUE 1 @@ -84,6 +85,7 @@ typedef struct } Gimbal_t; ///////////////////////// +// Degrees relative typedef struct { uint8_t new_angle_flag; From 5ae3f9931a3e2d12c4fe289cd40e3dc38ab2fb69 Mon Sep 17 00:00:00 2001 From: JoshMarangoni Date: Wed, 18 Mar 2020 01:04:55 -0700 Subject: [PATCH 17/22] progressa --- CMSIS/stm32f4xx_it.c | 72 +++++++++++++++++------------ Project/embed-infantry.uvprojx | 5 +- user/TASK/gimbal_task/gimbal_task.c | 28 +++-------- user/TASK/gimbal_task/gimbal_task.h | 4 +- 4 files changed, 52 insertions(+), 57 deletions(-) diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index dfc724d..0dfc27f 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -36,8 +36,6 @@ volatile char buffer_rx[BUFFER_SIZE]; //holds 100 bytes int count_rx = 0; -Gimbal_Angles* gimbal_angles; - /** @addtogroup Template_Project * @{ */ @@ -183,25 +181,24 @@ void USART6_IRQHandler(void) // make sure USART6 was intended to be called for this interrupt if(USART_GetITStatus(USART6, USART_IT_RXNE) != RESET) { uint16_t USART_Data = USART_ReceiveData(USART6); - + // This just echos everything back once a CR (carriage return) - // character (13 in ascii) is received. Useful if debugging - // from Putty which appends CR if enter key is pressed. + // character (13 in ascii) is received. Windows (or Putty, idk) + // appends a CR to enter/return so it should work if (USART_Data == 13) { buffer_rx[count_rx] = 0; serial_send_string(buffer_rx); - // clear the buffer and counter count_rx = 0; - for (int i = 0; i < BUFFER_SIZE; i++) { + for (int i = 0; i < 100; i++) { buffer_rx[i] = 0; } - } else if (count_rx < BUFFER_SIZE - 1) { + } else if (count_rx < 100 - 1) { buffer_rx[count_rx++] = USART_Data; } else { count_rx = 0; - for (int i = 0; i < BUFFER_SIZE; i++) { + for (int i = 0; i < 100; i++) { buffer_rx[i] = 0; } } @@ -210,26 +207,42 @@ void USART6_IRQHandler(void) } int hex_char_to_int(char c){ - switch(c) { - case '0': return 0b0000; - case '1': return 0b0001; - case '2': return 0b0010; - case '3': return 0b0011; - case '4': return 0b0100; - case '5': return 0b0101; - case '6': return 0b0110; - case '7': return 0b0111; - case '8': return 0b1000; - case '9': return 0b1001; - case 'a': return 0b1010; - case 'b': return 0b1011; - case 'c': return 0b1100; - case 'd': return 0b1101; - case 'e': return 0b1110; - case 'f': return 0b1111; - default: return 0; - } - return 0; + int num; + if (c == '0') + num = 0x0; + else if (c == '1') + num = 0x1; + else if (c == '2') + num = 0x2; + else if (c == '3') + num = 0x3; + else if (c == '4') + num = 0x4; + else if (c == '5') + num = 0x5; + else if (c == '6') + num = 0x6; + else if (c =='7') + num = 0x7; + else if (c =='8') + num = 0x8; + else if (c =='9') + num = 0x9; + else if (c =='a') + num = 0xa; + else if (c == 'b') + num = 0xb; + else if (c == 'c') + num = 0xc; + else if (c == 'd') + num = 0xd; + else if (c == 'e') + num = 0xe; + else if (c =='f') + num = 0xf; + else + num = 0; + return num; } void clear_gimbal_buffer(void) { @@ -263,6 +276,7 @@ void USART8_IRQHandler(void) // Once a carriage return is read, move data into struct, and // clear the buffer if (USART_Data == 13 || gimbal_angles->count_rx >= BUFFER_SIZE-1) { + vision_send_string(gimbal_angles->buffer_rx); //echo clear_gimbal_buffer(); } else { diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index fe93f9c..8d95077 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -10,8 +10,7 @@ embed-infantry 0x4 ARM-ADS - 5060750::V5.06 update 6 (build 750)::ARMCC - 0 + 5060422::V5.06 update 4 (build 422)::ARMCC STM32F427IIHx @@ -184,7 +183,6 @@ 0 0 2 - 0 1 0 8 @@ -325,7 +323,6 @@ 0 0 1 - 0 0 3 3 diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 0407ba5..397d101 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -38,7 +38,7 @@ Gimbal_Angles gimbal_angles; //UART mailbox char str[32] = {0}; // TODO: figure out why ins_task is using this... delete and compile to reproduce -static char message[64] = {0}; +//static char message[64] = {0}; static int loop_counter = 0; /******************** Functions ********************/ @@ -48,7 +48,8 @@ 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); -extern const Gimbal_Angles* get_gimbal_angle_buffer(void) + +Gimbal_Angles* get_gimbal_angle_struct(void) { return &gimbal_angles; } @@ -88,7 +89,7 @@ void gimbal_task(void* parameters){ while(1){ //send_to_uart(&gimbal); - send_vision_angle(); + //send_vision_angle(); /* For now using strictly encoder feedback for position */ @@ -242,24 +243,6 @@ static void increment_PID(Gimbal_t *gimbal_pid){ } -/** - * @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; -} - - /** @@ -306,9 +289,10 @@ static void send_feedback_to_uart(Gimbal_t *gimbal){ counter = (counter + 1) % 1000; */ } - +/* 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 1149d19..7f2c2b8 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -100,10 +100,10 @@ typedef struct int get_vision_signal(void); extern void gimbal_task(void *pvParameters); extern void send_to_uart(Gimbal_t *gimbal); -void send_vision_angle(); +//void send_vision_angle(); // get a pointer to the gimbal struct -extern const Gimbal_Angles* get_gimbal_angle_struct(void); +extern Gimbal_Angles* get_gimbal_angle_struct(void); /******************************* Variable Declarations ***********************/ // These will later be produced from RC From 17378ccfacd4f32b80b920f0bf190c47d0a51679 Mon Sep 17 00:00:00 2001 From: JoshMarangoni Date: Wed, 18 Mar 2020 14:29:07 -0700 Subject: [PATCH 18/22] send and receive working UART8 --- CMSIS/stm32f4xx_it.c | 159 +++++++----------------------------- user/hardware/usart/usart.c | 2 +- 2 files changed, 32 insertions(+), 129 deletions(-) diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index 0dfc27f..98b3927 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -32,10 +32,17 @@ #include "main.h" #include "gimbal_task.h" #include "USART_comms.h" +#include 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 * @{ */ @@ -206,53 +213,10 @@ void USART6_IRQHandler(void) } } -int hex_char_to_int(char c){ - int num; - if (c == '0') - num = 0x0; - else if (c == '1') - num = 0x1; - else if (c == '2') - num = 0x2; - else if (c == '3') - num = 0x3; - else if (c == '4') - num = 0x4; - else if (c == '5') - num = 0x5; - else if (c == '6') - num = 0x6; - else if (c =='7') - num = 0x7; - else if (c =='8') - num = 0x8; - else if (c =='9') - num = 0x9; - else if (c =='a') - num = 0xa; - else if (c == 'b') - num = 0xb; - else if (c == 'c') - num = 0xc; - else if (c == 'd') - num = 0xd; - else if (c == 'e') - num = 0xe; - else if (c =='f') - num = 0xf; - else - num = 0; - return num; -} void clear_gimbal_buffer(void) { Gimbal_Angles *gimbal_angles = get_gimbal_angle_struct(); - uint16_t angle = 0; - - for (int i=0; i < gimbal_angles->count_rx; i++) { - angle = angle << 4; - angle += hex_char_to_int(gimbal_angles->buffer_rx[i]); - } + uint16_t angle = atoi(gimbal_angles->buffer_rx); gimbal_angles->yaw_angle = angle; gimbal_angles->new_angle_flag = 1; @@ -266,110 +230,49 @@ void clear_gimbal_buffer(void) { // 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 USART8_IRQHandler(void) +void UART8_IRQHandler(void) { - // make sure USART8 was intended to be called for this interrupt + // make sure UART8 was intended to be called for this interrupt if(USART_GetITStatus(UART8, USART_IT_RXNE) != RESET) { - uint16_t USART_Data = USART_ReceiveData(UART8); + uint16_t UART_Data = USART_ReceiveData(UART8); + Gimbal_Angles *gimbal_angles = get_gimbal_angle_struct(); - // Once a carriage return is read, move data into struct, and - // clear the buffer - if (USART_Data == 13 || gimbal_angles->count_rx >= BUFFER_SIZE-1) { + // 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(); } else { - buffer_rx[count_rx++] = USART_Data; + gimbal_angles->buffer_rx[gimbal_angles->count_rx++] = UART_Data; } - } -} - -/* -// Interrupt handler for USART 8. This is called on the reception of every -// byte on USART8 -void USART8_IRQHandler(void) -{ - // make sure USART8 was intended to be called for this interrupt - if(USART_GetITStatus(UART8, USART_IT_RXNE) != RESET) { - uint16_t USART_Data = USART_ReceiveData(UART8); - serial_send_string(NOT_READY); - - uint8_t mask = 0x01; - Gimbal_buffer *gimbal_buff = get_gimbal_angle_buffer(); - // first 10 bits are decimal, next 9 integer, next 5 checksum - switch(gimbal_buff->num_filled) { - case 0: { - gimbal_buff->packets[0] = (uint8_t)USART_Data; - gimbal_buff->decimal = (uint8_t)USART_Data; - serial_send_string((char)READY); - }; - case 1: { - gimbal_buff->packets[1] = (uint8_t)USART_Data; - uint8_t last_decimal_bits; - uint8_t first_int_bits; - - for (int i=0; i < 8; i++) { - if (i < 2) { - last_decimal_bits += (uint8_t)USART_Data & mask; - last_decimal_bits = last_decimal_bits << 1; - } - else { - first_int_bits += (uint8_t)USART_Data & mask; - first_int_bits = first_int_bits << 1; - } - mask = mask << 1; - } - gimbal_buff->integer = first_int_bits; - gimbal_buff->decimal += last_decimal_bits; - serial_send_string((char)READY); - }; - case 2: { - gimbal_buff->packets[2] = (uint8_t)USART_Data; - uint8_t last_int_bits; - uint8_t checksum_bits; - - for (int i=0; i < 8; i++) { - if (i < 3) { - last_int_bits += (uint8_t)USART_Data & mask; - last_int_bits = last_int_bits << 1; - } - else { - checksum_bits += (uint8_t)USART_Data & mask; - checksum_bits = checksum_bits << 1; - } - mask = mask << 1; - } - gimbal_buff.checksum = checksum_bits; - serial_send_string(&READY); - } - default: - serial_send_string(&READY); - } - - // This just echos everything back once a CR (carriage return) + + // This just echos everything back once a CR (carriage return) // character (13 in ascii) is received. Windows (or Putty, idk) // appends a CR to enter/return so it should work - if (USART_Data == 13) { - buffer_rx[count_rx] = 0; - serial_send_string(buffer_rx); + /* + if (UART_Data == 13) { + UART8_buffer_rx[UART8_count_rx] = 0; + vision_send_string(UART8_buffer_rx); - count_rx = 0; + UART8_count_rx = 0; for (int i = 0; i < 100; i++) { - buffer_rx[i] = 0; + UART8_buffer_rx[i] = 0; } - } else if (count_rx < 100 - 1) { - buffer_rx[count_rx++] = USART_Data; + } else if (UART8_count_rx < 100 - 1) { + UART8_buffer_rx[UART8_count_rx++] = UART_Data; } else { - count_rx = 0; + UART8_count_rx = 0; for (int i = 0; i < 100; i++) { - buffer_rx[i] = 0; + UART8_buffer_rx[i] = 0; } } - + */ } } -*/ + /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/user/hardware/usart/usart.c b/user/hardware/usart/usart.c index 5be1712..8fa5cbc 100644 --- a/user/hardware/usart/usart.c +++ b/user/hardware/usart/usart.c @@ -89,7 +89,7 @@ void USART_8_INIT(void) USART_DeInit(UART8); // Configure UART - this all needs to match the config on the Pi - USART_InitStructure.USART_BaudRate = 9600; // idk what max is so 9600 to be safe + 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 From 0ce11cec88ecd0896609cf1d185c9b485e31dedd Mon Sep 17 00:00:00 2001 From: JoshMarangoni Date: Wed, 18 Mar 2020 14:49:47 -0700 Subject: [PATCH 19/22] v2e fully coded --- CMSIS/stm32f4xx_it.c | 11 +++++++++-- user/TASK/gimbal_task/gimbal_task.c | 8 +++++++- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index 98b3927..df886c3 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -33,6 +33,7 @@ #include "gimbal_task.h" #include "USART_comms.h" #include +#include volatile char buffer_rx[BUFFER_SIZE]; //holds 100 bytes int count_rx = 0; @@ -215,8 +216,12 @@ void USART6_IRQHandler(void) void clear_gimbal_buffer(void) { + char str[20]; Gimbal_Angles *gimbal_angles = get_gimbal_angle_struct(); + uint16_t angle = atoi(gimbal_angles->buffer_rx); + sprintf(str, "delta angle: %d\n\r", angle); + vision_send_string(str); gimbal_angles->yaw_angle = angle; gimbal_angles->new_angle_flag = 1; @@ -241,9 +246,11 @@ void UART8_IRQHandler(void) // 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); + //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; diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 397d101..81ff4bf 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -95,7 +95,11 @@ void gimbal_task(void* parameters){ get_new_data(&gimbal); //send_to_uart(&gimbal); - update_setpoints(&gimbal); + + if (gimbal_angles.new_angle_flag) { + update_setpoints(&gimbal); + gimbal_angles.new_angle_flag = 0; + } //send_to_uart(&gimbal); increment_PID(&gimbal); //send_to_uart(&gimbal); @@ -139,6 +143,8 @@ static void initialization(Gimbal_t *gimbal_ptr, Gimbal_Angles *gimbal_angles){ 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 } From 48443494f9dbe0e1ba7ab6a6cf54fb1ad12f612f Mon Sep 17 00:00:00 2001 From: JoshMarangoni Date: Wed, 18 Mar 2020 17:13:38 -0700 Subject: [PATCH 20/22] successfully reading angles from vision --- CMSIS/stm32f4xx_it.c | 33 +++++------------------------ user/TASK/gimbal_task/gimbal_task.c | 2 ++ user/TASK/gimbal_task/gimbal_task.h | 4 ++-- 3 files changed, 9 insertions(+), 30 deletions(-) diff --git a/CMSIS/stm32f4xx_it.c b/CMSIS/stm32f4xx_it.c index df886c3..abd8bdf 100644 --- a/CMSIS/stm32f4xx_it.c +++ b/CMSIS/stm32f4xx_it.c @@ -216,12 +216,12 @@ void USART6_IRQHandler(void) void clear_gimbal_buffer(void) { - char str[20]; Gimbal_Angles *gimbal_angles = get_gimbal_angle_struct(); - uint16_t angle = atoi(gimbal_angles->buffer_rx); - sprintf(str, "delta angle: %d\n\r", angle); - vision_send_string(str); + 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; @@ -240,7 +240,6 @@ 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 @@ -255,31 +254,9 @@ void UART8_IRQHandler(void) else { gimbal_angles->buffer_rx[gimbal_angles->count_rx++] = UART_Data; } - - // This just echos everything back once a CR (carriage return) - // character (13 in ascii) is received. Windows (or Putty, idk) - // appends a CR to enter/return so it should work - /* - if (UART_Data == 13) { - UART8_buffer_rx[UART8_count_rx] = 0; - vision_send_string(UART8_buffer_rx); - - UART8_count_rx = 0; - for (int i = 0; i < 100; i++) { - UART8_buffer_rx[i] = 0; - } - - } else if (UART8_count_rx < 100 - 1) { - UART8_buffer_rx[UART8_count_rx++] = UART_Data; - } else { - UART8_count_rx = 0; - for (int i = 0; i < 100; i++) { - UART8_buffer_rx[i] = 0; - } - } - */ } } + /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 81ff4bf..31c9980 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -97,6 +97,8 @@ void gimbal_task(void* parameters){ //send_to_uart(&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; } diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index 7f2c2b8..9d1a4bc 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -89,8 +89,8 @@ typedef struct typedef struct { uint8_t new_angle_flag; - uint16_t yaw_angle; - uint16_t pitch_angle; + int yaw_angle; + int pitch_angle; uint8_t count_rx; char buffer_rx[BUFFER_SIZE]; } Gimbal_Angles; From 2b7160e9eadd784dcb0f0e6f601bfb9be6dc95af Mon Sep 17 00:00:00 2001 From: JoshMarangoni Date: Wed, 18 Mar 2020 17:44:06 -0700 Subject: [PATCH 21/22] debug --- user/TASK/gimbal_task/gimbal_task.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 31c9980..ffb16cb 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -43,7 +43,7 @@ static int loop_counter = 0; /******************** Functions ********************/ static void initialization(Gimbal_t *gimbal, Gimbal_Angles *gimbal_angles); -static void get_new_data(Gimbal_t *gimbal); +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); @@ -93,7 +93,7 @@ void gimbal_task(void* parameters){ /* For now using strictly encoder feedback for position */ - get_new_data(&gimbal); + get_new_encoder_data(&gimbal); //send_to_uart(&gimbal); if (gimbal_angles.new_angle_flag) { @@ -155,7 +155,7 @@ static void initialization(Gimbal_t *gimbal_ptr, Gimbal_Angles *gimbal_angles){ * @param None * @retval None */ -static void get_new_data(Gimbal_t *gimbal_data){ +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; @@ -209,6 +209,8 @@ static void update_setpoints(Gimbal_t *gimbal_set){ char print[30]; sprintf(print, "Constrain pitch %d between %d, %d \n\r", gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); serial_send_string(print); + sprintf(print, "New set point: %d \n\r", gimbal_set->pitch_motor.pos_set); + serial_send_string(print); //int16_constrain(gimbal_set->yaw_motor.pos_set, YAW_MIN, YAW_MAX); gimbal_set->pitch_motor.pos_set = int16_constrain(gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); sprintf(print, "Constrained to %d \n\r", gimbal_set->pitch_motor.pos_set); From bb879df9cfeca54cb1764f2e85937b7dc6322808 Mon Sep 17 00:00:00 2001 From: Benji Date: Fri, 8 May 2020 23:36:04 -0600 Subject: [PATCH 22/22] cleaned up to help with debugging --- Project/embed-infantry.uvprojx | 7 ++-- user/TASK/gimbal_task/gimbal_task.c | 51 ++++------------------------- 2 files changed, 12 insertions(+), 46 deletions(-) diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index 8d95077..fd6731a 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -11,12 +11,13 @@ 0x4 ARM-ADS 5060422::V5.06 update 4 (build 422)::ARMCC + 0 STM32F427IIHx STMicroelectronics - Keil.STM32F4xx_DFP.2.12.0 - http://www.keil.com/pack + Keil.STM32F4xx_DFP.2.14.0 + http://www.keil.com/pack/ IRAM(0x20000000,0x30000) IRAM2(0x10000000,0x10000) IROM(0x08000000,0x200000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE @@ -183,6 +184,7 @@ 0 0 2 + 0 1 0 8 @@ -323,6 +325,7 @@ 0 0 1 + 0 0 3 3 diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index ffb16cb..688f726 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -54,28 +54,6 @@ Gimbal_Angles* get_gimbal_angle_struct(void) return &gimbal_angles; } -/** -* Slew Control -* While the current is over a limiting constant: -* Reduce control signal by 0.5 every 5 ms -* When current returns the acceptable range, double the -* allowed current every 5 ms until it returns to full output. -* -* Disable/reset I-term at this time whenever current limiting is active.\ -* -* Do we want reductions to follow : 1/2, 1/4, 1/8, ... -* or 1/2, 1/4 , OFF -* Or mutliply out put constant -* Tunable Parameters -* redutions factor: e.g. reduce current by 0.5 -* reduction time: e.g. reduce current every 5 ms -* increase factor -* increase time -* I-term off/reset -* hysteresis time: how long should the current limiter -* wait before starting to increase/decrease current. -*/ - /** * @brief Initializes PID and fetches Gimbal motor data to ensure * gimbal points in direction specified. @@ -193,27 +171,20 @@ static void make_unit_length(fp32 n[2]){ * @retval None */ static void update_setpoints(Gimbal_t *gimbal_set){ - - //yaw: - //rc -> theta -> complex rotation -> new setpoint - - // TODO: FIX - if(gimbal_set->rc_update->rc.s[0] == RC_SW_MID || gimbal_set->rc_update->rc.s[0] == RC_SW_UP){ + if(1){ fp32 theta = gimbal_angles.yaw_angle * Degree_To_Rad; - fp32 rotation[2] = {cos(theta), sin(theta)}; // TODO: consider alternative {cos{theta}, sin{theta}} + 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); char print[30]; - sprintf(print, "Constrain pitch %d between %d, %d \n\r", gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); - serial_send_string(print); - sprintf(print, "New set point: %d \n\r", gimbal_set->pitch_motor.pos_set); + sprintf(print, "Yaw setpoint x: %.4f y: %.4f \n\r", gimbal_set->yaw_setpoint[0], gimbal_set->yaw_setpoint[1]); serial_send_string(print); - //int16_constrain(gimbal_set->yaw_motor.pos_set, YAW_MIN, YAW_MAX); - gimbal_set->pitch_motor.pos_set = int16_constrain(gimbal_set->pitch_motor.pos_set, PITCH_MIN, PITCH_MAX); - sprintf(print, "Constrained to %d \n\r", gimbal_set->pitch_motor.pos_set); + sprintf(print, "Pitch setpoint: %d (encoder position 0-8191)\n\r", gimbal_set->pitch_motor.pos_set); serial_send_string(print); } @@ -236,21 +207,13 @@ static int16_t get_error_sign(fp32 actual_position[2], fp32 set_position[2]){ * @retval None */ static void increment_PID(Gimbal_t *gimbal_pid){ - /* Get error sign - get error val - insert into pid*/ - // error magnitude 1 * ERROR_MULTIPLIER corresponds to gimbal being 90 degrees out of place --> jump action observed by pedram - // TODO: consider revising PID to eliminate the ERROR_MULTIPLIER constant - // TODO: retune PID currently very slow 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; - // TODO: Modify pid to reject massive d term spike this could create. - // TODO: Consider how this isn't quite a linear error (dot product) + 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); - }