#include <stdio.h>#include <aversive.h>#include <quadramp_derivate.h>Go to the source code of this file.
Functions | |
| void | quadramp_derivate_init (struct quadramp_derivate_filter *q) |
| void | quadramp_derivate_set_gain_anticipation (struct quadramp_derivate_filter *q, uint16_t gain_anticipation) |
| void | quadramp_derivate_set_goal_window (struct quadramp_derivate_filter *q, uint32_t goal_window) |
| void | quadramp_derivate_set_2nd_order_vars (struct quadramp_derivate_filter *q, uint32_t var_2nd_ord_pos, uint32_t var_2nd_ord_neg) |
| void | quadramp_derivate_set_1st_order_vars (struct quadramp_derivate_filter *q, uint32_t var_1st_ord_pos, uint32_t var_1st_ord_neg) |
| void | quadramp_derivate_set_divisor (struct quadramp_derivate_filter *q, uint8_t divisor) |
| int32_t | quadramp_derivate_do_filter (void *data, int32_t in_position) |
| int32_t quadramp_derivate_do_filter | ( | void * | data, | |
| int32_t | in_position | |||
| ) |
Process the ramp
| data | should be a (struct quadramp_derivate_filter *) pointer | |
| in | is the input of the filter |
sampling divisor this is a state machine who executes the algorithm only one time out of "divisor"
compensation of the inversion before the input (inversion of the control system where error = consign - feedback)
limitation of this speed, due to overflows, and calculations based on theoretical max value and also the peak created when the position_consign changes
calculation of the pivot position. when this position is atteined, it is just the right time to begin to deccelerate. The length to this position is given by a linear decceleration to 0 : x = speedē/ (2 * acceleration)
this is the heart of the trajectory generation. Pretty simple but indeed unstable, because of this corresponds to an infinite gain, in the following equation : acceleration_consign = ( position_pivot - in_position ) * gain
In fact this unstability is erased by the fact that the acc value is nearly always limited
integration and limitation of the acceleration to obtain a speed consign
creation of an end arrival window. This is done to stop the oscillations when the goal is achieved.
refresh the memories
Definition at line 106 of file quadramp_derivate.c.
References ABS, quadramp_derivate_filter::divisor, quadramp_derivate_filter::divisor_counter, quadramp_derivate_filter::gain_anticipation, quadramp_derivate_filter::goal_window, MAX, MIN, quadramp_derivate_filter::pivot, quadramp_derivate_filter::previous_in_position, quadramp_derivate_filter::previous_out_speed, quadramp_derivate_filter::var_1st_ord_neg, quadramp_derivate_filter::var_1st_ord_pos, quadramp_derivate_filter::var_2nd_ord_neg, and quadramp_derivate_filter::var_2nd_ord_pos.
| void quadramp_derivate_init | ( | struct quadramp_derivate_filter * | r | ) | [inline] |
Initialization of the filter as this filter has always an infinite gain, it is initilized with an integrative infinite gain limited by 1
Definition at line 28 of file quadramp_derivate.c.
References IRQ_LOCK, IRQ_UNLOCK, quadramp_derivate_filter::previous_in_position, quadramp_derivate_filter::previous_out_speed, quadramp_derivate_filter::var_1st_ord_neg, quadramp_derivate_filter::var_1st_ord_pos, quadramp_derivate_filter::var_2nd_ord_neg, and quadramp_derivate_filter::var_2nd_ord_pos.
| void quadramp_derivate_set_1st_order_vars | ( | struct quadramp_derivate_filter * | q, | |
| uint32_t | var_1st_ord_pos, | |||
| uint32_t | var_1st_ord_neg | |||
| ) | [inline] |
Definition at line 77 of file quadramp_derivate.c.
References IRQ_LOCK, IRQ_UNLOCK, quadramp_derivate_filter::var_1st_ord_neg, and quadramp_derivate_filter::var_1st_ord_pos.
| void quadramp_derivate_set_2nd_order_vars | ( | struct quadramp_derivate_filter * | q, | |
| uint32_t | var_2nd_ord_pos, | |||
| uint32_t | var_2nd_ord_neg | |||
| ) | [inline] |
as in the quadramp, we can set here the maximum speed (1st order) and maximum acceleration (2nd order) and this in both directions, positive, and negative.
Definition at line 68 of file quadramp_derivate.c.
References IRQ_LOCK, IRQ_UNLOCK, quadramp_derivate_filter::var_2nd_ord_neg, and quadramp_derivate_filter::var_2nd_ord_pos.
| void quadramp_derivate_set_divisor | ( | struct quadramp_derivate_filter * | q, | |
| uint8_t | divisor | |||
| ) | [inline] |
this sets a divisor. (executing only 1 time of n) this permits to make a bigger resolution on the speed and acceleration consign.
default is 1. When using n>1, the new acceleration (2nd order) unit is divided by n (increasing precision) The speed remains at the same unit.
The drawback is that the speed will have the forma of a stair, so do not abuse of it !
Definition at line 87 of file quadramp_derivate.c.
References quadramp_derivate_filter::divisor, quadramp_derivate_filter::divisor_counter, IRQ_LOCK, and IRQ_UNLOCK.
| void quadramp_derivate_set_gain_anticipation | ( | struct quadramp_derivate_filter * | q, | |
| uint16_t | gain_anticipation | |||
| ) | [inline] |
set the anticipation value. This gain is a fixed point value that will be divided by 256. set this gain to have enough anticipation, so the goal is not atteined with too much speed. too much, and the goal will be overlooked, and the system will oscillate. Too less, and the goal will be atteined with speed, and the goal window will eventually cutoff brutally
Definition at line 52 of file quadramp_derivate.c.
References quadramp_derivate_filter::gain_anticipation, IRQ_LOCK, and IRQ_UNLOCK.
| void quadramp_derivate_set_goal_window | ( | struct quadramp_derivate_filter * | q, | |
| uint32_t | goal_window | |||
| ) | [inline] |
goal window is a shutdown of the integration when the goal is atteined. this aims to get rid of the very little oscillations when immobile
Definition at line 60 of file quadramp_derivate.c.
References quadramp_derivate_filter::goal_window, IRQ_LOCK, and IRQ_UNLOCK.
1.5.6