Skip to content

Commit

Permalink
shooting reversal
Browse files Browse the repository at this point in the history
  • Loading branch information
@jia-xie
jia-xie committed Jun 18, 2024
1 parent 39d9505 commit 49be636
Show file tree
Hide file tree
Showing 8 changed files with 88 additions and 50 deletions.
6 changes: 5 additions & 1 deletion src/app/inc/launch_task.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
#define FLYWHEEL_VELOCITY_20 (5000.0f * M3508_REDUCTION_RATIO)
#define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO)
#define FEED_HOLE_NUM (6.0f)
#define LAUNCH_FREQUENCY (12)
#define LAUNCH_FREQUENCY (20)
#define LAUNCH_PERIOD (1000.0f/LAUNCH_FREQUENCY)
#define FEED_1_PROJECTILE_ANGLE (2.0f*PI/FEED_HOLE_NUM)
#define FEED_FREQUENCY_6 (6.0f / FEED_HOLE_NUM * 60.0f)
Expand All @@ -28,11 +28,15 @@ typedef struct
uint8_t single_launch_flag;
uint8_t single_launch_finished_flag;
uint8_t burst_launch_flag;
uint8_t prev_burst_launch_flag;
uint8_t flywheel_enabled;
uint8_t reverse_burst_launch_pending_flag;

int16_t calculated_heat;
uint16_t heat_count;
uint16_t launch_freq_count;
uint8_t reverse_flag;
uint8_t prev_reverse_flag;
} Launch_Target_t;

void Launch_Task_Init(void);
Expand Down
1 change: 1 addition & 0 deletions src/app/inc/robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ typedef struct {
uint8_t prev_F;
uint8_t prev_Q;
uint8_t prev_E;
uint8_t prev_R;
uint8_t prev_left_switch;
uint8_t prev_right_switch;
} Key_Prev_t;
Expand Down
2 changes: 1 addition & 1 deletion src/app/inc/robot_tasks.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void Robot_Tasks_Start()
osThreadDef(motor_task, Robot_Tasks_Motor, osPriorityNormal, 0, 256);
motor_task_handle = osThreadCreate(osThread(motor_task), NULL);

osThreadDef(robot_control_task, Robot_Tasks_Robot_Control, osPriorityAboveNormal, 0, 2048);
osThreadDef(robot_control_task, Robot_Tasks_Robot_Control, osPriorityHigh, 0, 2048);
robot_control_task_handle = osThreadCreate(osThread(robot_control_task), NULL);

osThreadDef(ui_task, Robot_Tasks_UI, osPriorityAboveNormal, 0, 256);
Expand Down
4 changes: 2 additions & 2 deletions src/app/src/chassis_task.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -419,11 +419,11 @@ void _chassis_cmd()
forward_angle_diff += 2 * PI;
}
float backward_angle_diff = fmod(ideal_angle_diff + PI / 2, 2 * PI);
if (backward_angle_diff > PI)
while (backward_angle_diff > PI)
{
backward_angle_diff -= 2 * PI;
}
else if (backward_angle_diff < -PI)
while (backward_angle_diff < -PI)
{
backward_angle_diff += 2 * PI;
}
Expand Down
43 changes: 23 additions & 20 deletions src/app/src/debug_task.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ extern PID_t g_pid_anti_split;
const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n";
const char* bottom_border = "/***** End of Info *****/\r\n";
extern lqr_u_t g_u_left, g_u_right;
// #define DEBUG_ENABLED
#define DEBUG_ENABLED
#include "chassis_task.h"
extern Chassis_t g_chassis;
extern float vel_kalman;
Expand Down Expand Up @@ -61,28 +61,31 @@ void Debug_Task_Loop(void)
// }
// DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_swerve_fl.azimuth_motor->angle_pid->ref,g_swerve_fl.azimuth_motor->stats->absolute_angle_rad);
// DEBUG_PRINTF(&huart6, "/*%f,%f*/", g_robot_state.chassis_total_power, Referee_Robot_State.Chassis_Power);
DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B);
DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B);
// DEBUG_PRINTF(&huart6, ">TA_l:%f\n>TB_l:%f\n", g_u_left.T_A, g_u_left.T_B);
// DEBUG_PRINTF(&huart6, ">TA_r:%f\n>TB_r:%f\n", g_u_right.T_A, g_u_right.T_B);

DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot);
DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot);
DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output);
// DEBUG_PRINTF(&huart6, ">x_l:%f\n>x_dot_l:%f\n>theta_l:%f\n>theta_dot_l:%f\n>phi_l:%f\n>phi_dot_l:%f\n", g_lqr_left_state.x, g_lqr_left_state.x_dot, g_lqr_left_state.theta, g_lqr_left_state.theta_dot, g_lqr_left_state.phi, g_lqr_left_state.phi_dot);
// DEBUG_PRINTF(&huart6, ">x_r:%f\n>x_dot_r:%f\n>theta_r:%f\n>theta_dot_r:%f\n>phi_r:%f\n>phi_dot_r:%f\n", g_lqr_right_state.x, g_lqr_right_state.x_dot, g_lqr_right_state.theta, g_lqr_right_state.theta_dot, g_lqr_right_state.phi, g_lqr_right_state.phi_dot);
// DEBUG_PRINTF(&huart6, ">anti:%f\n", g_pid_anti_split.output);

DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x);
DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot);
DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x);
DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot);
// DEBUG_PRINTF(&huart6, ">left_x:%f\n", g_lqr_left_state.target_x);
// DEBUG_PRINTF(&huart6, ">left_x_dot:%f\n", g_lqr_left_state.target_x_dot);
// DEBUG_PRINTF(&huart6, ">right_x:%f\n", g_lqr_right_state.target_x);
// DEBUG_PRINTF(&huart6, ">right_x_dot:%f\n", g_lqr_right_state.target_x_dot);

DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch);
DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw);
DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch);
DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw);
DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman);
DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]);
DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]);
DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]);
DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS);
DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot);
// DEBUG_PRINTF(&huart6, ">pitch_fusion:%f\n", g_imu.rad_fusion.pitch);
// DEBUG_PRINTF(&huart6, ">yaw_fusion:%f\n", g_imu.rad_fusion.yaw);
// DEBUG_PRINTF(&huart6, ">pitch_m:%f\n", g_imu.deg.pitch);
// DEBUG_PRINTF(&huart6, ">yaw_m:%f\n", g_imu.deg.yaw);
// DEBUG_PRINTF(&huart6, ">kalman_test:%f\n", vel_kalman);
// DEBUG_PRINTF(&huart6, ">y_ddot:%f\n", g_imu.accel_earth[1]);
// DEBUG_PRINTF(&huart6, ">x_ddot:%f\n", g_imu.accel_earth[0]);
// DEBUG_PRINTF(&huart6, ">z_ddot:%f\n", g_imu.accel_earth[2]);
// DEBUG_PRINTF(&huart6, ">before:%f\n", -g_left_foot_motor->stats->velocity * DEG_TO_RAD * FOOT_WHEEL_RADIUS);
// DEBUG_PRINTF(&huart6, ">after:%f\n", g_lqr_left_state.x_dot);
DEBUG_PRINTF(&huart6, ">actual:%f\n", DJI_Motor_Get_Total_Angle(g_motor_feed ));
DEBUG_PRINTF(&huart6, ">target:%f\n", g_launch_target.feed_angle);

// DEBUG_PRINTF(&huart1, ">central:%f\n", g_chassis.centripetal_force);
// DEBUG_PRINTF(&huart1, ">r:%f\n", g_chassis.turning_radius);
// DEBUG_PRINTF(&huart1, ">yaw_rate:%f\n", g_imu.bmi088_raw.gyro[2]);
Expand Down
27 changes: 15 additions & 12 deletions src/app/src/launch_task.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ extern Remote_t g_remote;
extern IMU_t g_imu;
DJI_Motor_Handle_t *g_flywheel_left, *g_flywheel_right, *g_motor_feed;
Launch_Target_t g_launch_target;

float g_launch_target_step;
void Feed_Angle_Calc(void);

