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/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/INS_task/INS_task.c b/user/TASK/INS_task/INS_task.c index bd2bd4a..8211962 100644 --- a/user/TASK/INS_task/INS_task.c +++ b/user/TASK/INS_task/INS_task.c @@ -381,41 +381,41 @@ 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_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) { + if (accel == 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..c0dc5dd 100644 --- a/user/TASK/chassis_task/chassis_task.c +++ b/user/TASK/chassis_task/chassis_task.c @@ -28,15 +28,14 @@ /******************** 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 calculate_chassis_motion_setpoints(chassis_user_mode_e mode); -static void set_control_mode(void); -static void calculate_motor_setpoints(void); -static void increment_PID(uint8_t debug); +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 Chassis_t chassis; - +#define DEBUG 0 /******************** Main Task/Functions Called from Outside ********************/ @@ -48,23 +47,26 @@ 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); while(1) { + get_new_data(&chassis); //updates RC commands and CAN motor feedback - set_control_mode(); //Note: currently not implemented - calculate_chassis_motion_setpoints(CHASSIS_VECTOR_RAW); - calculate_motor_setpoints(); - increment_PID(FALSE); + set_control_mode(&chassis); //Note: currently not implemented + calculate_chassis_motion_setpoints(&chassis); + calculate_motor_setpoints(&chassis); + increment_PID(&chassis); //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); + vTaskDelay(CHASSIS_TASK_DELAY); + } } @@ -94,12 +96,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].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].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 @@ -107,7 +109,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 +120,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].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 +134,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 } @@ -145,32 +147,32 @@ static void set_control_mode(void){ * @retval None */ -static void calculate_chassis_motion_setpoints(chassis_user_mode_e mode){ - //Get remote control data and put into x_speed_raw etc +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 // 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_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_raw; - chassis.y_speed_set = chassis.y_speed_raw; - chassis.z_speed_set = chassis.z_speed_raw; + 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; } @@ -179,13 +181,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_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); } @@ -198,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; @@ -206,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_control, - chassis.motor[FRONT_RIGHT].speed_raw, - chassis.motor[FRONT_RIGHT].speed_set); - chassis.motor[FRONT_RIGHT].speed_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_control, - chassis.motor[BACK_RIGHT].speed_raw, - chassis.motor[BACK_RIGHT].speed_set); - chassis.motor[BACK_RIGHT].speed_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_control, - chassis.motor[FRONT_LEFT].speed_raw, - chassis.motor[FRONT_LEFT].speed_set); - chassis.motor[FRONT_LEFT].speed_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_control, - chassis.motor[BACK_LEFT].speed_raw, - chassis.motor[BACK_LEFT].speed_set); - chassis.motor[BACK_LEFT].speed_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_raw, - chassis.motor[FRONT_RIGHT].speed_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_raw, - chassis.motor[BACK_RIGHT].speed_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_raw, - chassis.motor[FRONT_LEFT].speed_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_raw, - chassis.motor[BACK_LEFT].speed_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/chassis_task/chassis_task.h b/user/TASK/chassis_task/chassis_task.h index 5560f55..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 @@ -39,42 +43,51 @@ #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 *motor_raw; + const motor_measure_t *motor_feedback; //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; 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_raw; + const RC_ctrl_t *rc_update; //Current front vector 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; @@ -83,11 +96,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 fa4d7b7..3286cb6 100644 --- a/user/TASK/gimbal_task/gimbal_task.c +++ b/user/TASK/gimbal_task/gimbal_task.c @@ -26,73 +26,119 @@ #include "remote_control.h" #include "USART_comms.h" #include +#include "pid.h" -float rc_channel_1; -float rc_channel_2; -const RC_ctrl_t* rc_ptr; +#define DEADBAND 10 + +// This is accessbile globally and some data is loaded from INS_task Gimbal_t gimbal; //UART mailbox char str[32] = {0}; +/******************** Functions ********************/ +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); - - 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(); + vTaskDelay(GIMBAL_INIT_DELAY); + initialization(&gimbal); while(1){ - /* For now using strictly encoder feedback for position */ - + //send_to_uart(&gimbal); - // 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. - 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); - } + /* For now using strictly encoder feedback for position */ - 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); - + 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_speed_set, gimbal.pitch_speed_set, 0, 0); - vTaskDelay(1); - //send_to_uart(gimbal_pitch_motor, pid_pitch, pitch_signal); //Sending data via UART + CAN_CMD_GIMBAL( (int16_t) gimbal.yaw_motor.current_out, + (int16_t) gimbal.pitch_motor.current_out, + 0, + 0); + + //Sending data via UART + vTaskDelay(GIMBAL_TASK_DELAY); } } +/** + * @brief Initializes gimbal struct and loads pointers for RC and motor feedback + * @param None + * @retval None + */ +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_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 = 6000; + gimbal_ptr->yaw_motor.pos_set = 6000; +} + +/** + * @brief Update encoder positions + * @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; + + 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; +} + +/** + * @brief Updates desired setpoints from RC signal + * @param Gimbal struct containing info on Gimbal + * @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; + + 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. +} +/** + * @brief Increments PID loop based on latest setpoints and latest positions + * @param None + * @retval None + */ +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); +} /** * @brief Reads vision instruction from UART and cap to certin values @@ -111,7 +157,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 @@ -120,28 +165,22 @@ 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_msg) { char str[20]; //uart data buffer - sprintf(str, "position: %d\n\r", gimbal.yaw_motor->pos_raw); - serial_send_string(str); + //TODO - fix below / fill as needed - sprintf(str, "speed: %d\n\r", gimbal.yaw_motor->speed_raw); +/* + sprintf(str, "yaw speed read: %d\n\r", gimbal->yaw_motor->speed_read); serial_send_string(str); - sprintf(str, "current: %d\n\r", gimbal.yaw_motor->current_raw); - serial_send_string(str); - - sprintf(str, "motor kp: %f\n\r", pid_pitch.Kp); - serial_send_string(str); - - sprintf(str, "motor kd: %f\n\r", pid_pitch.Kd); - 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 e7962ec..3a4fde4 100644 --- a/user/TASK/gimbal_task/gimbal_task.h +++ b/user/TASK/gimbal_task/gimbal_task.h @@ -8,6 +8,11 @@ #include "main.h" #include "CAN_receive.h" #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 *****************/ @@ -17,14 +22,20 @@ /****************************** 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 +#define GIMBAL_TASK_INIT_TIME 300 #define CONTROL_TIME 1 #define RC_MIN -660 #define RC_MAX 660 @@ -39,36 +50,34 @@ /************************** 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 *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 { - 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; - uint16_t pitch_pos_set; - uint16_t yaw_pos_set; + 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; + // TODO: Add gimbal angles when we care about orientation of robot in 3-d space - int16_t pitch_speed_raw; - int16_t yaw_speed_raw; - int16_t pitch_speed_set; - int16_t yaw_speed_set; } 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 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); //限幅函数