Skip to content

Commit

Permalink
Dev acceleration ramp (#16)
Browse files Browse the repository at this point in the history 
* add acceleration ramp on action

* rampe accélération via Speed_Driver

* rampe accélération et de décélération

* rampe accélération et décélération non symétrique, fonctionnelle

* correction de trajectoire smooth

* rotation bug fix

* rotation bug fix

---------

Co-authored-by: Florian BARRE <florian.barre78@gmail.com>
Co-authored-by: Florian-BARRE <flo.others@gmail.com>
  • Loading branch information
@UrbsKali @Florian-BARRE
3 people authored Feb 1, 2024
1 parent 6a4e0a9 commit 2c31963
Show file tree
Hide file tree
Showing 6 changed files with 49 additions and 177 deletions.
1 change: 1 addition & 0 deletions teensy/.vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
"sstream": "cpp",
"streambuf": "cpp",
"cmath": "cpp",
"cmath": "cpp",
"array": "cpp",
"string_view": "cpp",
"initializer_list": "cpp",
Expand Down
143 changes: 2 additions & 141 deletions teensy/lib/actions/include/structures.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ struct Precision_Params
unsigned int trajectory_precision;
};


struct Rolling_Basis_Params
{
int encoder_resolution;
Expand Down Expand Up @@ -90,144 +91,4 @@ struct Profil_params
byte offset;
float gamma; // Gamma is the slope of the affine line representing the acceleration profile
float distance;
};

