Skip to content
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

Merge Fix/low velocity error to development #99

Merged
merged 3 commits into from
Nov 30, 2023
Merged

Merge Fix/low velocity error to development #99

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b236481
Fixed stateInfo.wheelSpeeds being overwritten with wrong values
jujumuch Nov 28, 2023
be575f4
Added dry friction compensation to feedforward wheels
jujumuch Nov 28, 2023
b703076
Feedforward dry friction implemented
jujumuch Nov 28, 2023
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
2 changes: 1 addition & 1 deletion Core/Inc/roboteam_embedded_messages
Open in desktop
Submodule roboteam_embedded_messages updated 3 files
+40 −24 README.md
+228 −42 generator/Generator.py
+47 −0 generator/README.md
11 changes: 6 additions & 5 deletions Core/Src/Control/stateEstimation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,16 +112,17 @@ float stateEstimation_GetFilteredRoT() {
///////////////////////////////////////////////////// PRIVATE FUNCTION IMPLEMENTATIONS

static void wheels2Body(float wheelSpeeds[4], float output[3]){

float wheelSpeedsLinear[4]; // The linear speed for each wheel [m/s]

// Translate angular wheel velocities [rad/s] into regular velocities [m/s] on the outside of each wheel.
for (wheel_names wheel=wheels_RF; wheel <= wheels_RB; wheel++) {
wheelSpeeds[wheel] = wheelSpeeds[wheel] * rad_wheel;
wheelSpeedsLinear[wheel] = wheelSpeeds[wheel] * rad_wheel;
}

// Translate the wheel speeds into local u, v and r * w speeds.
output[vel_u] = wheelSpeeds[0] * Dinv[0] + wheelSpeeds[1] * Dinv[1] + wheelSpeeds[2] * Dinv[2] + wheelSpeeds[3] * Dinv[3];
output[vel_v] = wheelSpeeds[0] * Dinv[4] + wheelSpeeds[1] * Dinv[5] + wheelSpeeds[2] * Dinv[6] + wheelSpeeds[3] * Dinv[7];
output[vel_w] = wheelSpeeds[0] * Dinv[8] + wheelSpeeds[1] * Dinv[9] + wheelSpeeds[2] * Dinv[10] + wheelSpeeds[3] * Dinv[11];
output[vel_u] = wheelSpeedsLinear[0] * Dinv[0] + wheelSpeedsLinear[1] * Dinv[1] + wheelSpeedsLinear[2] * Dinv[2] + wheelSpeedsLinear[3] * Dinv[3];
output[vel_v] = wheelSpeedsLinear[0] * Dinv[4] + wheelSpeedsLinear[1] * Dinv[5] + wheelSpeedsLinear[2] * Dinv[6] + wheelSpeedsLinear[3] * Dinv[7];
output[vel_w] = wheelSpeedsLinear[0] * Dinv[8] + wheelSpeedsLinear[1] * Dinv[9] + wheelSpeedsLinear[2] * Dinv[10] + wheelSpeedsLinear[3] * Dinv[11];

// Translate the rotational velocity of the robot back to rad/s.
output[vel_w] = output[vel_w] / rad_robot;
Expand Down
15 changes: 14 additions & 1 deletion Core/Src/peripherals/wheels.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -170,8 +170,21 @@ void wheels_Update(){
wheelsK[wheel].I = 0;
}

float feed_forward[4];
float threshold = 0.05;

if (abs(wheels_commanded_speeds[wheel]) < threshold) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was just wondering about the reason for this threshold value, since there is also already a PWM treshold that makes sure that the robot is not slightly jittering on the field, if no explicit instructions are send out.

See

roboteam_microcontroller5.0/Core/Src/peripherals/wheels.c

Line 325 in fc08aa7

static void limitWheelPWMs(uint32_t pwms[4]){

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh no I don't think this is linked to the PWM signal. It is only linked to the wheel velocity and the dry friction. Here the code simply adds (if driving forward) or subtract (if driving backwards) 13 to the feedforward term. However the feedforward term is equal to 0 if wheel velocity is almost 0 (< 0.05), thus 0.05 if your threshold.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

frcition
picture b

feed_forward[wheel] = 0;
}
else if (wheels_commanded_speeds[wheel] > 0) {
feed_forward[wheel] = wheels_commanded_speeds[wheel] + 13;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It would be nice to document on why 13 is being added (or subtracted on line 183). It would be better to rename it to some constant that can then have its own documentation string too.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes 13 came from implementing the dry friction. The constant was found after testing our robot. But yes you're right I will rename it as a constant, it is probably more clear.

}
else if (wheels_commanded_speeds[wheel] < 0) {
feed_forward[wheel] = wheels_commanded_speeds[wheel] - 13;
}

// Add PID to commanded speed and convert to PWM
int32_t wheel_speed = OMEGAtoPWM * (wheels_commanded_speeds[wheel] + PID(angular_velocity_error, &wheelsK[wheel]));
int32_t wheel_speed = OMEGAtoPWM * (feed_forward[wheel] + PID(angular_velocity_error, &wheelsK[wheel]));

// Determine direction and if pwm is negative, switch directions
// PWM < 0 : CounterClockWise. Direction = 0
Expand Down
Loading