Skip to content

Rc for gimbal #13

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
BenYKB wants to merge 7 commits into
base: master
Choose a base branch
from
Open

Rc for gimbal #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
11a4aef
refactored
BenYKB Feb 16, 2020
6eb2661
now builds with refactor
BenYKB Feb 22, 2020
1801b6f
refactored to improve general readability, cleaned up gimbal function
BenYKB Feb 22, 2020
7087f85
fixed bugs on master lolololol
BenYKB Feb 23, 2020
5d3be5c
fixed chassis task pointers for consistency, added soft limits on gim…
BenYKB Feb 23, 2020
7b8b643
get vectored
Feb 23, 2020
3d413f5
resolved changes
Feb 23, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .vscode/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@
"files.associations": {
"can.h": "c",
"pid.h": "c",
"ins_task.h": "c"
"ins_task.h": "c",
"gimbal_task.h": "c"
}

}
10 changes: 5 additions & 5 deletions Project/embed-infantry.uvprojx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -892,16 +892,16 @@
<FileType>5</FileType>
<FilePath>..\user\TASK\start_task\start_task.h</FilePath>
</File>
<File>
<FileName>INS_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\user\TASK\INS_task\INS_task.c</FilePath>
</File>
<File>
<FileName>INS_task.h</FileName>
<FileType>5</FileType>
<FilePath>..\user\TASK\INS_task\INS_task.h</FilePath>
</File>
<File>
<FileName>INS_task.c</FileName>
<FileType>1</FileType>
<FilePath>..\user\TASK\INS_task\INS_task.c</FilePath>
</File>
<File>
<FileName>chassis_task.c</FileName>
<FileType>1</FileType>
Expand Down
28 changes: 14 additions & 14 deletions user/TASK/INS_task/INS_task.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}
Expand Down
158 changes: 80 additions & 78 deletions user/TASK/chassis_task/chassis_task.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 ********************/

Expand All @@ -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);

}
}

Expand Down Expand Up @@ -94,20 +96,20 @@ 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
chassis_init->vec_raw = get_INS_angle_point();
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();
}


Expand All @@ -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;
}
}

Expand All @@ -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
}
Expand All @@ -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;
}


Expand All @@ -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);
}


Expand All @@ -198,57 +200,57 @@ 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;
fp32 back_right;
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);
}

Expand Down
Loading