void Launch_Task_Init() {
Expand Down Expand Up @@ -59,7 +59,6 @@ void Launch_Task_Init() {
.kp = 5000.0f,
.kd = 3500000.0f,
.ki = 0.1f,
.kf = 5000.0f,
.output_limit = M2006_MAX_CURRENT,
.integral_limit = 1000.0f,
}
Expand All @@ -73,7 +72,7 @@ void Launch_Task_Init() {
void Launch_Ctrl_Loop() {
if (g_robot_state.enabled) {
if (g_launch_target.flywheel_enabled) {
g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30;
g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_LOW_FOR_DEBUG;
DJI_Motor_Set_Velocity(g_flywheel_left,g_launch_target.flywheel_velocity);
DJI_Motor_Set_Velocity(g_flywheel_right,g_launch_target.flywheel_velocity);
Feed_Angle_Calc();
Expand All @@ -96,12 +95,7 @@ void Feed_Angle_Calc()
g_launch_target.launch_freq_count++;
if (Referee_System.Online_Flag)
{
if (g_remote.mouse.right == 1)
{
g_launch_target.calculated_heat = Referee_Robot_State.Shooter_Heat_1;
}
if ((Referee_System.Robot_State.Shooter_Power_Output == 0) \
|| !g_launch_target.burst_launch_flag)
if (Referee_System.Robot_State.Shooter_Power_Output == 0 || !g_launch_target.burst_launch_flag)
{
g_launch_target.feed_angle = g_motor_feed->stats->total_angle_rad;
}
Expand All @@ -110,7 +104,7 @@ void Feed_Angle_Calc()
g_launch_target.calculated_heat -= Referee_Robot_State.Cooling_Rate/10;
__MAX_LIMIT(g_launch_target.calculated_heat,0,Referee_Robot_State.Heat_Max);
}
if (g_launch_target.burst_launch_flag)
if (g_launch_target.burst_launch_flag && !g_launch_target.reverse_flag)
{
if (g_launch_target.launch_freq_count*2 > LAUNCH_PERIOD)
{
Expand All @@ -122,7 +116,16 @@ void Feed_Angle_Calc()
}
}
DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE);
DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle);
}
DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle);
}
if(g_launch_target.reverse_flag && !g_launch_target.prev_reverse_flag)
// if (g_launch_target.reverse_burst_launch_pending_flag)
{
// g_launch_target.reverse_burst_launch_pending_flag = 0;
g_launch_target.feed_angle -= FEED_1_PROJECTILE_ANGLE;
DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE);
DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle);

}
}
}
53 changes: 40 additions & 13 deletions src/app/src/robot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,10 @@ void Robot_Cmd_Loop()
{
g_robot_state.wheel_facing_mode = 1;
}
if (g_remote.mouse.right == 1 && fabs(g_robot_state.chassis_x_speed) < 0.3f)
{
g_robot_state.wheel_facing_mode = 0;
}
if (g_remote.controller.left_switch == DOWN)
{
if (g_key_prev.prev_left_switch != DOWN)
Expand Down Expand Up @@ -179,20 +183,20 @@ void Robot_Cmd_Loop()
// this does not interfere with remote control
if (g_remote.controller.right_switch != UP)
{
g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * g_chassis_height_arr[g_current_height_index];
g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * g_chassis_height_arr[g_current_height_index];
}
/* Chassis ends here */

/* Gimbal starts here */
if ((g_remote.controller.right_switch == UP) || (g_remote.mouse.right == 1)) // mouse right button auto aim
{
#ifdef ORIN
#ifdef ORIN
if (g_orin_data.receiving.auto_aiming.yaw != 0 || g_orin_data.receiving.auto_aiming.pitch != 0)
{
g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw + g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin
g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw + g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin
g_robot_state.gimbal_pitch_angle = (1 - 0.2f) * g_robot_state.gimbal_pitch_angle + (0.2f) * (g_imu.rad.pitch + g_orin_data.receiving.auto_aiming.pitch / 180.0f * PI); // + orin
}
#endif
#endif
}
if ((g_remote.controller.right_switch == UP))
{
Expand All @@ -212,12 +216,12 @@ void Robot_Cmd_Loop()
yaw_increment = PI - 0.2f;
g_robot_state.gimbal_switching_dir_pending = 1;
}
else {
else
{
yaw_increment = (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 50000.0f);
yaw_increment = fabs(yaw_increment) > MAX_YAW_INCREMENT ? (fabs(yaw_increment) / (yaw_increment)) * MAX_YAW_INCREMENT : yaw_increment;

}
g_robot_state.gimbal_yaw_angle -= yaw_increment; // controller and mouse
g_robot_state.gimbal_yaw_angle -= yaw_increment; // controller and mouse
g_robot_state.gimbal_pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f - g_remote.mouse.y / 50000.0f); // controller and mouse
}

