Skip to content

Commit

Permalink
RMUL version (#62)
Browse files Browse the repository at this point in the history 
* referee uart bug sometimes occur, level indicator added

* referee alignment fixed, safe mode when referee package lose connection added

* barrel heat control added, not fully tested

* barrel heat regulation added, still need some final adjustments

* huge bug found, gonna fix it tomorrow

* bug fixed

* power regulation tuned

* final final tuning -leo

---------

Co-authored-by: Leo Liu <65527170+CuboiLeo@users.noreply.github.com>
  • Loading branch information
@jia-xie @CuboiLeo
jia-xie and CuboiLeo committed Jun 12, 2024
1 parent 2bfdf0a commit 0e1327e
Show file tree
Hide file tree
Showing 27 changed files with 521 additions and 334 deletions.
2 changes: 2 additions & 0 deletions Makefile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,7 @@ src/devices/src/remote.c \
src/devices/src/imu_task.c \
src/devices/src/referee_system.c \
src/devices/src/jetson_orin.c \
src/devices/src/supercap.c \
src/app/src/motor_task.c \
src/app/src/chassis_task.c \
src/app/src/gimbal_task.c \
Expand All @@ -154,6 +155,7 @@ src/ui/src/ui_indicator_0_7.c \
src/ui/src/ui_indicator_0_8.c \
src/ui/src/ui_indicator_0_9.c \
src/ui/src/ui_indicator_0_10.c \
src/ui/src/ui_indicator_0_11.c \
src/ui/src/ui_indicator_1_0.c

# ASM sources
Expand Down
2 changes: 1 addition & 1 deletion src/algo/inc/Swerve_Locomotion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
#define NUMBER_OF_MODULES 4

#define SWERVE_OPTIMIZE
#define POWER_CONTROL
#define POWER_REGULATION

typedef struct
{
Expand Down
2 changes: 1 addition & 1 deletion src/algo/inc/user_math.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
} \
} while (0);

