Commit from tof on branch b_tof (2007-01-09 20:36 CET)
-------------------------------
adding quadramp derivate
aversive config/Configure.help
1.13.2.4
aversive config/config.in
1.42.2.4
aversive config/generate_aversive_config
1.23.2.4
+ aversive modules/devices/control_system/filters/quadramp_derivate/Makefile
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/quadramp_derivate.c
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/quadramp_derivate.h
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/test/Makefile
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/test/brushless_3phase_digital_hall_double_config.h
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/test/error_config.h
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/test/main.c
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/test/pwm_config.h
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/test/time_config.h
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/test/uart_config.h
1.1.2.1
+ aversive
modules/devices/control_system/filters/quadramp_derivate/test/utils_config.h
1.1.2.1
------------------------------
aversive/config/Configure.help (1.13.2.3 -> 1.13.2.4)
------------------------------
***************
*** 187,192 ****
--- 187,199 ----
system as a consign filter to specify a maximum acceleration and a
maximum speed.
+ CONFIG_MODULE_QUADRAMP_DERIVATE
+ This module generates the same kind of ramp than the quadramp.
+ however here we generate the derivate of this ramp, which is a speed
+ consign which can be fed to a speed PID.
+ The advantages are less computation (no square root), and a better
+ robustness against blocking.
+
CONFIG_MODULE_BIQUAD
This module is a general digital filter. It is very useful for all
filtering purposes, not only for control loops. You can implement
***************
*** 238,245 ****
saving the configuration. This script uses avrdude to program the
fuse value of your target device. NOT IMPLEMENTED YET.
CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL
This module drives a 3-phase synchronous motor (called brushless)
This is done with 3 hall sensors who give the position of the rotor.
The controller polls these sensors, and updates 3 PWM outputs to drive
! the 3 phases.
\ No newline at end of file
--- 245,260 ----
saving the configuration. This script uses avrdude to program the
fuse value of your target device. NOT IMPLEMENTED YET.
+
CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL
This module drives a 3-phase synchronous motor (called brushless)
This is done with 3 hall sensors who give the position of the rotor.
The controller polls these sensors, and updates 3 PWM outputs to drive
! the 3 phases.
!
! CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE
! This is a double implementation of the MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL
! This module drives two 3-phase synchronous motor (called brushless)
! This is done with 3 hall sensors who give the position of the rotor.
! The controller polls these sensors, and updates 3 PWM outputs to drive
! the 3 phases of each motor
\ No newline at end of file
-------------------------
aversive/config/config.in (1.42.2.3 -> 1.42.2.4)
-------------------------
***************
*** 183,193 ****
#### ADC (not yet)
! # dep_bool 'ADC' CONFIG_MODULE_ADC \
! # $CONFIG_MODULE_UTILS
! # dep_bool ' Create Default adc config' CONFIG_MODULE_ADC_CREATE_CONFIG \
! # $CONFIG_MODULE_ADC
endmenu
--- 183,193 ----
#### ADC (not yet)
! dep_bool 'ADC' CONFIG_MODULE_ADC \
! $CONFIG_MODULE_UTILS
! dep_bool ' Create Default adc config' CONFIG_MODULE_ADC_CREATE_CONFIG \
! $CONFIG_MODULE_ADC
endmenu
***************
*** 232,237 ****
--- 232,245 ----
dep_bool ' Create Default brushless_3phase_digital_hall config'
CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG \
$CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL
+ #### CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE
+ dep_bool 'two 3 phase motor with digital hall sensors'
CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE \
+ $CONFIG_MODULE_UTILS \
+ $CONFIG_MODULE_PWM
+
+ dep_bool ' Create Default brushless_3phase_digital_hall_double config'
CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG \
+ $CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE
+
endmenu # (brushless)
mainmenu_option next_comment
***************
*** 300,305 ****
--- 308,317 ----
dep_bool 'Quadramp' CONFIG_MODULE_QUADRAMP \
$CONFIG_MODULE_UTILS
+ #### QUADRAMP_DERIVATE
+ dep_bool 'Quadramp derivate' CONFIG_MODULE_QUADRAMP_DERIVATE \
+ $CONFIG_MODULE_UTILS
+
#### BIQUAD
dep_bool 'Biquad' CONFIG_MODULE_BIQUAD \
$CONFIG_MODULE_UTILS
----------------------------------------
aversive/config/generate_aversive_config (1.23.2.3 -> 1.23.2.4)
----------------------------------------
***************
*** 8,13 ****
--- 8,14 ----
# order is important !! High level modules first
#
MODULES_LIST="CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL,/devices/brushless_motors/brushless_3phase_digital_hall
+
CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE,/devices/brushless_motors/brushless_3phase_digital_hall_double
CONFIG_MODULE_UART,comm/uart
CONFIG_MODULE_PWM,hardware/pwm
CONFIG_MODULE_ADC,hardware/adc
***************
*** 22,27 ****
--- 23,29 ----
CONFIG_MODULE_PID,devices/control_system/filters/pid
CONFIG_MODULE_RAMP,devices/control_system/filters/ramp
CONFIG_MODULE_QUADRAMP,devices/control_system/filters/quadramp
+
CONFIG_MODULE_QUADRAMP_DERIVATE,devices/control_system/filters/quadramp_derivate
CONFIG_MODULE_BIQUAD,devices/control_system/filters/biquad
CONFIG_MODULE_CONTROL_SYSTEM_MANAGER,devices/control_system/control_system_manager
CONFIG_MODULE_TIME,base/time
--------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/Makefile
(1.1.2.1)
--------------------------------------------------------------------------
***************
*** 0 ****
--- 1,6 ----
+ TARGET = quadramp_derivate
+
+ # List C source files here. (C dependencies are automatically generated.)
+ SRC = quadramp_derivate.c
+
+ include $(AVERSIVE_DIR)/mk/aversive_module.mk
-------------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/quadramp_derivate.c
(1.1.2.1)
-------------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,232 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: quadramp_derivate.c,v 1.1.2.1 2007-01-09 19:36:51 tof Exp
$
+ *
+ */
+
+ #include <stdio.h>
+
+ #include <utils.h>
+ #include <quadramp_derivate.h>
+
+
+ inline void quadramp_derivate_init(struct quadramp_derivate_filter * q)
+ {
+ uint8_t flags;
+ IRQ_LOCK(flags);
+
+ q->var_2nd_ord_pos = 1;
+ q->var_2nd_ord_neg = 1;
+ q->var_1st_ord_pos = 0;
+ q->var_1st_ord_neg = 0;
+
+ q->previous_in_position = 0;
+ q->previous_out_speed = 0;
+
+ q-> gain_anticipation= 0;
+ q-> goal_window= 0;
+
+ q-> divisor = 1;
+
+ q-> pivot = 0;
+
+ IRQ_UNLOCK(flags);
+ }
+
+
+ inline void quadramp_derivate_set_gain_anticipation(struct
quadramp_derivate_filter * q, uint16_t gain_anticipation)
+ {
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ q->gain_anticipation = gain_anticipation;
+ IRQ_UNLOCK(flags);
+ }
+
+ inline void quadramp_derivate_set_goal_window(struct quadramp_derivate_filter
* q, uint32_t goal_window)
+ {
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ q->goal_window = goal_window;
+ IRQ_UNLOCK(flags);
+ }
+
+ inline void quadramp_derivate_set_2nd_order_vars(struct
quadramp_derivate_filter * q, uint32_t var_2nd_ord_pos, uint32_t
var_2nd_ord_neg)
+ {
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ q->var_2nd_ord_pos = var_2nd_ord_pos;
+ q->var_2nd_ord_neg = var_2nd_ord_neg;
+ IRQ_UNLOCK(flags);
+ }
+
+ inline void quadramp_derivate_set_1st_order_vars(struct
quadramp_derivate_filter * q, uint32_t var_1st_ord_pos, uint32_t
var_1st_ord_neg)
+ {
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ q->var_1st_ord_pos = var_1st_ord_pos;
+ q->var_1st_ord_neg = var_1st_ord_neg;
+ IRQ_UNLOCK(flags);
+ }
+
+
+ inline void quadramp_derivate_set_divisor(struct quadramp_derivate_filter *
q, uint8_t divisor)
+ {
+ uint8_t flags;
+ IRQ_LOCK(flags);
+
+ q->divisor = divisor;
+ q->divisor_counter = 1;
+
+ IRQ_UNLOCK(flags);
+ }
+
+
+ /**
+ * Process the ramp
+ *
+ * \param data should be a (struct quadramp_derivate_filter *) pointer
+ * \param in is the input of the filter
+ *
+ */
+ int32_t quadramp_derivate_do_filter(void * data, int32_t in_position)
+ {
+ struct quadramp_derivate_filter * q = data;
+
+ /** sampling divisor
+ this is a state machine who executes the algorithm only one time out of
"divisor" */
+ if( q->divisor != 1)
+ {
+ if (-- (q->divisor_counter) !=0 )
+ {
+ // if it is not time to exec the algorithm, we just test the goal_window
+ if(ABS( in_position ) < q->goal_window)
+ q->previous_out_speed =0;
+ // and return the previous consign
+ return q->previous_out_speed;
+ }
+
+ q->divisor_counter = q->divisor;
+ }
+
+
+
+ s32 position_pivot, speed, var_2nd_ord, acceleration_consign, speed_consign;
+
+
+ /** compensation of the inversion before the input
+ (inversion of the control system where error = consign - feedback)
+ */
+ in_position = -in_position;
+
+
+ // calculating the actual speed (derivate)
+ speed = in_position - q->previous_in_position;
+
+
+
+ /** limitation of this speed, due to overflows, and calculations based on
theoretical max value
+ and also the peak created when the position_consign changes */
+ if (speed >=0)
+ {
+ if(q->var_1st_ord_pos)
+ MAX(speed , (q->var_1st_ord_pos * q-> divisor) ); // divisor
reequilibrates the value.
+ }
+ else
+ {
+ if(q->var_1st_ord_neg)
+ MIN(speed , (-(q->var_1st_ord_neg* q-> divisor)) ); // divisor
reequilibrates the value.
+ }
+
+
+
+
+ /** 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)
+
+ */
+
+ // taking the concerned acc. value
+ if (speed >0) // why not position ?
+ var_2nd_ord = q->var_2nd_ord_pos;
+ else
+ var_2nd_ord = q->var_2nd_ord_neg;
+
+ // anticipation, proportionnal to speed. Gain_anticipation is a fixed point
value, with 8 bits shift
+ position_pivot = (ABS(speed) * q->gain_anticipation) >>8 ;
+
+ // if necessary, compensation of the output units, when using a sampler
divisor
+ if(q->divisor != 1)
+ {
+ var_2nd_ord *= q-> divisor;
+ position_pivot /= q-> divisor;
+ }
+
+ // pivot calculation itself
+ position_pivot += speed*speed /(2*var_2nd_ord);
+
+
+
+ // taking the right sign
+ if(speed >0)
+ position_pivot = - position_pivot;
+
+
+ // mem only for debug
+ q-> pivot = position_pivot;
+
+ /** 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
+ */
+ if(position_pivot > in_position)
+ acceleration_consign = q->var_2nd_ord_pos;
+ else
+ acceleration_consign = -q->var_2nd_ord_neg;
+
+
+
+ /** integration and limitation of the acceleration to obtain a speed
consign */
+ speed_consign = q->previous_out_speed + acceleration_consign;
+
+ if (speed_consign >0)
+ {
+ if(q->var_1st_ord_pos)
+ MAX(speed_consign , q->var_1st_ord_pos);
+ }
+ else
+ {
+ if(q->var_1st_ord_neg)
+ MIN(speed_consign , -q->var_1st_ord_neg);
+ }
+
+
+ /** creation of an end arrival window. This is done to stop the
oscillations when the goal is achieved. */
+ if(ABS( in_position ) < q->goal_window)
+ speed_consign=0;
+
+ /** refresh the memories */
+ q->previous_in_position = in_position;
+ q->previous_out_speed = speed_consign;
+
+
+ return speed_consign ;
+ }
-------------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/quadramp_derivate.h
(1.1.2.1)
-------------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,91 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: quadramp_derivate.h,v 1.1.2.1 2007-01-09 19:36:51 tof Exp
$
+ *
+ */
+
+ #ifndef _QUADRAMP_DERIVATE_H_
+ #define _QUADRAMP_DERIVATE_H_
+
+ #include <utils.h>
+
+ struct quadramp_derivate_filter
+ {
+ uint32_t var_2nd_ord_pos;
+ uint32_t var_2nd_ord_neg;
+ uint32_t var_1st_ord_pos;
+ uint32_t var_1st_ord_neg;
+
+ int32_t previous_in_position;
+ int32_t previous_out_speed;
+
+ int32_t goal_window;
+ int32_t gain_anticipation; /* fixed point value, * 1/256 */
+
+ int32_t pivot; /* debug only */
+
+ uint8_t divisor;
+ uint8_t divisor_counter;
+ };
+
+ /** Initialization of the filter
+ as this filter has always an infinite gain, it is initilized with an
integrative infinite gain limited by 1 */
+ extern inline void quadramp_derivate_init(struct quadramp_derivate_filter *
r);
+
+ /** 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
+ */
+ extern inline void quadramp_derivate_set_gain_anticipation(struct
quadramp_derivate_filter * q, uint16_t gain_anticipation);
+
+ /** 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 */
+ extern inline void quadramp_derivate_set_goal_window(struct
quadramp_derivate_filter * q, uint32_t goal_window);
+
+ /** 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. */
+ extern inline void quadramp_derivate_set_2nd_order_vars(struct
quadramp_derivate_filter * q, uint32_t var_2nd_ord_pos, uint32_t
var_2nd_ord_neg);
+ extern inline void quadramp_derivate_set_1st_order_vars(struct
quadramp_derivate_filter * q, uint32_t var_1st_ord_pos, uint32_t
var_1st_ord_neg);
+
+ /** 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 !
+ */
+ extern inline void quadramp_derivate_set_divisor(struct
quadramp_derivate_filter * q, uint8_t divisor);
+
+
+ /**
+ * Process the ramp
+ *
+ * \param data should be a (struct quadramp_filter *) pointer
+ * \param in is the input of the filter
+ *
+ * the input is a position (relative to goal)
+ * The output of the function is a speed, which is typically fed as consign
to a speed PID.
+ * Beware !! the speed unit at the output must absolutely be the same that
the derivate of the input,
+ * which means that the frequency of the quadramp_derivate_do_filter must be
the same than that of the speed PID.
+ */
+ extern int32_t quadramp_derivate_do_filter(void * data, int32_t in);
+
+ #endif
-------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/test/Makefile
(1.1.2.1)
-------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,26 ----
+ TARGET = main
+
+ # repertoire des modules
+ AVERSIVE_DIR = ../../../../../..
+
+ # List C source files here. (C dependencies are automatically generated.)
+ SRC = $(TARGET).c
+
+ # List Assembler source files here.
+ # Make them always end in a capital .S. Files ending in a lowercase .s
+ # will not be considered source files but generated files (assembler
+ # output from the compiler), and will be deleted upon "make clean"!
+ # Even though the DOS/Win* filesystem matches both .s and .S the same,
+ # it will preserve the spelling of the filenames, and gcc itself does
+ # care about how the name is spelled on its command-line.
+ ASRC =
+
+ ########################################
+
+ -include .aversive_conf
+ include $(AVERSIVE_DIR)/mk/aversive_project.mk
+
+ .PHONY:plot
+ plot: all
+ ./main > data
+ ./plot
\ No newline at end of file
--------------------------------------------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/test/brushless_3phase_digital_hall_double_config.h
(1.1.2.1)
--------------------------------------------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,136 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: brushless_3phase_digital_hall_double_config.h,v 1.1.2.1
2007-01-09 19:36:51 tof Exp $
+ *
+ */
+
+ #ifndef BRUSHLESS_TYPE
+ #define BRUSHLESS_TYPE BRUSHLESS_DIGITAL_DOUBLE
+
+
+ /** a setting of 1 gives 6 steps per electrical turn
+ the number of electrical poles per mechanical revolution depends on your
motor.
+ Please consider that a value different from 1 rises the processor load !
so try to use 1
+ */
+ #define BRUSHLESS_POSITION_PRECISION 1
+
+
+ typedef s16 brushless_speed;
+ typedef s32 brushless_position;
+
+ typedef s16 brushless_torque; // max value is that of the PWM
+
+
+ /** motor 0 connection definition */
+
+ /** inversion of sensors*/
+ //#define BRUSHLESS_0_SENSORS_INVERT
+
+ /** sensor pull ups. This is recommended for safety */
+ //#define BRUSHLESS_0_SENSORS_PULL_UP_RESISTORS
+
+ /** sensors port definitions */
+ #define BRUSHLESS_0_SENSOR_1_PORT PORTA
+ #define BRUSHLESS_0_SENSOR_1_BIT 4
+ #define BRUSHLESS_0_SENSOR_2_PORT PORTA
+ #define BRUSHLESS_0_SENSOR_2_BIT 3
+ #define BRUSHLESS_0_SENSOR_3_PORT PORTA
+ #define BRUSHLESS_0_SENSOR_3_BIT 5
+
+ /** PWM definitions
+ Please activate the PWM synch in pwm.h if you use PWMs from separate
timers
+ */
+ #define BRUSHLESS_0_PWM_SET_1(value) pwm_set_1A(value)
+ #define BRUSHLESS_0_PWM_SET_2(value) pwm_set_1B(value)
+ #define BRUSHLESS_0_PWM_SET_3(value) pwm_set_1C(value)
+
+
+
+ /** total inversion of the motor behaviour, other configuration remains the
same */
+ #define BRUSHLESS_0_INVERT
+
+
+ /** motor 1 connection definition */
+
+ /** inversion of sensors*/
+ //#define BRUSHLESS_1_SENSORS_INVERT
+
+ /** sensor pull ups. This is recommended for safety */
+ //#define BRUSHLESS_1_SENSORS_PULL_UP_RESISTORS
+
+ /** sensors port definitions */
+ #define BRUSHLESS_1_SENSOR_1_PORT PORTA
+ #define BRUSHLESS_1_SENSOR_1_BIT 1
+ #define BRUSHLESS_1_SENSOR_2_PORT PORTA
+ #define BRUSHLESS_1_SENSOR_2_BIT 0
+ #define BRUSHLESS_1_SENSOR_3_PORT PORTA
+ #define BRUSHLESS_1_SENSOR_3_BIT 2
+
+ /** PWM definitions
+ Please activate the PWM synch in pwm.h if you use PWMs from separate
timers
+ */
+ #define BRUSHLESS_1_PWM_SET_1(value) pwm_set_3A(value)
+ #define BRUSHLESS_1_PWM_SET_2(value) pwm_set_3B(value)
+ #define BRUSHLESS_1_PWM_SET_3(value) pwm_set_3C(value)
+
+
+
+ /** total inversion of the motor behaviour, other configuration remains the
same */
+ //#define BRUSHLESS_1_INVERT
+
+
+
+ /** this selects the timer overflow int to use
+ please enter the timer number of one of the 6 PWMs
+ for example,if one of your pwm is the 2A, enter "2" here
+ */
+ #define BRUSHLESS_TIMER 1
+
+ /** following parameters calculateas such :
+
+ fsample = fpwm / PWM_TO_SAMPLE_DIVISOR
+
+ fevent = fsample / SAMPLE_TO_EVENT_DIVISOR
+
+ it is recommended to have fsample at approx 10 kHz max, less if you plan
a slow motor
+ SAMPLE_TO_EVENT_DIVISOR should be at least 100, or more, so that the
event is slow enough for your need
+
+ these parameters should be carefully chosen to not overload the processor
+
+ */
+
+ /** max 255 */
+ #define BRUSHLESS_PWM_TO_SAMPLE_DIVISOR 4 // PWM9 bits, quartz 16 MHz >>
7.8 kHz
+ /** max 65535, min recommended 100 */
+ #define BRUSHLESS_SAMPLE_TO_EVENT_DIVISOR (78*1) // environ 10ms
+
+
+ /** max speed, is in general to BRUSHLESS_SAMPLE_TO_EVENT_DIVISOR
+ but can be adapted to your needs, if you need to go further down with the
speed */
+ #define BRUSHLESS_MAX_SPEED BRUSHLESS_SAMPLE_TO_EVENT_DIVISOR
+
+
+
+ //#define LOADTEST // define this to test the maximum processor load !
+
+ //#define ASMHEADER // optimized PWM division : particularly useful with 8
bit timers ! Using this can display one warning
+
+
+ #endif // BRUSHLESS_TYPE
+
+
-------------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/test/error_config.h
(1.1.2.1)
-------------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,31 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: error_config.h,v 1.1.2.1 2007-01-09 19:36:51 tof Exp $
+ *
+ */
+
+ #ifndef _ERROR_CONFIG_
+ #define _ERROR_CONFIG_
+
+ /** enable the dump of the comment */
+ #define ERROR_DUMP_TEXTLOG
+
+ /** enable the dump of filename and line number */
+ #define ERROR_DUMP_FILE_LINE
+
+ #endif
-----------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/test/main.c
(1.1.2.1)
-----------------------------------------------------------------------------
***************
*** 0 ****
--- 1,186 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: main.c,v 1.1.2.1 2007-01-09 19:36:51 tof Exp $
+ *
+ */
+
+ #include <avr/io.h>
+ #include <wait.h>
+ #include <utils.h>
+
+ #include <brushless.h>
+
+ #include <pwm.h>
+
+
+
+ #include <uart.h>
+ #include <stdio.h>
+ #include <aversive_pgmspace.h>
+
+
+
+
+ #include <pid.h>
+ #include <control_system_manager.h>
+ #include <quadramp_derivate.h>
+
+
+
+ struct cs speed;
+ struct pid_filter speed_pid;
+
+ struct cs position;
+ struct quadramp_derivate_filter position_quadr_derv;
+
+
+ // periodical 10ms execution
+ void asserv_rapide_manage(brushless data_moteur)
+ {
+
+ cs_manage(&position);
+
+ cs_manage(&speed);
+
+
+
+ // for debug graphs
+ printf_P(PSTR("C%lip%liP%liv%liV%li\n"), position.consign_value,
brushless_get_pos((void*)0), position_quadr_derv.pivot + position.consign_value
+ , brushless_get_speed((void*)0),
speed.consign_value);
+
+ //printf_P(PSTR("i%lio%liv%li\n"), speed_pid.prev_in, speed_pid.prev_out,
vitesse);
+ }
+
+
+ int main(void)
+ {
+
+
+
+ // init motors and PWMs
+ brushless_init();
+
+
+ // enable power bridges
+ sbi(DDRG, 1);
+ sbi(PORTG, 1);
+
+
+ // init uart
+ uart_init();
+ fdevopen((void *)uart0_send,NULL,0);
+
+
+
+ //printf_P(PSTR("\nbonjour\n"));
+
+
+
+ /** replaces the scheduler. This is directly derived from the interrupt
which runs the brushless motors, for convenience */
+ brushless_0_register_periodic_event(asserv_rapide_manage); // 10 ms
+
+
+
+ /** speed PID stuff */
+
+ pid_init(&speed_pid);
+
+
+ pid_set_gains(&speed_pid, 180, 70, 40); // sur alim
+
+ pid_set_maximums(&speed_pid, 0, 80, PWM_MAX*4/5);
+ pid_set_out_shift(&speed_pid, 0);
+
+ cs_init(&speed);
+ cs_set_correct_filter(&speed, pid_do_filter, &speed_pid);
+ cs_set_process_in(&speed, brushless_set_torque, (void *)0 );
+ cs_set_process_out(&speed,brushless_get_speed , (void *)0 );
+ cs_set_consign(&speed, 0);
+
+
+
+ /** ramp generator parameters */
+
+ quadramp_derivate_init(&position_quadr_derv);
+
+ quadramp_derivate_set_gain_anticipation(&position_quadr_derv, 256 *3);
+
+ quadramp_derivate_set_goal_window(&position_quadr_derv, 5);
+
+ quadramp_derivate_set_2nd_order_vars(&position_quadr_derv, 1 , 1);
+ quadramp_derivate_set_1st_order_vars(&position_quadr_derv, 12, 12);
+
+ quadramp_derivate_set_divisor(&position_quadr_derv, 2);
+
+
+ cs_init(&position);
+ cs_set_correct_filter(&position, quadramp_derivate_do_filter,
&position_quadr_derv);
+ cs_set_process_in(&position, cs_set_consign, (void *)&speed );
+ cs_set_process_out(&position,brushless_get_pos , (void *)0 );
+ cs_set_consign(&position, 0);
+
+
+ /** begin */
+
+ brushless_set_speed((void *)0 , BRUSHLESS_MAX_SPEED); // init speed
+
+ sei();
+
+
+
+
+ // some simple trajectories (enable one )
+
+ while(1)
+ {
+ wait_ms(500);
+ cs_set_consign(&position, 400);
+ wait_ms(500);
+ cs_set_consign(&position, 0);
+ }
+
+
+ /*
+ while(1)
+ {
+ wait_ms(2500);
+ cs_set_consign(&position, 2000);
+ wait_ms(2500);
+ cs_set_consign(&position, 0);
+ }
+
+
+
+ // test of speed pid only, deactivate the position.
+ while(1)
+ {
+ wait_ms(300);
+ cs_set_consign(&speed, 10);
+ wait_ms(300);
+ cs_set_consign(&speed, -10);
+ } */
+
+
+
+
+ while(1);
+
+
+ return 0;
+ }
+
-----------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/test/pwm_config.h
(1.1.2.1)
-----------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,147 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: pwm_config.h,v 1.1.2.1 2007-01-09 19:36:51 tof Exp $
+ *
+ */
+
+ /* Droids-corp, Eirbot, Microb Technology 2005 - Zer0
+ * Config for PWM
+ */
+ /** \file pwm_config.h
+ \brief Module to operate all PWM outputs
+
+ \test not tested
+
+ */
+
+
+ #ifndef _PWM_CONFIG_
+ #define _PWM_CONFIG_
+
+ /* Which PWM are enabled ? */
+ //#define PWM0_ENABLED
+ #define PWM1A_ENABLED
+ #define PWM1B_ENABLED
+ #define PWM1C_ENABLED
+ //#define PWM2_ENABLED
+ #define PWM3A_ENABLED
+ #define PWM3B_ENABLED
+ #define PWM3C_ENABLED
+
+
+ /** max value for PWM entry, default 12 bits > 4095 */
+ #define PWM_SIGNIFICANT_BITS 12
+
+ // timer configs
+
+ //#define TIMER0_MODE TIMER_8_MODE_PWM
+ //#define TIMER0_PRESCALE TIMER_8_PRESCALE_1
+
+ // 31 kHz PWM
+ #define TIMER1_MODE TIMER_16_MODE_PWM_9
+ #define TIMER1_PRESCALE TIMER_16_PRESCALE_1
+
+ //#define TIMER2_MODE TIMER_8_MODE_PWM
+ //#define TIMER2_PRESCALE TIMER_8_PRESCALE_1
+
+ // 31 kHz PWM
+ #define TIMER3_MODE TIMER_16_MODE_PWM_9
+ #define TIMER3_PRESCALE TIMER_16_PRESCALE_1
+
+
+
+
+ /** config for pwm and signs
+
+ The pwm mode is defined as follows :
+ you can add flags like the ones who follow :
+
+ PWM_NORMAL : normal pwm, just to put a value if nothing else is
needed
+ PWM_REVERSE : invert pwm output, not sign
+
+ PWM_SIGNED : activate the sign output on a port (see config)
+ PWM_SIGN_INVERTED : invert sign output
+ PWM_SPECIAL_SIGN_MODE : if defined, the pwm is always near 0 for low values,
+ else negative low values are near 100%
+
+
+ the values of PWMxx_SIGN_PORT and PWMxx_SIGN_BIT are simply ignored if the
PWM is not signed, but must be defined
+
+
+ if you need for example a PWM1A with special sign mode you configure like
this :
+
+ #define PWM1A_MODE (PWM_SIGNED | PWM_SPECIAL_SIGN_MODE)
+ #define PWM1A_SIGN_PORT PORTB
+ #define PWM1A_SIGN_BIT 2
+
+ */
+
+
+
+ // PWM definitions
+ #define PWM1A_MODE (PWM_NORMAL)
+ #define PWM1A_SIGN_PORT PORTB // ignored
+ #define PWM1A_SIGN_BIT 2 // ignored
+
+ #define PWM1B_MODE (PWM_NORMAL)
+ #define PWM1B_SIGN_PORT PORTB // ignored
+ #define PWM1B_SIGN_BIT 2 // ignored
+
+ #define PWM1C_MODE (PWM_NORMAL)
+ #define PWM1C_SIGN_PORT PORTB // ignored
+ #define PWM1C_SIGN_BIT 2 // ignored
+
+ #define PWM3A_MODE (PWM_NORMAL)
+ #define PWM3A_SIGN_PORT PORTB // ignored
+ #define PWM3A_SIGN_BIT 2 // ignored
+
+ #define PWM3B_MODE (PWM_NORMAL)
+ #define PWM3B_SIGN_PORT PORTB // ignored
+ #define PWM3B_SIGN_BIT 2 // ignored
+
+ #define PWM3C_MODE (PWM_NORMAL)
+ #define PWM3C_SIGN_PORT PORTB // ignored
+ #define PWM3C_SIGN_BIT 2 // ignored
+
+
+
+ /**
+ PWM synchronization.
+
+ this makes the PWMs synchronized.
+ just activate the timers you want to synchronize
+
+ to synch PWMs you need to enshure that the timers have same prescales. This
is verified.
+ you need also to enshure that the PWM mode is the same, this is NOT verified
!!
+ especially, for syncing 8 and 16 bit timers, the PWM mode should be 8 bit.
+
+
+ side effect : on some controllers prescalers are shared, so unwanted
prescalers can be reset.
+
+ This feature is not 100% shure for the moment, but has been tested on M32 and
M128
+ */
+
+ //#define TIMER0_SYNCH
+ #define TIMER1_SYNCH
+ //#define TIMER2_SYNCH
+ #define TIMER3_SYNCH
+
+
+
+ #endif
+
------------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/test/time_config.h
(1.1.2.1)
------------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,23 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: time_config.h,v 1.1.2.1 2007-01-09 19:36:51 tof Exp $
+ *
+ */
+
+ /** precision of the time processor, in us */
+ #define TIME_PRECISION 10000l
------------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/test/uart_config.h
(1.1.2.1)
------------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,72 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: uart_config.h,v 1.1.2.1 2007-01-09 19:36:51 tof Exp $
+ *
+ */
+
+ /* Droids-corp 2004 - Zer0
+ * config for uart module
+ */
+
+ #ifndef UART_CONFIG_H
+ #define UART_CONFIG_H
+
+ /*
+ * UART0 definitions
+ */
+
+ /* compile uart0 fonctions, undefine it to pass compilation */
+ #define UART0_COMPILE
+
+ /* enable uart0 if == 1, disable if == 0 */
+ #define UART0_ENABLED 1
+
+ /* enable uart0 interrupts if == 1, disable if == 0 */
+ #define UART0_INTERRUPT_ENABLED 1
+
+ #define UART0_BAUDRATE 38400
+
+ /*
+ * if you enable this, the maximum baudrate you can reach is
+ * higher, but the precision is lower.
+ */
+ #define UART0_USE_DOUBLE_SPEED 0
+ //#define UART0_USE_DOUBLE_SPEED 1
+
+ #define UART0_RX_FIFO_SIZE 4
+ #define UART0_TX_FIFO_SIZE 4
+ //#define UART0_NBITS 5
+ //#define UART0_NBITS 6
+ //#define UART0_NBITS 7
+ #define UART0_NBITS 8
+ //#define UART0_NBITS 9
+
+ #define UART0_PARITY UART_PARTITY_NONE
+ //#define UART0_PARITY UART_PARTITY_ODD
+ //#define UART0_PARITY UART_PARTITY_EVEN
+
+ #define UART0_STOP_BIT UART_STOP_BITS_1
+ //#define UART0_STOP_BIT UART_STOP_BITS_2
+
+
+
+
+ /* .... same for uart 1, 2, 3 ... */
+
+ #endif
+
-------------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/test/utils_config.h
(1.1.2.1)
-------------------------------------------------------------------------------------
***************
*** 0 ****
--- 1,32 ----
+ /*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: utils_config.h,v 1.1.2.1 2007-01-09 19:36:51 tof Exp $
+ *
+ */
+
+ #ifndef _UTILS_CONFIG_
+ #define _UTILS_CONFIG_ 1.0 // version
+
+ /** there are two variants of the IRQ_LOCK() macros
+ the "microb" one is stack space optimized
+ the normal one is code space and speed optimized (and reliable ;)
+ this is not very important for most applications, so don't care */
+ //#define USE_MICROB_IRQ
+
+
+ #endif //_UTILS_CONFIG_
Commit from tof on branch b_tof (2007-01-10 00:21 CET)
-------------------------------
correction little bug, but with big effect
aversive
modules/devices/control_system/filters/quadramp_derivate/quadramp_derivate.c
1.1.2.2
-------------------------------------------------------------------------------------
aversive/modules/devices/control_system/filters/quadramp_derivate/quadramp_derivate.c
(1.1.2.1 -> 1.1.2.2)
-------------------------------------------------------------------------------------
***************
*** 15,21 ****
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
! * Revision : $Id: quadramp_derivate.c,v 1.1.2.1 2007-01-09 19:36:51 tof Exp
$
*
*/
--- 15,21 ----
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
! * Revision : $Id: quadramp_derivate.c,v 1.1.2.2 2007-01-09 23:21:30 tof Exp
$
*
*/
***************
*** 162,168 ****
*/
// taking the concerned acc. value
! if (speed >0) // why not position ?
var_2nd_ord = q->var_2nd_ord_pos;
else
var_2nd_ord = q->var_2nd_ord_neg;
--- 162,168 ----
*/
// taking the concerned acc. value
! if (speed >=0) // why not position ?
var_2nd_ord = q->var_2nd_ord_pos;
else
var_2nd_ord = q->var_2nd_ord_neg;
***************
*** 183,189 ****
// taking the right sign
! if(speed >0)
position_pivot = - position_pivot;
--- 183,189 ----
// taking the right sign
! if(speed >=0)
position_pivot = - position_pivot;
***************
*** 197,203 ****
In fact this unstability is erased by the fact that the acc value is
nearly always limited
*/
! if(position_pivot > in_position)
acceleration_consign = q->var_2nd_ord_pos;
else
acceleration_consign = -q->var_2nd_ord_neg;
--- 197,203 ----
In fact this unstability is erased by the fact that the acc value is
nearly always limited
*/
! if(position_pivot >= in_position)
acceleration_consign = q->var_2nd_ord_pos;
else
acceleration_consign = -q->var_2nd_ord_neg;
***************
*** 207,213 ****
/** integration and limitation of the acceleration to obtain a speed
consign */
speed_consign = q->previous_out_speed + acceleration_consign;
! if (speed_consign >0)
{
if(q->var_1st_ord_pos)
MAX(speed_consign , q->var_1st_ord_pos);
--- 207,213 ----
/** integration and limitation of the acceleration to obtain a speed
consign */
speed_consign = q->previous_out_speed + acceleration_consign;
! if (speed_consign >=0)
{
if(q->var_1st_ord_pos)
MAX(speed_consign , q->var_1st_ord_pos);
_______________________________________________
Avr-list mailing list
Avr-list@droids-corp.org
CVSWEB : http://cvsweb.droids-corp.org/cgi-bin/viewcvs.cgi/aversive
WIKI : http://wiki.droids-corp.org/index.php/Aversive
DOXYGEN : http://zer0.droids-corp.org/doxygen_aversive/html/
BUGZILLA : http://bugzilla.droids-corp.org
COMMIT LOGS : http://zer0.droids-corp.org/aversive_commitlog
