From 11a4aefa90aa8d9be7a6777db3c02ca8e1ba4bec Mon Sep 17 00:00:00 2001 From: Benji Date: Sun, 16 Feb 2020 15:36:38 -0800 Subject: [PATCH 1/7] refactored --- user/TASK/INS_task/INS_task.c | 24 ++++---- user/TASK/chassis_task/chassis_task.c | 64 ++++++++++---------- user/TASK/chassis_task/chassis_task.h | 18 +++--- user/TASK/gimbal_task/gimbal_task.c | 84 ++++++++++++++++++--------- user/TASK/gimbal_task/gimbal_task.h | 24 ++++---- 5 files changed, 122 insertions(+), 92 deletions(-) diff --git a/user/TASK/INS_task/INS_task.c b/user/TASK/INS_task/INS_task.c index bd2bd4a..95a0fce 100644 --- a/user/TASK/INS_task/INS_task.c +++ b/user/TASK/INS_task/INS_task.c @@ -383,39 +383,39 @@ extern char str[]; */ void test_imu_readings(uint8_t angle, uint8_t gyro, uint8_t acce){ //Link pointers - gimbal.angle_reading_raw = get_INS_angle_point(); - gimbal.gyro_reading_raw = get_MPU6500_Gyro_Data_Point(); - gimbal.acce_reading_raw = get_MPU6500_Accel_Data_Point(); + gimbal.angle_update = get_INS_angle_point(); + gimbal.gyro_update = get_MPU6500_Gyro_Data_Point(); + gimbal.accel_update = get_MPU6500_Accel_Data_Point(); if (angle == TRUE) { //Sending angle data via UART - sprintf(str, "Angle yaw: %f\n\r", gimbal.angle_reading_raw[INS_YAW_ADDRESS_OFFSET]); + 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_reading_raw[INS_PITCH_ADDRESS_OFFSET]); + 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_reading_raw[INS_ROLL_ADDRESS_OFFSET]); + sprintf(str, "Angle roll: %f\n\r", gimbal.angle_update[INS_ROLL_ADDRESS_OFFSET]); serial_send_string(str); serial_send_string("\n"); } if (gyro == TRUE) { //Sending gyro data via UART - sprintf(str, "Gyro X: %f\n\r", gimbal.gyro_reading_raw[INS_GYRO_X_ADDRESS_OFFSET]); + 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_reading_raw[INS_GYRO_Y_ADDRESS_OFFSET]); + 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_reading_raw[INS_GYRO_Z_ADDRESS_OFFSET]); + sprintf(str, "Gyro Z: %f\n\r", gimbal.gyro_update[INS_GYRO_Z_ADDRESS_OFFSET]); serial_send_string(str); serial_send_string("\n"); } if (acce == TRUE) { //Sending accelerometer data via UART - sprintf(str, "Acce X: %f\n\r", gimbal.acce_reading_raw[INS_ACCEL_X_ADDRESS_OFFSET]); + 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.acce_reading_raw[INS_ACCEL_Y_ADDRESS_OFFSET]); + 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.acce_reading_raw[INS_ACCEL_Z_ADDRESS_OFFSET]); + sprintf(str, "Acce Z: %f\n\r", gimbal.accel_update[INS_ACCEL_Z_ADDRESS_OFFSET]); serial_send_string(str); serial_send_string("\n"); } diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 3499347..f4a5f0d 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -94,10 +94,10 @@ static void chassis_init(Chassis_t *chassis_init){ //Link pointers with CAN motors for (int i = 0; i < 4; i++) { - chassis_init->motor[i].motor_raw = get_Chassis_Motor_Measure_Point(i); - chassis_init->motor[i].speed_raw = chassis_init->motor[i].motor_raw->speed_rpm; - chassis_init->motor[i].pos_raw = chassis_init->motor[i].motor_raw->ecd; - chassis_init->motor[i].current_raw = chassis_init->motor[i].motor_raw->given_current; + chassis_init->motor[i].latest_motor_data = get_Chassis_Motor_Measure_Point(i); + chassis_init->motor[i].speed_read = chassis_init->motor[i].latest_motor_data->speed_rpm; + chassis_init->motor[i].pos_read = chassis_init->motor[i].latest_motor_data->ecd; + chassis_init->motor[i].current_read = chassis_init->motor[i].latest_motor_data->given_current; PID_Init(&chassis_init->motor[i].pid_control, PID_DELTA, def_pid_constants, M3508_MAX_OUT, M3508_MIN_OUT); } @@ -107,7 +107,7 @@ static void chassis_init(Chassis_t *chassis_init){ chassis_init->yaw_pos_raw = chassis_init->vec_raw + INS_YAW_ADDRESS_OFFSET; //Pointer remote - chassis_init->rc_raw = get_remote_control_point(); + chassis_init->rc_update = get_remote_control_point(); } @@ -118,9 +118,9 @@ static void chassis_init(Chassis_t *chassis_init){ */ static void get_new_data(Chassis_t *chassis_update){ for (int i = 0; i < 4; i++) { - chassis_update->motor[i].speed_raw = chassis_update->motor[i].motor_raw->speed_rpm; - chassis_update->motor[i].pos_raw = chassis_update->motor[i].motor_raw->ecd; - chassis_update->motor[i].current_raw = chassis_update->motor[i].motor_raw->given_current; + chassis_update->motor[i].speed_read = chassis_update->motor[i].latest_motor_data->speed_rpm; + chassis_update->motor[i].pos_read = chassis_update->motor[i].latest_motor_data->ecd; + chassis_update->motor[i].current_read = chassis_update->motor[i].latest_motor_data->given_current; } } @@ -146,31 +146,31 @@ static void set_control_mode(void){ */ static void calculate_chassis_motion_setpoints(chassis_user_mode_e mode){ - //Get remote control data and put into x_speed_raw etc + //Get remote control data and put into x_speed_read etc //process based on mode (which is currently none) and put into x_speed_set //Debug print out current // get rc data and put into chassis struct //Switch Chassis Only - if(switch_is_down(chassis.rc_raw->rc.s[0])){ - chassis.x_speed_raw = chassis.rc_raw->rc.ch[RC_X]; - chassis.y_speed_raw = chassis.rc_raw->rc.ch[RC_Y]; - chassis.z_speed_raw = chassis.rc_raw->rc.ch[RC_Z]; - }else if(switch_is_mid(chassis.rc_raw->rc.s[0])){ - chassis.x_speed_raw = 0; - chassis.y_speed_raw = chassis.rc_raw->rc.ch[RC_Y]; - chassis.z_speed_raw = chassis.rc_raw->rc.ch[RC_Z]; - }else if(switch_is_up(chassis.rc_raw->rc.s[0])){ - chassis.x_speed_raw = 0; - chassis.y_speed_raw = 0; - chassis.z_speed_raw = 0; + if(switch_is_down(chassis.rc_update->rc.s[0])){ + chassis.x_speed_read = chassis.rc_update->rc.ch[RC_X]; + chassis.y_speed_read = chassis.rc_update->rc.ch[RC_Y]; + chassis.z_speed_read = chassis.rc_update->rc.ch[RC_Z]; + }else if(switch_is_mid(chassis.rc_update->rc.s[0])){ + chassis.x_speed_read = 0; + chassis.y_speed_read = chassis.rc_update->rc.ch[RC_Y]; + chassis.z_speed_read = chassis.rc_update->rc.ch[RC_Z]; + }else if(switch_is_up(chassis.rc_update->rc.s[0])){ + chassis.x_speed_read = 0; + chassis.y_speed_read = 0; + chassis.z_speed_read = 0; } // process raw sppeds based on modes (for now they are just the same) - chassis.x_speed_set = chassis.x_speed_raw; - chassis.y_speed_set = chassis.y_speed_raw; - chassis.z_speed_set = chassis.z_speed_raw; + chassis.x_speed_set = chassis.x_speed_read; + chassis.y_speed_set = chassis.y_speed_read; + chassis.z_speed_set = chassis.z_speed_read; } @@ -207,47 +207,47 @@ static void increment_PID(uint8_t debug){ fp32 back_left; front_right = PID_Calc(&chassis.motor[FRONT_RIGHT].pid_control, - chassis.motor[FRONT_RIGHT].speed_raw, + chassis.motor[FRONT_RIGHT].speed_read, chassis.motor[FRONT_RIGHT].speed_set); chassis.motor[FRONT_RIGHT].speed_out += front_right; back_right = PID_Calc(&chassis.motor[BACK_RIGHT].pid_control, - chassis.motor[BACK_RIGHT].speed_raw, + chassis.motor[BACK_RIGHT].speed_read, chassis.motor[BACK_RIGHT].speed_set); chassis.motor[BACK_RIGHT].speed_out += back_right; front_left = PID_Calc(&chassis.motor[FRONT_LEFT].pid_control, - chassis.motor[FRONT_LEFT].speed_raw, + chassis.motor[FRONT_LEFT].speed_read, chassis.motor[FRONT_LEFT].speed_set); chassis.motor[FRONT_LEFT].speed_out += front_left; back_left = PID_Calc(&chassis.motor[BACK_LEFT].pid_control, - chassis.motor[BACK_LEFT].speed_raw, + chassis.motor[BACK_LEFT].speed_read, chassis.motor[BACK_LEFT].speed_set); chassis.motor[BACK_LEFT].speed_out += back_left; if (debug == 1) { sprintf(pid_out, "Front Right - target: %d, sensor: %d, output: %d \n\r", chassis.motor[FRONT_RIGHT].speed_set, - chassis.motor[FRONT_RIGHT].speed_raw, + chassis.motor[FRONT_RIGHT].speed_read, chassis.motor[FRONT_RIGHT].speed_out); serial_send_string(pid_out); sprintf(pid_out, "Back Right - target: %d, sensor: %d, output: %d \n\r", chassis.motor[BACK_RIGHT].speed_set, - chassis.motor[BACK_RIGHT].speed_raw, + chassis.motor[BACK_RIGHT].speed_read, chassis.motor[BACK_RIGHT].speed_out); serial_send_string(pid_out); sprintf(pid_out, "Front Left - target: %d, sensor: %d, output: %d \n\r", chassis.motor[FRONT_LEFT].speed_set, - chassis.motor[FRONT_LEFT].speed_raw, + chassis.motor[FRONT_LEFT].speed_read, chassis.motor[FRONT_LEFT].speed_out); serial_send_string(pid_out); sprintf(pid_out, "Back Left - target: %d, sensor: %d, output: %d \n\r", chassis.motor[BACK_LEFT].speed_set, - chassis.motor[BACK_LEFT].speed_raw, + chassis.motor[BACK_LEFT].speed_read, chassis.motor[BACK_LEFT].speed_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 5560f55..3c1e76c 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -41,12 +41,12 @@ typedef struct { - const motor_measure_t *motor_raw; + const motor_measure_t *latest_motor_data; //Current speed read from motors - int16_t pos_raw; - int16_t speed_raw; - int16_t current_raw; + int16_t pos_read; + int16_t speed_read; + int16_t current_read; //Target speed set by user/remote control int16_t pos_set; @@ -65,16 +65,16 @@ typedef struct Chassis_Motor_t motor[4]; //Raw remote control data - const RC_ctrl_t *rc_raw; + const RC_ctrl_t *rc_update; - //Current front vector + //Current front vectord const fp32 *vec_raw; const fp32 *yaw_pos_raw; //Current speed, vector combination of the speed read from motors - int16_t x_speed_raw; - int16_t y_speed_raw; - int16_t z_speed_raw; + int16_t x_speed_read; + int16_t y_speed_read; + int16_t z_speed_read; //Speed set by user/remote control int16_t x_speed_set; diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index fa4d7b7..49be2ad 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -32,9 +32,17 @@ float rc_channel_2; const RC_ctrl_t* rc_ptr; Gimbal_t gimbal; +PidTypeDef pid_pitch; +PidTypeDef pid_yaw; + //UART mailbox char str[32] = {0}; +/******************** Functions ********************/ +static void initialisation(void); +static void get_new_data(void); +static void increment_PID(void); + /** * @brief Initialises PID and fetches Gimbal motor data to ensure @@ -45,32 +53,14 @@ char str[32] = {0}; void gimbal_task(void* parameters){ vTaskDelay(GIMBAL_TASK_INIT_TIME); - - gimbal.pitch_motor->gimbal_motor_raw = get_Pitch_Gimbal_Motor_Measure_Point(); - gimbal.yaw_motor->gimbal_motor_raw = get_Yaw_Gimbal_Motor_Measure_Point(); - - fp32 pid_constants[3] = {pid_kp, pid_ki, pid_kd}; - PidTypeDef pid_pitch; - PID_Init(&pid_pitch, PID_POSITION, pid_constants, max_out, max_iout); - - PidTypeDef pid_yaw; - PID_Init(&pid_yaw, PID_POSITION, pid_constants, max_out, max_iout); - - - rc_ptr = get_remote_control_point(); + initialisation(); while(1){ /* For now using strictly encoder feedback for position */ + get_new_data(); - // Get CAN received data - gimbal.pitch_pos_raw = gimbal.pitch_motor->gimbal_motor_raw->ecd; - gimbal.pitch_speed_raw = gimbal.pitch_motor->gimbal_motor_raw->speed_rpm; - - gimbal.yaw_pos_raw = gimbal.yaw_motor->gimbal_motor_raw->ecd; - gimbal.yaw_speed_raw = gimbal.yaw_motor->gimbal_motor_raw->speed_rpm; - - // Calculate setpoints based on RC signal. + // Calculate setpoints based on RC signal. //TODO:Place in function if(switch_is_mid(rc_ptr->rc.s[0]) || switch_is_up(rc_ptr->rc.s[0])){ gimbal.yaw_pos_set = linear_map_int_to_int(rc_ptr->rc.ch[2], RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); gimbal.pitch_pos_set = linear_map_int_to_int(rc_ptr->rc.ch[3], RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); @@ -79,11 +69,13 @@ void gimbal_task(void* parameters){ gimbal.pitch_pos_set = linear_map_int_to_int(0, RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); } - gimbal.pitch_speed_set = PID_Calc(&pid_pitch, gimbal.pitch_motor->pos_raw, gimbal.pitch_pos_set); - gimbal.yaw_speed_set = PID_Calc(&pid_yaw, gimbal.yaw_motor->pos_raw, gimbal.yaw_pos_set); + increment_PID(); // Turn gimbal motor - CAN_CMD_GIMBAL(gimbal.yaw_speed_set, gimbal.pitch_speed_set, 0, 0); + CAN_CMD_GIMBAL( gimbal.yaw_speed_set, + gimbal.pitch_speed_set, + 0, + 0); vTaskDelay(1); @@ -91,6 +83,44 @@ void gimbal_task(void* parameters){ } } +/** + * @brief Initialises gimbal struct and loads pointers for RC and motor feedback + * @param None + * @retval None + */ +static void initialisation(void){ + gimbal.pitch_motor->gimbal_motor_feedback = get_Pitch_Gimbal_Motor_Measure_Point(); + gimbal.yaw_motor->gimbal_motor_feedback = get_Yaw_Gimbal_Motor_Measure_Point(); + + fp32 pid_constants[3] = {pid_kp, pid_ki, pid_kd}; + + PID_Init(&pid_pitch, PID_POSITION, pid_constants, max_out, max_iout); + + + PID_Init(&pid_yaw, PID_POSITION, pid_constants, max_out, max_iout); + + rc_ptr = get_remote_control_point(); +} + +/** + * @brief Update encoder positions + * @param None + * @retval None + */ +static void get_new_data(void){ + // Get CAN received data + gimbal.pitch_pos_read = gimbal.pitch_motor->gimbal_motor_feedback->ecd; + gimbal.pitch_speed_read = gimbal.pitch_motor->gimbal_motor_feedback->speed_rpm; + + gimbal.yaw_pos_read = gimbal.yaw_motor->gimbal_motor_feedback->ecd; + gimbal.yaw_speed_read = gimbal.yaw_motor->gimbal_motor_feedback->speed_rpm; +} + +static void increment_PID(void){ + gimbal.pitch_speed_set = PID_Calc(&pid_pitch, gimbal.pitch_motor->pos_read, gimbal.pitch_pos_set); + gimbal.yaw_speed_set = PID_Calc(&pid_yaw, gimbal.yaw_motor->pos_read, gimbal.yaw_pos_set); +} + @@ -124,13 +154,13 @@ void send_to_uart(Gimbal_Motor_t gimbal_yaw_motor, PidTypeDef pid_pitch, fp32 pi { char str[20]; //uart data buffer - sprintf(str, "position: %d\n\r", gimbal.yaw_motor->pos_raw); + sprintf(str, "position: %d\n\r", gimbal.yaw_motor->pos_read); serial_send_string(str); - sprintf(str, "speed: %d\n\r", gimbal.yaw_motor->speed_raw); + sprintf(str, "speed: %d\n\r", gimbal.yaw_motor->speed_read); serial_send_string(str); - sprintf(str, "current: %d\n\r", gimbal.yaw_motor->current_raw); + sprintf(str, "current: %d\n\r", gimbal.yaw_motor->current_read); serial_send_string(str); sprintf(str, "motor kp: %f\n\r", pid_pitch.Kp); diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index e7962ec..b55d843 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -39,27 +39,27 @@ /************************** Gimbal Data Structures ***************************/ typedef struct { - const motor_measure_t *gimbal_motor_raw; - uint16_t pos_raw; - uint16_t speed_raw; - uint16_t current_raw; + const motor_measure_t *gimbal_motor_feedback; + uint16_t pos_read; + uint16_t speed_read; + uint16_t current_read; } Gimbal_Motor_t; typedef struct { Gimbal_Motor_t *yaw_motor; Gimbal_Motor_t *pitch_motor; - const fp32 *angle_reading_raw; - const fp32 *gyro_reading_raw; - const fp32 *acce_reading_raw; - - uint16_t pitch_pos_raw; - uint16_t yaw_pos_raw; + const fp32 *angle_update; + const fp32 *gyro_update; + const fp32 *accel_update; + + uint16_t pitch_pos_read; + uint16_t yaw_pos_read; uint16_t pitch_pos_set; uint16_t yaw_pos_set; - int16_t pitch_speed_raw; - int16_t yaw_speed_raw; + int16_t pitch_speed_read; + int16_t yaw_speed_read; int16_t pitch_speed_set; int16_t yaw_speed_set; } Gimbal_t; From 6eb2661a6fe6321423646400f42999c98372907d Mon Sep 17 00:00:00 2001 From: Benji Date: Fri, 21 Feb 2020 21:45:41 -0800 Subject: [PATCH 2/7] now builds with refactor --- user/TASK/INS_task/INS_task.c | 4 +- user/TASK/chassis_task/chassis_task.c | 69 ++++++++++++------------ user/TASK/chassis_task/chassis_task.h | 21 +++++--- user/TASK/gimbal_task/gimbal_task.c | 77 ++++++++++++++------------- user/TASK/gimbal_task/gimbal_task.h | 36 +++++++------ 5 files changed, 108 insertions(+), 99 deletions(-) diff --git a/user/TASK/INS_task/INS_task.c b/user/TASK/INS_task/INS_task.c index 95a0fce..8211962 100644 --- a/user/TASK/INS_task/INS_task.c +++ b/user/TASK/INS_task/INS_task.c @@ -381,7 +381,7 @@ extern char str[]; * @param acce: displays accelerometer data if TRUE * @retval None */ -void test_imu_readings(uint8_t angle, uint8_t gyro, uint8_t acce){ +void test_imu_readings(uint8_t angle, uint8_t gyro, uint8_t accel){ //Link pointers gimbal.angle_update = get_INS_angle_point(); gimbal.gyro_update = get_MPU6500_Gyro_Data_Point(); @@ -409,7 +409,7 @@ void test_imu_readings(uint8_t angle, uint8_t gyro, uint8_t acce){ serial_send_string("\n"); } - if (acce == TRUE) { + 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); diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index f4a5f0d..1cdc25e 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -28,10 +28,9 @@ /******************** Private User Declarations ********************/ static void chassis_init(Chassis_t *chassis_init); static void get_new_data(Chassis_t *chassis_update); -static void set_control_mode(void); +static void set_control_mode(Chassis_t *chassis); static void calculate_chassis_motion_setpoints(chassis_user_mode_e mode); -static void set_control_mode(void); -static void calculate_motor_setpoints(void); +static void calculate_motor_setpoints(Chassis_t *chassis); static void increment_PID(uint8_t debug); static Chassis_t chassis; @@ -54,15 +53,15 @@ void chassis_task(void *pvParameters){ while(1) { get_new_data(&chassis); //updates RC commands and CAN motor feedback - set_control_mode(); //Note: currently not implemented + set_control_mode(&chassis); //Note: currently not implemented calculate_chassis_motion_setpoints(CHASSIS_VECTOR_RAW); - calculate_motor_setpoints(); + calculate_motor_setpoints(&chassis); increment_PID(FALSE); //output - CAN_CMD_CHASSIS(chassis.motor[FRONT_RIGHT].speed_out, - chassis.motor[FRONT_LEFT].speed_out, - chassis.motor[BACK_LEFT].speed_out, - chassis.motor[BACK_RIGHT].speed_out); + CAN_CMD_CHASSIS(chassis.motor[FRONT_RIGHT].current_out, + chassis.motor[FRONT_LEFT].current_out, + chassis.motor[BACK_LEFT].current_out, + chassis.motor[BACK_RIGHT].current_out); vTaskDelay(1); } @@ -94,12 +93,12 @@ static void chassis_init(Chassis_t *chassis_init){ //Link pointers with CAN motors for (int i = 0; i < 4; i++) { - chassis_init->motor[i].latest_motor_data = get_Chassis_Motor_Measure_Point(i); - chassis_init->motor[i].speed_read = chassis_init->motor[i].latest_motor_data->speed_rpm; - chassis_init->motor[i].pos_read = chassis_init->motor[i].latest_motor_data->ecd; - chassis_init->motor[i].current_read = chassis_init->motor[i].latest_motor_data->given_current; + 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; - PID_Init(&chassis_init->motor[i].pid_control, PID_DELTA, def_pid_constants, M3508_MAX_OUT, M3508_MIN_OUT); + PID_Init(&chassis_init->motor[i].pid_controller, PID_DELTA, def_pid_constants, M3508_MAX_OUT, M3508_MIN_OUT); } //Init yaw and front vector @@ -118,9 +117,9 @@ static void chassis_init(Chassis_t *chassis_init){ */ 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].latest_motor_data->speed_rpm; - chassis_update->motor[i].pos_read = chassis_update->motor[i].latest_motor_data->ecd; - chassis_update->motor[i].current_read = chassis_update->motor[i].latest_motor_data->given_current; + 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; } } @@ -132,7 +131,7 @@ static void get_new_data(Chassis_t *chassis_update){ * @retval None */ -static void set_control_mode(void){ +static void set_control_mode(Chassis_t *chassis){ //Don't do anything //Implement chassis_follow_gimbal_yaw mode in the future } @@ -179,13 +178,13 @@ static void calculate_chassis_motion_setpoints(chassis_user_mode_e mode){ * @param None * @retval None */ -static void calculate_motor_setpoints(void){ +static void calculate_motor_setpoints(Chassis_t *chassis){ //Take x_speed_set etc and handle mechanum wheels //Put results into Chassis_Motor_t speed_set (and/or pos_set) - chassis.motor[FRONT_LEFT].speed_set = MULTIPLIER * (chassis.y_speed_set + chassis.z_speed_set + chassis.x_speed_set); - chassis.motor[BACK_LEFT].speed_set = MULTIPLIER * (chassis.y_speed_set + chassis.z_speed_set - chassis.x_speed_set); - chassis.motor[FRONT_RIGHT].speed_set = MULTIPLIER * (-chassis.y_speed_set + chassis.z_speed_set + chassis.x_speed_set); - chassis.motor[BACK_RIGHT].speed_set = MULTIPLIER * (-chassis.y_speed_set + chassis.z_speed_set - chassis.x_speed_set); + chassis->motor[FRONT_LEFT].speed_set = MULTIPLIER * (chassis->y_speed_set + chassis->z_speed_set + chassis->x_speed_set); + chassis->motor[BACK_LEFT].speed_set = MULTIPLIER * (chassis->y_speed_set + chassis->z_speed_set - chassis->x_speed_set); + chassis->motor[FRONT_RIGHT].speed_set = MULTIPLIER * (-chassis->y_speed_set + chassis->z_speed_set + chassis->x_speed_set); + chassis->motor[BACK_RIGHT].speed_set = MULTIPLIER * (-chassis->y_speed_set + chassis->z_speed_set - chassis->x_speed_set); } @@ -206,49 +205,49 @@ static void increment_PID(uint8_t debug){ fp32 front_left; fp32 back_left; - front_right = PID_Calc(&chassis.motor[FRONT_RIGHT].pid_control, + front_right = PID_Calc(&chassis.motor[FRONT_RIGHT].pid_controller, chassis.motor[FRONT_RIGHT].speed_read, chassis.motor[FRONT_RIGHT].speed_set); - chassis.motor[FRONT_RIGHT].speed_out += front_right; + chassis.motor[FRONT_RIGHT].current_out += front_right; - back_right = PID_Calc(&chassis.motor[BACK_RIGHT].pid_control, + back_right = PID_Calc(&chassis.motor[BACK_RIGHT].pid_controller, chassis.motor[BACK_RIGHT].speed_read, chassis.motor[BACK_RIGHT].speed_set); - chassis.motor[BACK_RIGHT].speed_out += back_right; + chassis.motor[BACK_RIGHT].current_out += back_right; - front_left = PID_Calc(&chassis.motor[FRONT_LEFT].pid_control, + front_left = PID_Calc(&chassis.motor[FRONT_LEFT].pid_controller, chassis.motor[FRONT_LEFT].speed_read, chassis.motor[FRONT_LEFT].speed_set); - chassis.motor[FRONT_LEFT].speed_out += front_left; + chassis.motor[FRONT_LEFT].current_out += front_left; - back_left = PID_Calc(&chassis.motor[BACK_LEFT].pid_control, + back_left = PID_Calc(&chassis.motor[BACK_LEFT].pid_controller, chassis.motor[BACK_LEFT].speed_read, chassis.motor[BACK_LEFT].speed_set); - chassis.motor[BACK_LEFT].speed_out += back_left; + chassis.motor[BACK_LEFT].current_out += back_left; if (debug == 1) { sprintf(pid_out, "Front Right - target: %d, sensor: %d, output: %d \n\r", chassis.motor[FRONT_RIGHT].speed_set, chassis.motor[FRONT_RIGHT].speed_read, - chassis.motor[FRONT_RIGHT].speed_out); + chassis.motor[FRONT_RIGHT].current_out); serial_send_string(pid_out); sprintf(pid_out, "Back Right - target: %d, sensor: %d, output: %d \n\r", chassis.motor[BACK_RIGHT].speed_set, chassis.motor[BACK_RIGHT].speed_read, - chassis.motor[BACK_RIGHT].speed_out); + chassis.motor[BACK_RIGHT].current_out); serial_send_string(pid_out); sprintf(pid_out, "Front Left - target: %d, sensor: %d, output: %d \n\r", chassis.motor[FRONT_LEFT].speed_set, chassis.motor[FRONT_LEFT].speed_read, - chassis.motor[FRONT_LEFT].speed_out); + chassis.motor[FRONT_LEFT].current_out); serial_send_string(pid_out); sprintf(pid_out, "Back Left - target: %d, sensor: %d, output: %d \n\r", chassis.motor[BACK_LEFT].speed_set, chassis.motor[BACK_LEFT].speed_read, - chassis.motor[BACK_LEFT].speed_out); + chassis.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 3c1e76c..fa2814e 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -39,9 +39,17 @@ #define M3508_KI 0.00 #define M3508_KD 0.1 + +typedef enum{ + CHASSIS_VECTOR_RAW, + CHASSIS_FOLLOW_GIMBAL_YAW, + CHASSIS_INDIVIDUAL_CONTROL, +} chassis_user_mode_e; + + typedef struct { - const motor_measure_t *latest_motor_data; + const motor_measure_t *motor_feedback; //Current speed read from motors int16_t pos_read; @@ -53,16 +61,17 @@ typedef struct int16_t speed_set; //Final output speed - int16_t speed_out; + int16_t current_out; //Control - PidTypeDef pid_control; + PidTypeDef pid_controller; } Chassis_Motor_t; typedef struct { Chassis_Motor_t motor[4]; + chassis_user_mode_e mode; //Raw remote control data const RC_ctrl_t *rc_update; @@ -83,11 +92,7 @@ typedef struct } Chassis_t; -typedef enum{ - CHASSIS_VECTOR_RAW, - CHASSIS_FOLLOW_GIMBAL_YAW, - CHASSIS_INDIVIDUAL_CONTROL, -} chassis_user_mode_e; + diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 49be2ad..6a98cbd 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -27,21 +27,17 @@ #include "USART_comms.h" #include -float rc_channel_1; -float rc_channel_2; -const RC_ctrl_t* rc_ptr; +// This is accessbile globally and some data is loaded from INS_task Gimbal_t gimbal; -PidTypeDef pid_pitch; -PidTypeDef pid_yaw; - //UART mailbox char str[32] = {0}; /******************** Functions ********************/ -static void initialisation(void); -static void get_new_data(void); -static void increment_PID(void); +static void initialisation(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); /** @@ -53,27 +49,21 @@ static void increment_PID(void); void gimbal_task(void* parameters){ vTaskDelay(GIMBAL_TASK_INIT_TIME); - initialisation(); + initialisation(&gimbal); while(1){ /* For now using strictly encoder feedback for position */ - get_new_data(); + get_new_data(&gimbal); - // Calculate setpoints based on RC signal. //TODO:Place in function - if(switch_is_mid(rc_ptr->rc.s[0]) || switch_is_up(rc_ptr->rc.s[0])){ - gimbal.yaw_pos_set = linear_map_int_to_int(rc_ptr->rc.ch[2], RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); - gimbal.pitch_pos_set = linear_map_int_to_int(rc_ptr->rc.ch[3], RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); - }else{ - gimbal.yaw_pos_set = linear_map_int_to_int(0, RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); - gimbal.pitch_pos_set = linear_map_int_to_int(0, RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); - } + update_setpoints(&gimbal); + - increment_PID(); + increment_PID(&gimbal); // Turn gimbal motor - CAN_CMD_GIMBAL( gimbal.yaw_speed_set, - gimbal.pitch_speed_set, + CAN_CMD_GIMBAL( gimbal.yaw_motor->current_out, + gimbal.pitch_motor->current_out, 0, 0); @@ -88,18 +78,18 @@ void gimbal_task(void* parameters){ * @param None * @retval None */ -static void initialisation(void){ - gimbal.pitch_motor->gimbal_motor_feedback = get_Pitch_Gimbal_Motor_Measure_Point(); - gimbal.yaw_motor->gimbal_motor_feedback = get_Yaw_Gimbal_Motor_Measure_Point(); - - fp32 pid_constants[3] = {pid_kp, pid_ki, pid_kd}; +static void initialisation(Gimbal_t *gimbal){ + gimbal->pitch_motor->motor_feedback = get_Pitch_Gimbal_Motor_Measure_Point(); + gimbal->yaw_motor->motor_feedback = get_Yaw_Gimbal_Motor_Measure_Point(); - PID_Init(&pid_pitch, PID_POSITION, pid_constants, max_out, max_iout); + 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}; + PID_Init(&gimbal->pitch_motor->pid_controller, PID_POSITION, pid_constants_pitch, max_out_pitch, max_i_term_out_pitch); - PID_Init(&pid_yaw, PID_POSITION, pid_constants, max_out, max_iout); + PID_Init(&gimbal->yaw_motor->pid_controller, PID_POSITION, pid_constants_yaw, max_out_yaw, max_i_term_out_yaw); - rc_ptr = get_remote_control_point(); + gimbal->rc_update = get_remote_control_point(); } /** @@ -107,18 +97,29 @@ static void initialisation(void){ * @param None * @retval None */ -static void get_new_data(void){ +static void get_new_data(Gimbal_t *gimbal){ // Get CAN received data - gimbal.pitch_pos_read = gimbal.pitch_motor->gimbal_motor_feedback->ecd; - gimbal.pitch_speed_read = gimbal.pitch_motor->gimbal_motor_feedback->speed_rpm; + gimbal->pitch_motor->pos_read = gimbal->pitch_motor->motor_feedback->ecd; + gimbal->pitch_motor->speed_read = gimbal->pitch_motor->motor_feedback->speed_rpm; - gimbal.yaw_pos_read = gimbal.yaw_motor->gimbal_motor_feedback->ecd; - gimbal.yaw_speed_read = gimbal.yaw_motor->gimbal_motor_feedback->speed_rpm; + gimbal->yaw_motor->pos_read = gimbal->yaw_motor->motor_feedback->ecd; + gimbal->yaw_motor->speed_read = gimbal->yaw_motor->motor_feedback->speed_rpm; +} + +static void update_setpoints(Gimbal_t *gimbal){ + // Calculate setpoints based on RC signal. //TODO:Place in function + if(switch_is_mid(gimbal->rc_update->rc.s[0]) || switch_is_up(gimbal->rc_update->rc.s[0])){ + gimbal->yaw_motor->pos_set = linear_map_int_to_int(gimbal->rc_update->rc.ch[2], RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); + gimbal->pitch_motor->pos_set = linear_map_int_to_int(gimbal->rc_update->rc.ch[3], RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); + }else{ + gimbal->yaw_motor->pos_set = linear_map_int_to_int(average(YAW_MIN, YAW_MAX), RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); + gimbal->pitch_motor->pos_set = linear_map_int_to_int(average(PITCH_MIN, PITCH_MAX), RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); + } } -static void increment_PID(void){ - gimbal.pitch_speed_set = PID_Calc(&pid_pitch, gimbal.pitch_motor->pos_read, gimbal.pitch_pos_set); - gimbal.yaw_speed_set = PID_Calc(&pid_yaw, gimbal.yaw_motor->pos_read, gimbal.yaw_pos_set); +static void increment_PID(Gimbal_t *gimbal){ + gimbal->pitch_motor->current_out = PID_Calc(&gimbal->pitch_motor->pid_controller, gimbal->pitch_motor->pos_read, gimbal->pitch_motor->pos_set); + gimbal->yaw_motor->current_out = PID_Calc(&gimbal->yaw_motor->pid_controller, gimbal->yaw_motor->pos_read, gimbal->yaw_motor->pos_set); } diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index b55d843..6aba6c2 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -8,6 +8,7 @@ #include "main.h" #include "CAN_receive.h" #include "pid.h" +#include "remote_control.h" /*************** Converts between motor position and degrees *****************/ @@ -17,11 +18,17 @@ /****************************** PID Constants ********************************/ -#define pid_kp 40.0f -#define pid_ki 0.0f -#define pid_kd 0.0f -#define max_out 5000.0f -#define max_iout 0 +#define pid_kp_yaw 40.0f +#define pid_ki_yaw 0.0f +#define pid_kd_yaw 0.0f +#define max_out_yaw 5000.0f +#define max_i_term_out_yaw 1000.0f + +#define pid_kp_pitch 40.0f +#define pid_ki_pitch 0.0f +#define pid_kd_pitch 0.0f +#define max_out_pitch 5000.0f +#define max_i_term_out_pitch 1000.0f /***************************** Gimbal Constants *****************************/ #define GIMBAL_TASK_INIT_TIME 201 @@ -39,29 +46,26 @@ /************************** Gimbal Data Structures ***************************/ typedef struct { - const motor_measure_t *gimbal_motor_feedback; + const motor_measure_t *motor_feedback; uint16_t pos_read; uint16_t speed_read; uint16_t current_read; + + uint16_t pos_set; + int16_t current_out; + + PidTypeDef pid_controller; } Gimbal_Motor_t; typedef struct { + const RC_ctrl_t *rc_update; Gimbal_Motor_t *yaw_motor; Gimbal_Motor_t *pitch_motor; const fp32 *angle_update; const fp32 *gyro_update; const fp32 *accel_update; - - uint16_t pitch_pos_read; - uint16_t yaw_pos_read; - uint16_t pitch_pos_set; - uint16_t yaw_pos_set; - - int16_t pitch_speed_read; - int16_t yaw_speed_read; - int16_t pitch_speed_set; - int16_t yaw_speed_set; + // TODO: Add gimbal angles when we care about orientation of robot in 3-d space } Gimbal_t; From 1801b6f660f5d8f36499f527dbc3f4735fd7aa8d Mon Sep 17 00:00:00 2001 From: Benji Date: Sat, 22 Feb 2020 11:43:38 -0800 Subject: [PATCH 3/7] refactored to improve general readability, cleaned up gimbal function --- user/TASK/gimbal_task/gimbal_task.c | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 6a98cbd..7b86ffd 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -53,12 +53,8 @@ void gimbal_task(void* parameters){ while(1){ /* For now using strictly encoder feedback for position */ - get_new_data(&gimbal); - update_setpoints(&gimbal); - - increment_PID(&gimbal); // Turn gimbal motor @@ -106,6 +102,11 @@ static void get_new_data(Gimbal_t *gimbal){ gimbal->yaw_motor->speed_read = gimbal->yaw_motor->motor_feedback->speed_rpm; } +/** + * @brief Updates desired setpoints from RC signal + * @param Gimbal struct containing info on Gimbal + * @retval None + */ static void update_setpoints(Gimbal_t *gimbal){ // Calculate setpoints based on RC signal. //TODO:Place in function if(switch_is_mid(gimbal->rc_update->rc.s[0]) || switch_is_up(gimbal->rc_update->rc.s[0])){ @@ -117,14 +118,16 @@ static void update_setpoints(Gimbal_t *gimbal){ } } +/** + * @brief Increments PID loop based on latest setpoints and latest positions + * @param None + * @retval None + */ static void increment_PID(Gimbal_t *gimbal){ gimbal->pitch_motor->current_out = PID_Calc(&gimbal->pitch_motor->pid_controller, gimbal->pitch_motor->pos_read, gimbal->pitch_motor->pos_set); gimbal->yaw_motor->current_out = PID_Calc(&gimbal->yaw_motor->pid_controller, gimbal->yaw_motor->pos_read, gimbal->yaw_motor->pos_set); } - - - /** * @brief Reads vision instruction from UART and cap to certin values * @param None @@ -142,7 +145,6 @@ 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 From 7087f85d1fad046cfca883bbe03aaf6c28e68e11 Mon Sep 17 00:00:00 2001 From: Benji Date: Sat, 22 Feb 2020 18:52:56 -0800 Subject: [PATCH 4/7] fixed bugs on master lolololol --- .vscode/settings.json | 3 +- user/TASK/chassis_task/chassis_task.c | 3 + user/TASK/gimbal_task/gimbal_task.c | 110 +++++++++++++++----------- user/TASK/gimbal_task/gimbal_task.h | 9 ++- user/TASK/start_task/start_task.c | 8 +- 5 files changed, 77 insertions(+), 56 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 81ba1c4..953a203 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,7 +3,8 @@ "files.associations": { "can.h": "c", "pid.h": "c", - "ins_task.h": "c" + "ins_task.h": "c", + "gimbal_task.h": "c" } } \ No newline at end of file diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 1cdc25e..ac46491 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -49,9 +49,11 @@ void chassis_task(void *pvParameters){ // Delay to make sure critical communications/timers have been initialised vTaskDelay(20); //Initializes chassis with pointers to RC commands and CAN feedback messages + chassis_init(&chassis); while(1) { + get_new_data(&chassis); //updates RC commands and CAN motor feedback set_control_mode(&chassis); //Note: currently not implemented calculate_chassis_motion_setpoints(CHASSIS_VECTOR_RAW); @@ -64,6 +66,7 @@ void chassis_task(void *pvParameters){ chassis.motor[BACK_RIGHT].current_out); vTaskDelay(1); + } } diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 7b86ffd..490f58b 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -26,6 +26,7 @@ #include "remote_control.h" #include "USART_comms.h" #include +#include "pid.h" // This is accessbile globally and some data is loaded from INS_task Gimbal_t gimbal; @@ -34,58 +35,66 @@ Gimbal_t gimbal; char str[32] = {0}; /******************** Functions ********************/ -static void initialisation(Gimbal_t *gimbal); +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); /** - * @brief Initialises PID and fetches Gimbal motor data to ensure + * @brief Initializes PID and fetches Gimbal motor data to ensure * gimbal points in direction specified. * @param FreeRTOS parameters * @retval None */ void gimbal_task(void* parameters){ - vTaskDelay(GIMBAL_TASK_INIT_TIME); - initialisation(&gimbal); + vTaskDelay(200); + initialization(&gimbal); while(1){ + //send_to_uart(&gimbal); + /* For now using strictly encoder feedback for position */ + get_new_data(&gimbal); + //send_to_uart(&gimbal); update_setpoints(&gimbal); + //send_to_uart(&gimbal); increment_PID(&gimbal); - + //send_to_uart(&gimbal); // Turn gimbal motor - CAN_CMD_GIMBAL( gimbal.yaw_motor->current_out, - gimbal.pitch_motor->current_out, + + + CAN_CMD_GIMBAL( (int16_t) gimbal.yaw_motor.current_out, + (int16_t) gimbal.pitch_motor.current_out, 0, 0); - vTaskDelay(1); - - //send_to_uart(gimbal_pitch_motor, pid_pitch, pitch_signal); //Sending data via UART + //Sending data via UART + vTaskDelay(5); } } /** - * @brief Initialises gimbal struct and loads pointers for RC and motor feedback + * @brief Initializes gimbal struct and loads pointers for RC and motor feedback * @param None * @retval None */ -static void initialisation(Gimbal_t *gimbal){ - gimbal->pitch_motor->motor_feedback = get_Pitch_Gimbal_Motor_Measure_Point(); - gimbal->yaw_motor->motor_feedback = get_Yaw_Gimbal_Motor_Measure_Point(); +static void initialization(Gimbal_t *gimbal_ptr){ + gimbal_ptr->pitch_motor.motor_feedback = get_Pitch_Gimbal_Motor_Measure_Point(); + gimbal_ptr->yaw_motor.motor_feedback = get_Yaw_Gimbal_Motor_Measure_Point(); 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}; - PID_Init(&gimbal->pitch_motor->pid_controller, PID_POSITION, pid_constants_pitch, max_out_pitch, max_i_term_out_pitch); - - PID_Init(&gimbal->yaw_motor->pid_controller, PID_POSITION, pid_constants_yaw, max_out_yaw, max_i_term_out_yaw); + 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->rc_update = get_remote_control_point(); + gimbal_ptr->pitch_motor.pos_set = 6000; + gimbal_ptr->yaw_motor.pos_set = 6000; } /** @@ -94,12 +103,12 @@ static void initialisation(Gimbal_t *gimbal){ * @retval None */ static void get_new_data(Gimbal_t *gimbal){ - // Get CAN received data - gimbal->pitch_motor->pos_read = gimbal->pitch_motor->motor_feedback->ecd; - gimbal->pitch_motor->speed_read = gimbal->pitch_motor->motor_feedback->speed_rpm; + // Get CAN received data + gimbal->pitch_motor.pos_read = gimbal->pitch_motor.motor_feedback->ecd; + gimbal->pitch_motor.speed_read = gimbal->pitch_motor.motor_feedback->speed_rpm; - gimbal->yaw_motor->pos_read = gimbal->yaw_motor->motor_feedback->ecd; - gimbal->yaw_motor->speed_read = gimbal->yaw_motor->motor_feedback->speed_rpm; + gimbal->yaw_motor.pos_read = gimbal->yaw_motor.motor_feedback->ecd; + gimbal->yaw_motor.speed_read = gimbal->yaw_motor.motor_feedback->speed_rpm; } /** @@ -108,14 +117,20 @@ static void get_new_data(Gimbal_t *gimbal){ * @retval None */ static void update_setpoints(Gimbal_t *gimbal){ + + gimbal->yaw_motor.pos_set += gimbal->rc_update->rc.ch[2] / 10; + gimbal->pitch_motor.pos_set += gimbal->rc_update->rc.ch[3] / 10; + // Calculate setpoints based on RC signal. //TODO:Place in function - if(switch_is_mid(gimbal->rc_update->rc.s[0]) || switch_is_up(gimbal->rc_update->rc.s[0])){ - gimbal->yaw_motor->pos_set = linear_map_int_to_int(gimbal->rc_update->rc.ch[2], RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); - gimbal->pitch_motor->pos_set = linear_map_int_to_int(gimbal->rc_update->rc.ch[3], RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); - }else{ - gimbal->yaw_motor->pos_set = linear_map_int_to_int(average(YAW_MIN, YAW_MAX), RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); - gimbal->pitch_motor->pos_set = linear_map_int_to_int(average(PITCH_MIN, PITCH_MAX), RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); - } +// if(switch_is_mid(gimbal->rc_update->rc.s[0]) || switch_is_up(gimbal->rc_update->rc.s[0])){ + + //linear_map_int_to_int(gimbal->rc_update->rc.ch[2], RC_MIN, RC_MAX, YAW_MIN, YAW_MAX); + //linear_map_int_to_int(gimbal->rc_update->rc.ch[3], RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); + + /*}else{ + gimbal->yaw_motor.pos_set = linear_map_int_to_int(average(YAW_MIN, YAW_MAX), RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); + gimbal->pitch_motor.pos_set = linear_map_int_to_int(average(PITCH_MIN, PITCH_MAX), RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); + }*/ } /** @@ -124,8 +139,8 @@ static void update_setpoints(Gimbal_t *gimbal){ * @retval None */ static void increment_PID(Gimbal_t *gimbal){ - gimbal->pitch_motor->current_out = PID_Calc(&gimbal->pitch_motor->pid_controller, gimbal->pitch_motor->pos_read, gimbal->pitch_motor->pos_set); - gimbal->yaw_motor->current_out = PID_Calc(&gimbal->yaw_motor->pid_controller, gimbal->yaw_motor->pos_read, gimbal->yaw_motor->pos_set); + gimbal->pitch_motor.current_out = PID_Calc(&gimbal->pitch_motor.pid_controller, gimbal->pitch_motor.pos_read, gimbal->pitch_motor.pos_set); + gimbal->yaw_motor.current_out = PID_Calc(&gimbal->yaw_motor.pid_controller, gimbal->yaw_motor.pos_read, gimbal->yaw_motor.pos_set); } /** @@ -153,28 +168,29 @@ int get_vision_signal(void) { * @param pitch_signal signal to pitch motor * @retval None */ -void send_to_uart(Gimbal_Motor_t gimbal_yaw_motor, PidTypeDef pid_pitch, fp32 pitch_signal) +void send_to_uart(Gimbal_t *gimbal) { char str[20]; //uart data buffer - sprintf(str, "position: %d\n\r", gimbal.yaw_motor->pos_read); + if(gimbal->yaw_motor.pos_read == NULL){ + sprintf(str, "null ... :( %d\n\r", 123); serial_send_string(str); - - sprintf(str, "speed: %d\n\r", gimbal.yaw_motor->speed_read); - serial_send_string(str); - - sprintf(str, "current: %d\n\r", gimbal.yaw_motor->current_read); - serial_send_string(str); - - sprintf(str, "motor kp: %f\n\r", pid_pitch.Kp); + } else{ + sprintf(str, "yaw position read: %d\n\r", 321); serial_send_string(str); + } + - sprintf(str, "motor kd: %f\n\r", pid_pitch.Kd); - serial_send_string(str); +/* + sprintf(str, "yaw speed read: %d\n\r", gimbal->yaw_motor->speed_read); + serial_send_string(str); - sprintf(str, "motor ki: %f\n\r", pid_pitch.Ki); + 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); - sprintf(str, "pitch signal: %f\n\r", pitch_signal); - serial_send_string(str); + //sprintf(str, "yaw current out: %d\n\r", gimbal->yaw_motor->current_out); + //serial_send_string(str); } diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index 6aba6c2..71c0030 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -31,7 +31,7 @@ #define max_i_term_out_pitch 1000.0f /***************************** Gimbal Constants *****************************/ -#define GIMBAL_TASK_INIT_TIME 201 +#define GIMBAL_TASK_INIT_TIME 300 #define CONTROL_TIME 1 #define RC_MIN -660 #define RC_MAX 660 @@ -60,19 +60,20 @@ typedef struct typedef struct { const RC_ctrl_t *rc_update; - Gimbal_Motor_t *yaw_motor; - Gimbal_Motor_t *pitch_motor; + Gimbal_Motor_t yaw_motor; + Gimbal_Motor_t pitch_motor; const fp32 *angle_update; const fp32 *gyro_update; const fp32 *accel_update; // TODO: Add gimbal angles when we care about orientation of robot in 3-d space + } Gimbal_t; /******************************* Function Declarations ***********************/ int get_vision_signal(void); extern void gimbal_task(void *pvParameters); -extern void send_to_uart(Gimbal_Motor_t gimbal_yaw_motor, PidTypeDef pid, fp32 pitch_signal); +extern void send_to_uart(Gimbal_t *gimbal); diff --git a/user/TASK/start_task/start_task.c b/user/TASK/start_task/start_task.c index c6123de..54b75a8 100644 --- a/user/TASK/start_task/start_task.c +++ b/user/TASK/start_task/start_task.c @@ -16,13 +16,13 @@ #define START_STK_SIZE 512 static TaskHandle_t start_task_handler; -#define INS_TASK_PRIO 2 +#define INS_TASK_PRIO 20 #define INS_STK_SIZE 512 -static TaskHandle_t chassis_task_handler; +static TaskHandle_t INS_task_handler; -#define CHASSIS_TASK_PRIO 3 +#define CHASSIS_TASK_PRIO 5 #define CHASSIS_STK_SIZE 512 -static TaskHandle_t INS_task_handler; +static TaskHandle_t chassis_task_handler; #define GIMBAL_TASK_PRIO 4 #define GIMBAL_STK_SIZE 512 From 5d3be5ce99e1ff74a2989d69094b0bd30371f08d Mon Sep 17 00:00:00 2001 From: Benji Date: Sun, 23 Feb 2020 11:04:35 -0800 Subject: [PATCH 5/7] fixed chassis task pointers for consistency, added soft limits on gimbal motion --- Project/embed-infantry.uvprojx | 10 +-- user/TASK/chassis_task/chassis_task.c | 114 +++++++++++++------------- user/TASK/gimbal_task/gimbal_task.c | 43 +++++----- user/user_lib/user_lib.c | 4 +- user/user_lib/user_lib.h | 4 +- 5 files changed, 86 insertions(+), 89 deletions(-) diff --git a/Project/embed-infantry.uvprojx b/Project/embed-infantry.uvprojx index d7f8fa1..d5ac67a 100644 --- a/Project/embed-infantry.uvprojx +++ b/Project/embed-infantry.uvprojx @@ -892,16 +892,16 @@ 5 ..\user\TASK\start_task\start_task.h - - INS_task.c - 1 - ..\user\TASK\INS_task\INS_task.c - INS_task.h 5 ..\user\TASK\INS_task\INS_task.h + + INS_task.c + 1 + ..\user\TASK\INS_task\INS_task.c + chassis_task.c 1 diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index ac46491..37bee70 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -29,13 +29,13 @@ static void chassis_init(Chassis_t *chassis_init); static void get_new_data(Chassis_t *chassis_update); static void set_control_mode(Chassis_t *chassis); -static void calculate_chassis_motion_setpoints(chassis_user_mode_e mode); -static void calculate_motor_setpoints(Chassis_t *chassis); -static void increment_PID(uint8_t debug); +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 Chassis_t chassis; - +#define DEBUG 0 /******************** Main Task/Functions Called from Outside ********************/ @@ -56,9 +56,9 @@ void chassis_task(void *pvParameters){ get_new_data(&chassis); //updates RC commands and CAN motor feedback set_control_mode(&chassis); //Note: currently not implemented - calculate_chassis_motion_setpoints(CHASSIS_VECTOR_RAW); + calculate_chassis_motion_setpoints(&chassis); calculate_motor_setpoints(&chassis); - increment_PID(FALSE); + increment_PID(&chassis); //output CAN_CMD_CHASSIS(chassis.motor[FRONT_RIGHT].current_out, chassis.motor[FRONT_LEFT].current_out, @@ -147,7 +147,7 @@ static void set_control_mode(Chassis_t *chassis){ * @retval None */ -static void calculate_chassis_motion_setpoints(chassis_user_mode_e mode){ +static void calculate_chassis_motion_setpoints(Chassis_t *chassis_set){ //Get remote control data and put into x_speed_read etc //process based on mode (which is currently none) and put into x_speed_set //Debug print out current @@ -155,24 +155,24 @@ static void calculate_chassis_motion_setpoints(chassis_user_mode_e mode){ // get rc data and put into chassis struct //Switch Chassis Only - if(switch_is_down(chassis.rc_update->rc.s[0])){ - chassis.x_speed_read = chassis.rc_update->rc.ch[RC_X]; - chassis.y_speed_read = chassis.rc_update->rc.ch[RC_Y]; - chassis.z_speed_read = chassis.rc_update->rc.ch[RC_Z]; - }else if(switch_is_mid(chassis.rc_update->rc.s[0])){ - chassis.x_speed_read = 0; - chassis.y_speed_read = chassis.rc_update->rc.ch[RC_Y]; - chassis.z_speed_read = chassis.rc_update->rc.ch[RC_Z]; - }else if(switch_is_up(chassis.rc_update->rc.s[0])){ - chassis.x_speed_read = 0; - chassis.y_speed_read = 0; - chassis.z_speed_read = 0; + if(switch_is_down(chassis_set->rc_update->rc.s[0])){ + chassis_set->x_speed_read = chassis_set->rc_update->rc.ch[RC_X]; + chassis_set->y_speed_read = chassis_set->rc_update->rc.ch[RC_Y]; + chassis_set->z_speed_read = chassis_set->rc_update->rc.ch[RC_Z]; + }else if(switch_is_mid(chassis_set->rc_update->rc.s[0])){ + chassis_set->x_speed_read = 0; + chassis_set->y_speed_read = chassis_set->rc_update->rc.ch[RC_Y]; + chassis_set->z_speed_read = chassis_set->rc_update->rc.ch[RC_Z]; + }else if(switch_is_up(chassis_set->rc_update->rc.s[0])){ + chassis_set->x_speed_read = 0; + chassis_set->y_speed_read = 0; + chassis_set->z_speed_read = 0; } // process raw sppeds based on modes (for now they are just the same) - chassis.x_speed_set = chassis.x_speed_read; - chassis.y_speed_set = chassis.y_speed_read; - chassis.z_speed_set = chassis.z_speed_read; + chassis_set->x_speed_set = chassis_set->x_speed_read; + chassis_set->y_speed_set = chassis_set->y_speed_read; + chassis_set->z_speed_set = chassis_set->z_speed_read; } @@ -181,13 +181,13 @@ static void calculate_chassis_motion_setpoints(chassis_user_mode_e mode){ * @param None * @retval None */ -static void calculate_motor_setpoints(Chassis_t *chassis){ +static void calculate_motor_setpoints(Chassis_t *chassis_motors){ //Take x_speed_set etc and handle mechanum wheels //Put results into Chassis_Motor_t speed_set (and/or pos_set) - chassis->motor[FRONT_LEFT].speed_set = MULTIPLIER * (chassis->y_speed_set + chassis->z_speed_set + chassis->x_speed_set); - chassis->motor[BACK_LEFT].speed_set = MULTIPLIER * (chassis->y_speed_set + chassis->z_speed_set - chassis->x_speed_set); - chassis->motor[FRONT_RIGHT].speed_set = MULTIPLIER * (-chassis->y_speed_set + chassis->z_speed_set + chassis->x_speed_set); - chassis->motor[BACK_RIGHT].speed_set = MULTIPLIER * (-chassis->y_speed_set + chassis->z_speed_set - chassis->x_speed_set); + chassis_motors->motor[FRONT_LEFT].speed_set = MULTIPLIER * (chassis_motors->y_speed_set + chassis_motors->z_speed_set + chassis_motors->x_speed_set); + chassis_motors->motor[BACK_LEFT].speed_set = MULTIPLIER * (chassis_motors->y_speed_set + chassis_motors->z_speed_set - chassis_motors->x_speed_set); + chassis_motors->motor[FRONT_RIGHT].speed_set = MULTIPLIER * (-chassis_motors->y_speed_set + chassis_motors->z_speed_set + chassis_motors->x_speed_set); + chassis_motors->motor[BACK_RIGHT].speed_set = MULTIPLIER * (-chassis_motors->y_speed_set + chassis_motors->z_speed_set - chassis_motors->x_speed_set); } @@ -200,7 +200,7 @@ char pid_out[64]; * @param None * @retval None */ -static void increment_PID(uint8_t debug){ +static void increment_PID(Chassis_t *chassis_pid){ //translation //rotation fp32 front_right; @@ -208,49 +208,49 @@ static void increment_PID(uint8_t debug){ fp32 front_left; fp32 back_left; - front_right = PID_Calc(&chassis.motor[FRONT_RIGHT].pid_controller, - chassis.motor[FRONT_RIGHT].speed_read, - chassis.motor[FRONT_RIGHT].speed_set); - chassis.motor[FRONT_RIGHT].current_out += front_right; + front_right = PID_Calc(&chassis_pid->motor[FRONT_RIGHT].pid_controller, + chassis_pid->motor[FRONT_RIGHT].speed_read, + chassis_pid->motor[FRONT_RIGHT].speed_set); + chassis_pid->motor[FRONT_RIGHT].current_out += front_right; - back_right = PID_Calc(&chassis.motor[BACK_RIGHT].pid_controller, - chassis.motor[BACK_RIGHT].speed_read, - chassis.motor[BACK_RIGHT].speed_set); - chassis.motor[BACK_RIGHT].current_out += back_right; + back_right = PID_Calc(&chassis_pid->motor[BACK_RIGHT].pid_controller, + chassis_pid->motor[BACK_RIGHT].speed_read, + chassis_pid->motor[BACK_RIGHT].speed_set); + chassis_pid->motor[BACK_RIGHT].current_out += back_right; - front_left = PID_Calc(&chassis.motor[FRONT_LEFT].pid_controller, - chassis.motor[FRONT_LEFT].speed_read, - chassis.motor[FRONT_LEFT].speed_set); - chassis.motor[FRONT_LEFT].current_out += front_left; + front_left = PID_Calc(&chassis_pid->motor[FRONT_LEFT].pid_controller, + chassis_pid->motor[FRONT_LEFT].speed_read, + chassis_pid->motor[FRONT_LEFT].speed_set); + chassis_pid->motor[FRONT_LEFT].current_out += front_left; - back_left = PID_Calc(&chassis.motor[BACK_LEFT].pid_controller, - chassis.motor[BACK_LEFT].speed_read, - chassis.motor[BACK_LEFT].speed_set); - chassis.motor[BACK_LEFT].current_out += back_left; + back_left = PID_Calc(&chassis_pid->motor[BACK_LEFT].pid_controller, + chassis_pid->motor[BACK_LEFT].speed_read, + chassis_pid->motor[BACK_LEFT].speed_set); + chassis_pid->motor[BACK_LEFT].current_out += back_left; - if (debug == 1) { + if (DEBUG == 1) { sprintf(pid_out, "Front Right - target: %d, sensor: %d, output: %d \n\r", - chassis.motor[FRONT_RIGHT].speed_set, - chassis.motor[FRONT_RIGHT].speed_read, - chassis.motor[FRONT_RIGHT].current_out); + chassis_pid->motor[FRONT_RIGHT].speed_set, + chassis_pid->motor[FRONT_RIGHT].speed_read, + chassis_pid->motor[FRONT_RIGHT].current_out); serial_send_string(pid_out); sprintf(pid_out, "Back Right - target: %d, sensor: %d, output: %d \n\r", - chassis.motor[BACK_RIGHT].speed_set, - chassis.motor[BACK_RIGHT].speed_read, - chassis.motor[BACK_RIGHT].current_out); + chassis_pid->motor[BACK_RIGHT].speed_set, + chassis_pid->motor[BACK_RIGHT].speed_read, + chassis_pid->motor[BACK_RIGHT].current_out); serial_send_string(pid_out); sprintf(pid_out, "Front Left - target: %d, sensor: %d, output: %d \n\r", - chassis.motor[FRONT_LEFT].speed_set, - chassis.motor[FRONT_LEFT].speed_read, - chassis.motor[FRONT_LEFT].current_out); + chassis_pid->motor[FRONT_LEFT].speed_set, + chassis_pid->motor[FRONT_LEFT].speed_read, + chassis_pid->motor[FRONT_LEFT].current_out); serial_send_string(pid_out); sprintf(pid_out, "Back Left - target: %d, sensor: %d, output: %d \n\r", - chassis.motor[BACK_LEFT].speed_set, - chassis.motor[BACK_LEFT].speed_read, - chassis.motor[BACK_LEFT].current_out); + chassis_pid->motor[BACK_LEFT].speed_set, + chassis_pid->motor[BACK_LEFT].speed_read, + chassis_pid->motor[BACK_LEFT].current_out); serial_send_string(pid_out); } diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 490f58b..4ec61ac 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -28,6 +28,8 @@ #include #include "pid.h" +#define DEADBAND 10 + // This is accessbile globally and some data is loaded from INS_task Gimbal_t gimbal; @@ -102,13 +104,13 @@ static void initialization(Gimbal_t *gimbal_ptr){ * @param None * @retval None */ -static void get_new_data(Gimbal_t *gimbal){ +static void get_new_data(Gimbal_t *gimbal_data){ // Get CAN received data - gimbal->pitch_motor.pos_read = gimbal->pitch_motor.motor_feedback->ecd; - gimbal->pitch_motor.speed_read = gimbal->pitch_motor.motor_feedback->speed_rpm; + 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->yaw_motor.pos_read = gimbal->yaw_motor.motor_feedback->ecd; - gimbal->yaw_motor.speed_read = gimbal->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; } /** @@ -116,21 +118,16 @@ static void get_new_data(Gimbal_t *gimbal){ * @param Gimbal struct containing info on Gimbal * @retval None */ -static void update_setpoints(Gimbal_t *gimbal){ - - gimbal->yaw_motor.pos_set += gimbal->rc_update->rc.ch[2] / 10; - gimbal->pitch_motor.pos_set += gimbal->rc_update->rc.ch[3] / 10; +static void update_setpoints(Gimbal_t *gimbal_set){ - // Calculate setpoints based on RC signal. //TODO:Place in function -// if(switch_is_mid(gimbal->rc_update->rc.s[0]) || switch_is_up(gimbal->rc_update->rc.s[0])){ - - //linear_map_int_to_int(gimbal->rc_update->rc.ch[2], RC_MIN, RC_MAX, YAW_MIN, YAW_MAX); - //linear_map_int_to_int(gimbal->rc_update->rc.ch[3], RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); + 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; - /*}else{ - gimbal->yaw_motor.pos_set = linear_map_int_to_int(average(YAW_MIN, YAW_MAX), RC_MIN, RC_MAX, YAW_MAX, YAW_MIN); - gimbal->pitch_motor.pos_set = linear_map_int_to_int(average(PITCH_MIN, PITCH_MAX), RC_MIN, RC_MAX, PITCH_MAX, PITCH_MIN); - }*/ + 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. } /** @@ -138,9 +135,9 @@ static void update_setpoints(Gimbal_t *gimbal){ * @param None * @retval None */ -static void increment_PID(Gimbal_t *gimbal){ - gimbal->pitch_motor.current_out = PID_Calc(&gimbal->pitch_motor.pid_controller, gimbal->pitch_motor.pos_read, gimbal->pitch_motor.pos_set); - gimbal->yaw_motor.current_out = PID_Calc(&gimbal->yaw_motor.pid_controller, gimbal->yaw_motor.pos_read, gimbal->yaw_motor.pos_set); +static void increment_PID(Gimbal_t *gimbal_pid){ + 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); } /** @@ -168,11 +165,11 @@ int get_vision_signal(void) { * @param pitch_signal signal to pitch motor * @retval None */ -void send_to_uart(Gimbal_t *gimbal) +void send_to_uart(Gimbal_t *gimbal_msg) { char str[20]; //uart data buffer - if(gimbal->yaw_motor.pos_read == NULL){ + if(gimbal_msg->yaw_motor.pos_read == NULL){ sprintf(str, "null ... :( %d\n\r", 123); serial_send_string(str); } else{ diff --git a/user/user_lib/user_lib.c b/user/user_lib/user_lib.c index 695bb92..59fcb76 100644 --- a/user/user_lib/user_lib.c +++ b/user/user_lib/user_lib.c @@ -129,7 +129,7 @@ fp32 sign(fp32 value) } //浮点死区 -fp32 fp32_deadline(fp32 Value, fp32 minValue, fp32 maxValue) +fp32 fp32_deadzone(fp32 Value, fp32 minValue, fp32 maxValue) { if (Value < maxValue && Value > minValue) { @@ -139,7 +139,7 @@ fp32 fp32_deadline(fp32 Value, fp32 minValue, fp32 maxValue) } //int26死区 -int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue) +int16_t int16_deadzone(int16_t Value, int16_t minValue, int16_t maxValue) { if (Value < maxValue && Value > minValue) { diff --git a/user/user_lib/user_lib.h b/user/user_lib/user_lib.h index c3195f0..5eaa476 100644 --- a/user/user_lib/user_lib.h +++ b/user/user_lib/user_lib.h @@ -31,9 +31,9 @@ extern void abs_limit(fp32 *num, fp32 Limit); //判断符号位 extern fp32 sign(fp32 value); //浮点死区 -extern fp32 fp32_deadline(fp32 Value, fp32 minValue, fp32 maxValue); +extern fp32 fp32_deadzone(fp32 Value, fp32 minValue, fp32 maxValue); //int26死区 -extern int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue); +extern int16_t int16_deadzone(int16_t Value, int16_t minValue, int16_t maxValue); //限幅函数 extern fp32 fp32_constrain(fp32 Value, fp32 minValue, fp32 maxValue); //限幅函数 From 7b8b6434a4633714464178ca1596773c35c1079a Mon Sep 17 00:00:00 2001 From: jirm2019 Date: Sun, 23 Feb 2020 11:46:21 -0800 Subject: [PATCH 6/7] get vectored --- user/TASK/chassis_task/chassis_task.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/user/TASK/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index fa2814e..db3fd4b 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -76,7 +76,7 @@ typedef struct //Raw remote control data const RC_ctrl_t *rc_update; - //Current front vectord + //Current front vector const fp32 *vec_raw; const fp32 *yaw_pos_raw; From 3d413f5e27a0fe7b9f58e3fa80d30630d7641eeb Mon Sep 17 00:00:00 2001 From: jirm2019 Date: Sun, 23 Feb 2020 12:22:29 -0800 Subject: [PATCH 7/7] resolved changes --- user/TASK/chassis_task/chassis_task.c | 4 ++-- user/TASK/chassis_task/chassis_task.h | 4 ++++ user/TASK/gimbal_task/gimbal_task.c | 13 +++---------- user/TASK/gimbal_task/gimbal_task.h | 4 ++++ 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/user/TASK/chassis_task/chassis_task.c b/user/TASK/chassis_task/chassis_task.c index 37bee70..c0dc5dd 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -47,7 +47,7 @@ static Chassis_t chassis; void chassis_task(void *pvParameters){ // Delay to make sure critical communications/timers have been initialised - vTaskDelay(20); + vTaskDelay(CHASSIS_INIT_DELAY); //Initializes chassis with pointers to RC commands and CAN feedback messages chassis_init(&chassis); @@ -65,7 +65,7 @@ void chassis_task(void *pvParameters){ chassis.motor[BACK_LEFT].current_out, chassis.motor[BACK_RIGHT].current_out); - vTaskDelay(1); + vTaskDelay(CHASSIS_TASK_DELAY); } } diff --git a/user/TASK/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index db3fd4b..ef01d6d 100644 --- a/user/TASK/chassis_task/chassis_task.h +++ b/user/TASK/chassis_task/chassis_task.h @@ -16,6 +16,10 @@ #include "remote_control.h" #include "pid.h" +/******************************* Task Delays *********************************/ +#define CHASSIS_TASK_DELAY 1 +#define CHASSIS_INIT_DELAY 20 + /******************** User Definitions ********************/ //Chassis motor CAN ID offset diff --git a/user/TASK/gimbal_task/gimbal_task.c b/user/TASK/gimbal_task/gimbal_task.c index 4ec61ac..3286cb6 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -51,7 +51,7 @@ static void increment_PID(Gimbal_t *gimbal); */ void gimbal_task(void* parameters){ - vTaskDelay(200); + vTaskDelay(GIMBAL_INIT_DELAY); initialization(&gimbal); while(1){ @@ -74,7 +74,7 @@ void gimbal_task(void* parameters){ 0); //Sending data via UART - vTaskDelay(5); + vTaskDelay(GIMBAL_TASK_DELAY); } } @@ -169,14 +169,7 @@ void send_to_uart(Gimbal_t *gimbal_msg) { char str[20]; //uart data buffer - if(gimbal_msg->yaw_motor.pos_read == NULL){ - sprintf(str, "null ... :( %d\n\r", 123); - serial_send_string(str); - } else{ - sprintf(str, "yaw position read: %d\n\r", 321); - serial_send_string(str); - } - + //TODO - fix below / fill as needed /* sprintf(str, "yaw speed read: %d\n\r", gimbal->yaw_motor->speed_read); diff --git a/user/TASK/gimbal_task/gimbal_task.h b/user/TASK/gimbal_task/gimbal_task.h index 71c0030..3a4fde4 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -10,6 +10,10 @@ #include "pid.h" #include "remote_control.h" +/******************************* Task Delays *********************************/ +#define GIMBAL_TASK_DELAY 1 +#define GIMBAL_INIT_DELAY 300 + /*************** Converts between motor position and degrees *****************/ #define Motor_Ecd_to_Rad 0.000766990394f