#define BUFFER_SIZE 500
#define BUFFER_SIZE (1000)
#define __MOVING_AVERAGE(buffer, index, update_value, count, sum, average) \
do \
{ \
Expand Down
99 changes: 43 additions & 56 deletions src/algo/src/Swerve_Locomotion.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,8 @@

Swerve_Module_t g_swerve_fl, g_swerve_rl, g_swerve_rr, g_swerve_fr;
Swerve_Module_t *swerve_modules[NUMBER_OF_MODULES] = {&g_swerve_fl, &g_swerve_rl, &g_swerve_rr, &g_swerve_fr};
Module_State_t optimized_module_state;
float last_swerve_angle[NUMBER_OF_MODULES] = {0.0f, 0.0f, 0.0f, 0.0f};
Kalman_Filter_t power_KF = {.Prev_P = 1.0f, .Q = 0.0001, .R = 5.0f};

/**
* @brief Inverse kinematics matrix for a 4 module swerve, defined counterclockwise from the front left
Expand Down Expand Up @@ -42,14 +42,14 @@ void Set_Module_Output(Swerve_Module_t *swerve_module, Module_State_t desired_st
void Swerve_Init()
{
// define constants for each module in an array [0] == fl, [1] == rl, [2] == rr, [3] == fr
int azimuth_can_bus_array[NUMBER_OF_MODULES] = {1, 1, 2, 2};
int azimuth_can_bus_array[NUMBER_OF_MODULES] = {2, 2, 2, 2};
int azimuth_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4};
int azimuth_offset_array[NUMBER_OF_MODULES] = {1990, 730, 6060, 1362};
int azimuth_offset_array[NUMBER_OF_MODULES] = {2050, 1940, 1430, 8150};
Motor_Reversal_t azimuth_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED};

int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 1, 2, 2};
int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 2, 2, 2};
int drive_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4};
Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_NORMAL};
Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED};

// init common PID configuration for azimuth motors
Motor_Config_t azimuth_motor_config = {
Expand Down Expand Up @@ -179,45 +179,22 @@ Module_State_t Optimize_Module_Angle(Module_State_t input_state, float measured_
{
Module_State_t optimized_module_state = {0};
optimized_module_state.speed = input_state.speed;
float target_angle = fmodf(input_state.angle, 2.0f * PI); // get positive normalized target angle
if (target_angle < 0)
{
target_angle += 2.0f * PI;
}
optimized_module_state.angle = input_state.angle;

float curr_angle_normalized = fmodf(measured_angle, 2 * PI); // get normalized current angle
if (curr_angle_normalized < 0)
{
curr_angle_normalized += 2.0f * PI;
}
if(measured_angle > PI)
measured_angle -= 2*PI;

float delta = target_angle - curr_angle_normalized; // calculate the angle delta
float angle_diff = optimized_module_state.angle - measured_angle;

// these conditions essentially create the step function for angles to "snap" too offset from the measured angle to avoid flipping
if (PI / 2.0f < fabsf(delta) && fabsf(delta) < 3 * PI / 2.0f)
{
float beta = PI - fabsf(delta);
beta *= (delta > 0 ? 1.0f : -1.0f);
target_angle = measured_angle - beta;
optimized_module_state.speed = -1.0f * input_state.speed; // invert velocity if optimal angle 180 deg from target
}
else if (fabsf(delta) >= 3.0f * PI / 2.0f)
{
if (delta < 0)
{
target_angle = measured_angle + (2.0f * PI + delta);
}
else
{
target_angle = measured_angle - (2.0f * PI - delta);
}
}
else
{
target_angle = measured_angle + delta;
}
if((angle_diff >= PI/2 && angle_diff <= 3*PI/2) || (angle_diff <= -PI/2 && angle_diff >= -3*PI/2))
{
optimized_module_state.angle += (angle_diff > 0) ? -PI:PI;
optimized_module_state.speed *= -1.0f;
}

if (optimized_module_state.angle < 0)
optimized_module_state.angle += 2.0f * PI;

optimized_module_state.angle = target_angle;
return optimized_module_state;
}

Expand Down Expand Up @@ -261,27 +238,37 @@ void Swerve_Drive(float x, float y, float omega)
y *= SWERVE_MAX_SPEED;
omega *= SWERVE_MAX_ANGLUAR_SPEED; // convert to rad/s
#ifdef POWER_REGULATION
if(fabs(x) > 0.1f || fabs(y) > 0.1f || fabs(omega) > 0.1f)
if(Referee_System.Online_Flag)
{
__MOVING_AVERAGE(g_robot_state.chassis_power_buffer,g_robot_state.chassis_power_index,
Referee_Robot_State.Chassis_Power,g_robot_state.chassis_power_count,g_robot_state.chassis_total_power,g_robot_state.chassis_avg_power);
if(g_robot_state.chassis_avg_power < (Referee_Robot_State.Chassis_Power_Max*0.7f))
g_robot_state.power_increment_ratio += 0.001f;
if(fabs(x) > 0.1f || fabs(y) > 0.1f || fabs(omega) > 0.1f)
{
__MOVING_AVERAGE(g_robot_state.chassis_power_buffer,g_robot_state.chassis_power_index,
Referee_Robot_State.Chassis_Power,g_robot_state.chassis_power_count,g_robot_state.chassis_total_power,g_robot_state.chassis_avg_power);
if(g_robot_state.chassis_avg_power < (Referee_Robot_State.Chassis_Power_Max*(0.9f-Referee_Robot_State.Level*0.2f)))
g_robot_state.power_increment_ratio += 0.001f;
else
g_robot_state.power_increment_ratio -= 0.001f;
}
else
g_robot_state.power_increment_ratio -= 0.001f;
}
{
memset(g_robot_state.chassis_power_buffer,0,sizeof(g_robot_state.chassis_power_buffer));
g_robot_state.chassis_power_index = 0;
g_robot_state.chassis_power_count = 0;
g_robot_state.chassis_total_power = 0;
g_robot_state.power_increment_ratio = 1.0f;
}
__MAX_LIMIT(g_robot_state.power_increment_ratio, 0.8f, 5.0f);
x *= g_robot_state.power_increment_ratio; // convert to m/s
y *= g_robot_state.power_increment_ratio;
omega *= g_robot_state.power_increment_ratio; // convert to rad/s
}
else
{
memset(g_robot_state.chassis_power_buffer,0,sizeof(g_robot_state.chassis_power_buffer));
g_robot_state.chassis_power_index = 0;
g_robot_state.chassis_power_count = 0;
g_robot_state.chassis_total_power = 0;
g_robot_state.power_increment_ratio = 1.0f;
g_robot_state.power_increment_ratio = 1 + Referee_Robot_State.Manual_Level*0.1f;
x *= g_robot_state.power_increment_ratio; // convert to m/s
y *= g_robot_state.power_increment_ratio;
omega *= g_robot_state.power_increment_ratio; // convert to rad/s
}
__MAX_LIMIT(g_robot_state.power_increment_ratio, 0.8f, 5.0f);
x *= g_robot_state.power_increment_ratio; // convert to m/s
y *= g_robot_state.power_increment_ratio;
omega *= g_robot_state.power_increment_ratio; // convert to rad/s
#endif
Chassis_Speeds_t desired_chassis_speeds = {.x = x, .y = y, .omega = omega};
Set_Desired_States(Chassis_Speeds_To_Module_States(desired_chassis_speeds));
Expand Down
12 changes: 10 additions & 2 deletions src/app/inc/launch_task.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,19 @@
#include <stdint.h>
#include "dji_motor.h"

#define FLYWHEEL_VELOCITY_23 (6000.0f * M3508_REDUCTION_RATIO)
#define FLYWHEEL_VELOCITY_10 (3000.0f * M3508_REDUCTION_RATIO)
#define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO)
#define FEED_HOLE_NUM (6.0f)
#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)
#define FEED_FREQUENCY_12 (12.0f / FEED_HOLE_NUM * 60.0f)
#define FEED_FREQUENCY_18 (18.0f / FEED_HOLE_NUM * 60.0f)
#define FEED_FREQUENCY_20 (20.0f / FEED_HOLE_NUM * 60.0f)
#define FEED_FREQUENCY_30 (30.0f / FEED_HOLE_NUM * 60.0f)

//#define HEAT_CONTROL

typedef struct
{
float flywheel_velocity;
Expand All @@ -22,6 +26,10 @@ typedef struct
uint8_t single_launch_finished_flag;
uint8_t burst_launch_flag;
uint8_t flywheel_enabled;

int16_t calculated_heat;
uint16_t heat_count;
uint16_t launch_freq_count;
} Launch_Target_t;

void Launch_Task_Init(void);
Expand Down
4 changes: 3 additions & 1 deletion src/app/inc/robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define ROBOT_H

#include <stdint.h>
#include "user_math.h"

typedef struct
{
Expand All @@ -14,7 +15,7 @@ typedef struct
float chassis_y_speed;
float chassis_omega;

float chassis_power_buffer[500];
float chassis_power_buffer[BUFFER_SIZE];
uint16_t chassis_power_index;
uint16_t chassis_power_count;
float chassis_avg_power;
Expand All @@ -34,6 +35,7 @@ typedef struct
uint8_t prev_B;
uint8_t prev_G;
uint8_t prev_V;
uint8_t prev_Z;
uint8_t prev_left_switch;
} Key_Prev_t;

Expand Down
4 changes: 2 additions & 2 deletions src/app/inc/robot_tasks.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ void Robot_Tasks_UI(void const *argument)
{
portTickType xLastWakeTime;
xLastWakeTime = xTaskGetTickCount();
const TickType_t TimeIncrement = pdMS_TO_TICKS(100);
const TickType_t TimeIncrement = pdMS_TO_TICKS(1);
while (1)
{
UI_Task_Loop();
Expand All @@ -116,7 +116,7 @@ void Robot_Tasks_Jetson_Orin(void const *argument)
const TickType_t TimeIncrement = pdMS_TO_TICKS(JETSON_ORIN_PERIOD);
while (1)
{
//Jetson_Orin_Send_Data();
Jetson_Orin_Send_Data();
vTaskDelayUntil(&xLastWakeTime, TimeIncrement);
}
}
Expand Down
5 changes: 2 additions & 3 deletions src/app/src/chassis_task.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
#include "imu_task.h"
#include "Swerve_Locomotion.h"

#define SPINTOP_RAMP_COEF (0.003f)
#define SPINTOP_RAMP_COEF (0.03f)
#define SPIN_TOP_OMEGA (1.0f)

extern Robot_State_t g_robot_state;
Expand All @@ -23,7 +23,6 @@ void Chassis_Task_Init()

void Chassis_Ctrl_Loop()
{

if (g_robot_state.enabled)
{
/*
Expand All @@ -34,7 +33,7 @@ void Chassis_Ctrl_Loop()
if (g_robot_state.spintop_mode)
{
float translation_speed = sqrtf(powf(g_robot_state.chassis_x_speed, 2) + powf(g_robot_state.chassis_y_speed, 2));
float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed + chassis_rad * SPIN_TOP_OMEGA);
float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed*0.2f + chassis_rad * SPIN_TOP_OMEGA);

// ramp up to target omega
float target_omega = SPIN_TOP_OMEGA * spin_coeff;
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 @@ -15,43 +15,46 @@
extern Robot_State_t g_robot_state;
extern DJI_Motor_Handle_t *g_yaw;
extern IMU_t g_imu;
extern Swerve_Module_t g_swerve_fl;
extern Swerve_Module_t g_swerve_rl, g_swerve_fr;
extern Remote_t g_remote;
extern Launch_Target_t g_launch_target;
extern uint64_t t;
extern Daemon_Instance_t *g_daemon_instances[3];
extern Daemon_Instance_t *g_remote_daemon;
#define PRINT_RUNTIME_STATS
extern Daemon_Instance_t *g_referee_daemon_instance_ptr;
extern float test_tmd;
//#define PRINT_RUNTIME_STATS
#ifdef PRINT_RUNTIME_STATS
char g_debug_buffer[1024*2] = {0};
#endif

const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n";
const char* bottom_border = "/***** End of Info *****/\r\n";

#define DEBUG_ENABLED
//#define DEBUG_ENABLED

void Debug_Task_Loop(void)
{
#ifdef DEBUG_ENABLED
// static uint32_t counter = 0;
// #ifdef PRINT_RUNTIME_STATS
// if (counter % 100 == 0) // Print every 100 cycles
// {
// vTaskGetRunTimeStats(g_debug_buffer);
// DEBUG_PRINTF(&huart6, "%s", top_border);
// DEBUG_PRINTF(&huart6, "%s", g_debug_buffer);
// DEBUG_PRINTF(&huart6, "%s", bottom_border);
// }
// #endif

static uint32_t counter = 0;
#ifdef PRINT_RUNTIME_STATS
if (counter % 100 == 0) // Print every 100 cycles
{
vTaskGetRunTimeStats(g_debug_buffer);
DEBUG_PRINTF(&huart6, "%s", top_border);
DEBUG_PRINTF(&huart6, "%s", g_debug_buffer);
DEBUG_PRINTF(&huart6, "%s", bottom_border);
}
#endif
//DEBUG_PRINTF(&huart6, ">time:%.1f\n>ref:%f\n",(float) counter / 1000.0f * DEBUG_PERIOD,Referee_Robot_State.Chassis_Power);
// DEBUG_PRINTF(&huart6, ">time:%.1f\n>yaw:%f\n>pitch:%f\n>roll:%f\n", (float) counter / 1000.0f * DEBUG_PERIOD,
// g_imu.deg.yaw, g_imu.deg.pitch, g_imu.deg.roll);
// DEBUG_PRINTF(&huart6, ">remote_daemon:%d\n", g_remote_daemon->counter);
// counter++;
// if (counter > 0xFFFFFFFF) {
// counter = 0;
// }
DEBUG_PRINTF(&huart6, ">ref:%f\n>act:%f\n",g_motor_feed->velocity_pid->ref,g_motor_feed->stats->current_vel_rpm);
counter++;
if (counter > 0xFFFFFFFF) {
counter = 0;
}

// DEBUG_PRINTF(&huart1, ">fr_ref_speed:%f\n>fr_actual_speed:%f\n",
// g_swerve_fr.drive_motor->velocity_pid->ref,g_swerve_fr.drive_motor->stats->current_vel_rpm);
#endif
}
4 changes: 2 additions & 2 deletions src/app/src/gimbal_task.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ void Gimbal_Task_Init()
Motor_Config_t yaw_motor_config = {
.can_bus = 1,
.speed_controller_id = 3,
.offset = 3690,
.offset = 2400,
.control_mode = POSITION_VELOCITY_SERIES,
.motor_reversal = MOTOR_REVERSAL_NORMAL,
.use_external_feedback = 1,
Expand All @@ -67,7 +67,7 @@ void Gimbal_Task_Init()
};

Motor_Config_t pitch_motor_config = {
.can_bus = 2,
.can_bus = 1,
.speed_controller_id = 2,
.offset = 4460,
.use_external_feedback = 1,
Expand Down
Loading

0 comments on commit 0e1327e

Please sign in to comment.