Expand All @@ -228,14 +232,15 @@ void Robot_Cmd_Loop()
/* Gimbal ends here */

/* Launch control starts here */
if (Referee_System.Power_Heat.Shooter_1_17mm_Heat < 200)
if (1)
{
g_launch_target.prev_burst_launch_flag = g_launch_target.burst_launch_flag;
if (g_remote.controller.wheel < -50.0f)
{ // dial wheel forward single fire
g_launch_target.single_launch_flag = 1;
g_launch_target.burst_launch_flag = 0;
}
else if ((g_remote.controller.wheel > 50.0f) || (g_remote.mouse.left == 1))
else if ((g_remote.controller.wheel > 50.0f) || (g_remote.mouse.left == 1) || (g_remote.mouse.right == 1))
{ // dial wheel backward burst fire
g_launch_target.single_launch_flag = 0;
g_launch_target.burst_launch_flag = 1;
Expand All @@ -246,6 +251,28 @@ void Robot_Cmd_Loop()
g_launch_target.single_launch_finished_flag = 0;
g_launch_target.burst_launch_flag = 0;
}
if (g_launch_target.burst_launch_flag == 1)
{
g_launch_target.flywheel_enabled = 1;
}
// reverse 1 projectile each time after shooting
g_launch_target.prev_reverse_flag = g_launch_target.reverse_flag;
if ((g_remote.keyboard.R == 1 && g_key_prev.prev_R == 0)
|| (g_remote.controller.wheel < -50.0f && g_launch_target.flywheel_enabled) ||
(!g_launch_target.burst_launch_flag && g_launch_target.prev_burst_launch_flag))
{
g_launch_target.reverse_flag = 1;
g_launch_target.calculated_heat = Referee_Robot_State.Shooter_Heat_1;
}
else
{
g_launch_target.reverse_flag = 0;
}
// if ((g_launch_target.burst_launch_flag == 0 && g_launch_target.prev_burst_launch_flag
// || g_remote.keyboard.R == 1 && g_key_prev.prev_R == 0))
// {
// g_launch_target.reverse_burst_launch_pending_flag = 1;
// }
}
else
{
Expand All @@ -263,7 +290,7 @@ void Robot_Cmd_Loop()
if (g_remote.keyboard.G == 1 && g_key_prev.prev_G == 0)
{
_toggle_robot_state(&g_robot_state.spintop_mode);
}
}
if (g_remote.keyboard.V == 1 && g_key_prev.prev_V == 0)
{
_toggle_robot_state(&g_robot_state.UI_enabled);
Expand All @@ -275,7 +302,7 @@ void Robot_Cmd_Loop()
__MAX_LIMIT(g_current_height_index, 0, 3);
}
if (g_remote.keyboard.C == 1 && g_key_prev.prev_C == 0)
{
{
// g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.13f;
g_current_height_index--;
__MAX_LIMIT(g_current_height_index, 0, 3);
Expand All @@ -291,6 +318,7 @@ void Robot_Cmd_Loop()
g_key_prev.prev_C = g_remote.keyboard.C;
g_key_prev.prev_Q = g_remote.keyboard.Q;
g_key_prev.prev_E = g_remote.keyboard.E;
g_key_prev.prev_R = g_remote.keyboard.R;
g_key_prev.prev_right_switch = g_remote.controller.right_switch;
/* Keyboard Toggles Start Here */

Expand Down Expand Up @@ -324,8 +352,7 @@ void Robot_Cmd_Loop()
{
// Ensure controller is down before enabling robot, ensure imu is initialized before enabling robot
#ifdef MASTER
if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) \
&& (g_remote.controller.left_stick.y != -660) && g_board_comm_first_part_established == 1 && g_board_comm_second_part_established == 1)
if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) && (g_remote.controller.left_stick.y != -660) && g_board_comm_first_part_established == 1 && g_board_comm_second_part_established == 1)
#else
if ((g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1))
#endif
Expand Down
2 changes: 1 addition & 1 deletion src/devices/inc/referee_system.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,7 +240,7 @@ typedef struct

struct __attribute__ ((__packed__))
{
uint16_t Type_17mm;
int16_t Type_17mm;
uint16_t Type_42mm;
uint16_t Type_Gold;
}Remaining_Ammo;
Expand Down

0 comments on commit 49be636

Please sign in to comment.