/*
struct Profil_params
{
byte offset;
float gamma; // Gamma is the slope of the affine line representing the acceleration profile
float distance;
};
class Speed_Driver
{
// How to use speed Driver:
// Give gamma and offset
// 2 cases:
// 1°) Give Gamma value, it will be directly use to compute the speed (use Speed_Driver_From_Gamma class)
// speed = gamma * ticks + offset (speed <= max_speed)
// 2°) Give distance_to_max_speed, it will be use to compute gamma (use Speed_Driver_From_Distance class)
// The distance_to_max_speed has to be given in cm, when the compute function will be
// called, the encoder_resolution and wheel_perimeter will be used to convert it in ticks
// Gamma = (max_speed - offset) / (distance_to_max_speed * encoder_resoltion / wheel_perimeter)
public:
// Attributes
byte max_speed;
long end_ticks;
bool correction = false;
// Acceleration params
Profil_params acceleration_params = {0, -1.0f, -1.0f};
// Deceleration params
Profil_params deceleration_params = {0, -1.0f, -1.0f};
Speed_Driver() = default;
// Methodes
void compute_acceleration_profile(Rolling_Basis_Params *rolling_basis_params, long end_ticks)
{
this->end_ticks = end_ticks;
// Compute acceleration profile
if (this->acceleration_params.gamma == -1.0f)
{
byte y_delta = this->max_speed - this->acceleration_params.offset;
float distance_to_max_speed_ticks = (this->acceleration_params.distance * rolling_basis_params->encoder_resolution) / rolling_basis_params->wheel_perimeter;
this->acceleration_params.gamma = (float)y_delta / distance_to_max_speed_ticks;
}
// Compute deceleration profile
if (this->deceleration_params.gamma == -1.0f)
{
byte y_delta = this->max_speed - this->deceleration_params.offset;
float distance_to_speed_down_ticks = this->end_ticks - (this->deceleration_params.distance * rolling_basis_params->encoder_resolution) / rolling_basis_params->wheel_perimeter;
this->deceleration_params.gamma = (float)y_delta / distance_to_speed_down_ticks; // Negative because we want to decelerate
}
}
byte compute_local_speed(long ticks)
{
// Calcul des points de changement de vitesse (acceleration -> plateau -> deceleration)
long debut_plateau_ticks = (this->max_speed - this->acceleration_params.offset) / this->acceleration_params.gamma;
long fin_plateau_ticks = ((this->max_speed - this->deceleration_params.offset) / this->deceleration_params.gamma) + this->end_ticks;
byte speed = 0;
// On verifie qu'il y a un plateau
if (debut_plateau_ticks < fin_plateau_ticks)
{
// On est dans la phase d'acceleration
if (ticks < debut_plateau_ticks)
speed = (byte)(this->acceleration_params.gamma * ticks + this->acceleration_params.offset);
// On est dans la phase de plateau
else if (ticks < fin_plateau_ticks)
speed = this->max_speed;
// On est dans la phase de deceleration
else
speed = (byte)(this->deceleration_params.gamma * (ticks - this->end_ticks) + this->deceleration_params.offset);
}
// Pas de plateau
else
{
// Calcul du point d'intersection de l'accélération et de la décélération
long intersection_ticks = (this->deceleration_params.offset - this->acceleration_params.offset - (this->deceleration_params.gamma * this->end_ticks)) / (this->acceleration_params.gamma - this->deceleration_params.gamma);
// phase acceleration
if(ticks < intersection_ticks)
speed = (byte)(this->acceleration_params.gamma * ticks + this->acceleration_params.offset);
// phase deceleration
else
speed = (byte)(this->deceleration_params.gamma * (ticks - this->end_ticks) + this->deceleration_params.offset);
}
if (this->correction)
{
this->correction = false;
return 80;
}
return speed;
}
};
class Speed_Driver_From_Gamma : public Speed_Driver
{
public:
// We only need to give gamma and offset, the distance is not used in this case (it's set to -1.0f by default and will be ingored if given)
Speed_Driver_From_Gamma(byte max_speed, Profil_params acceleration, Profil_params deceleration)
{
this->max_speed = max_speed;
// Acceleration params
this->acceleration_params.offset = acceleration.offset;
this->acceleration_params.gamma = acceleration.gamma;
// Deceleration params
this->deceleration_params.offset = deceleration.offset;
this->deceleration_params.gamma = deceleration.gamma;
}
};
class Speed_Driver_From_Distance : public Speed_Driver
{
public:
// We only need to give distance and offset, the gamma is not used in this case (it's set to -1.0f by default and will be ingored if given)
Speed_Driver_From_Distance(byte max_speed, Profil_params acceleration, Profil_params deceleration)
{
this->max_speed = max_speed;
// Acceleration params
this->acceleration_params.offset = acceleration.offset;
this->acceleration_params.distance = acceleration.distance;
// Deceleration params
this->deceleration_params.offset = deceleration.offset;
this->deceleration_params.distance = deceleration.distance;
}
};
*/
};
26 changes: 18 additions & 8 deletions teensy/lib/actions/src/basic_action.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,13 @@ void Basic_Action::handle(Point current_point, Ticks current_ticks, Rolling_Basi

// Move Straight
Move_Straight::Move_Straight(float target_x, float target_y, Direction *direction, Speed_Driver *speed_driver, Precision_Params *precision_params)
Move_Straight::Move_Straight(float target_x, float target_y, Direction *direction, Speed_Driver *speed_driver, Precision_Params *precision_params)
{
this->target_x = target_x;
this->target_y = target_y;
this->direction = direction;
this->speed_driver = speed_driver;
this->speed_driver = speed_driver;
this->precision_params = precision_params;
}

Expand All @@ -32,6 +34,7 @@ void Move_Straight::compute(Point current_point, Ticks current_ticks, Rolling_Ba

// Create the step action
this->step_action = new Step_Forward_Backward(distance, this->speed_driver, this->precision_params);
this->step_action = new Step_Forward_Backward(distance, this->speed_driver, this->precision_params);

// Compute the step action
this->step_action->compute(current_ticks, rolling_basis_params);
Expand All @@ -40,33 +43,37 @@ void Move_Straight::compute(Point current_point, Ticks current_ticks, Rolling_Ba

// Get Orientation in front of a point (turn on itself)
Get_Orientation::Get_Orientation(float target_x, float target_y, Direction *direction, Speed_Driver *speed_driver, Precision_Params *precision_params)
Get_Orientation::Get_Orientation(float target_x, float target_y, Direction *direction, Speed_Driver *speed_driver, Precision_Params *precision_params)
{
this->target_x = target_x;
this->target_y = target_y;
this->direction = direction;
this->speed_driver = speed_driver;
this->speed_driver = speed_driver;
this->precision_params = precision_params;
}

void Get_Orientation::compute(Point current_point, Ticks current_ticks, Rolling_Basis_Params *rolling_basis_params)
{
// Compute the angle to turn
float theta_dif = Point::angle(Point(this->target_x, this->target_y), current_point);
float theta = fmod((theta_dif - current_point.theta + PI), (2 * PI));
float theta = fmod((theta_dif - current_point.theta + PI), 2 * PI);

// Add PI rad if the direction is backward
if ((*this->direction) == backward)
theta += PI;
//Check if the angle is greater than PI
if (theta > PI)
{
//adjust the directun, turn to the left
theta -= (2* PI);

// Normalize the angle between -PI and PI
if (theta > PI) {
theta -= 2 * PI;
} else if (theta < -PI) {
theta += 2 * PI;
}



// Create the step action
this->step_action = new Step_Rotation(theta, this->speed_driver, this->precision_params);
this->step_action = new Step_Rotation(theta, this->speed_driver, this->precision_params);

// Compute the step action
this->step_action->compute(current_ticks, rolling_basis_params);
Expand All @@ -75,10 +82,12 @@ void Get_Orientation::compute(Point current_point, Ticks current_ticks, Rolling_

// Do a Rotation (turn on itself)
Move_Rotation::Move_Rotation(float target_theta, Direction *direction, Speed_Driver *speed_driver, Precision_Params *precision_params)
Move_Rotation::Move_Rotation(float target_theta, Direction *direction, Speed_Driver *speed_driver, Precision_Params *precision_params)
{
this->target_theta = target_theta;
this->direction = direction;
this->speed_driver = speed_driver;
this->speed_driver = speed_driver;
this->precision_params = precision_params;
}

Expand All @@ -95,6 +104,7 @@ void Move_Rotation::compute(Point current_point, Ticks current_ticks, Rolling_Ba

// Create the step action
this->step_action = new Step_Rotation(delta_theta, this->speed_driver, this->precision_params);
this->step_action = new Step_Rotation(delta_theta, this->speed_driver, this->precision_params);

// Compute the step action
this->step_action->compute(current_ticks, rolling_basis_params);
Expand Down
2 changes: 1 addition & 1 deletion teensy/lib/rolling_basis/include/rolling_basis.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ public :
inline float radius() { return this->center_distance / 2.0; };
inline float wheel_perimeter() { return this->wheel_diameter * PI; };
inline float wheel_unit_tick_cm() { return this->wheel_perimeter() / this->encoder_resolution; };
byte max_pwm;
byte standby_pwm;

// Properties
Point get_current_position();
Expand Down
8 changes: 4 additions & 4 deletions teensy/lib/rolling_basis/src/rolling_basis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,13 +52,13 @@ void Rolling_Basis::init_motors()
this->left_motor->init();
}

void Rolling_Basis::init_rolling_basis(float x, float y, float theta, long inactive_delay, byte max_pwm)
void Rolling_Basis::init_rolling_basis(float x, float y, float theta, long inactive_delay, byte standby_pwm)
{
this->X = x;
this->Y = y;
this->THETA = theta;
this->inactive_delay = inactive_delay;
this->max_pwm = max_pwm;
this->standby_pwm = standby_pwm;
}

// Odometrie function
Expand Down Expand Up @@ -108,8 +108,8 @@ void Rolling_Basis::reset_position(){

// Motors action function
void Rolling_Basis::keep_position(long current_right_ticks, long current_left_ticks) {
this->right_motor->handle(current_right_ticks, this->max_pwm);
this->left_motor->handle(current_left_ticks, this->max_pwm);
this->right_motor->handle(current_right_ticks, this->standby_pwm);
this->left_motor->handle(current_left_ticks, this->standby_pwm);
}

void Rolling_Basis::shutdown_motor()
Expand Down
46 changes: 23 additions & 23 deletions teensy/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,24 +5,20 @@
#include <messages.h>

// Mouvement params
#define ACTION_ERROR_AUTH 20
#define TRAJECTORY_PRECISION 50
#define ACTION_ERROR_AUTH 10
#define TRAJECTORY_PRECISION 100
#define NEXT_POSITION_DELAY 100
#define INACTIVE_DELAY 2000
#define RETURN_START_POSITION_DELAY 999999
#define STOP_MOTORS_DELAY 999999
#define DISTANCE_NEAR_START_POSITION 30.0
#define INACTIVE_DELAY 1000

// PID
#define MAX_PWM 150
#define LOW_PWM 80 // To use for pecise action with low speed

float Kp = 7.5f;
float Ki = 0.0f;
float Kd = 1.0f;
#define MAX_PWM 200
#define Kp 7.5
#define Ki 0.0
#define Kd 1.0

#define RIGHT_MOTOR_POWER_FACTOR 1.0
#define LEFT_MOTOR_POWER_FACTOR 1.0
#define LEFT_MOTOR_POWER_FACTOR 1.0

// Default position
#define START_X 0.0
Expand Down Expand Up @@ -318,18 +314,22 @@ void handle()
Point current_position = rolling_basis_ptr->get_current_position();
last_ticks_position = rolling_basis_ptr->get_current_ticks();

current_action->handle(
current_position,
last_ticks_position,
&rolling_basis_ptrs);

if (current_action->is_finished())
{
msg_Action_Finished fin_msg;
fin_msg.action_id = current_action->get_id();
com->send_msg((byte *)&fin_msg, sizeof(msg_Action_Finished));
if (!strat_test[action_index]->is_finished())
strat_test[action_index]->handle(
current_position,
last_ticks_position,
&rolling_basis_ptrs);
else
action_index++;
}
else
rolling_basis_ptr->keep_position(last_ticks_position.right, last_ticks_position.left);
}
else
rolling_basis_ptr->keep_position(last_ticks_position.right, last_ticks_position.left);
}
return;
else
rolling_basis_ptr->shutdown_motor();
}

/*
Expand Down

0 comments on commit 2c31963

Please sign in to comment.