Commit from enseirb on branch b_eirbot (2008-03-23 14:33 CET) ======================================
unioc_asserv/ + aversive projects/coupe2008/unioc_asserv/Makefile 1.1.2.1 + aversive projects/coupe2008/unioc_asserv/autoconf.h 1.1.2.1 + aversive projects/coupe2008/unioc_asserv/encoders_unioc_config.h 1.1.2.1 + aversive projects/coupe2008/unioc_asserv/main.c 1.1.2.1 + aversive projects/coupe2008/unioc_asserv/pid_config.h 1.1.2.1 + aversive projects/coupe2008/unioc_asserv/pwm_config.h 1.1.2.1 + aversive projects/coupe2008/unioc_asserv/scheduler_config.h 1.1.2.1 + aversive projects/coupe2008/unioc_asserv/uart_config.h 1.1.2.1 ================================================= aversive/projects/coupe2008/unioc_asserv/Makefile (1.1.2.1) ================================================= @@ -0,0 +1,22 @@ +TARGET = main + +# repertoire des modules +AVERSIVE_DIR = ../../.. +# VALUE, absolute or relative path : example ../.. # + +# 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 =================================================== aversive/projects/coupe2008/unioc_asserv/autoconf.h (1.1.2.1) =================================================== @@ -0,0 +1,240 @@ +/* + * Automatically generated by make menuconfig: don't edit + */ +#define AUTOCONF_INCLUDED + +/* + * Hardware + */ +#undef CONFIG_MCU_AT90S2313 +#undef CONFIG_MCU_AT90S2323 +#undef CONFIG_MCU_AT90S3333 +#undef CONFIG_MCU_AT90S2343 +#undef CONFIG_MCU_ATTINY22 +#undef CONFIG_MCU_ATTINY26 +#undef CONFIG_MCU_AT90S4414 +#undef CONFIG_MCU_AT90S4433 +#undef CONFIG_MCU_AT90S4434 +#undef CONFIG_MCU_AT90S8515 +#undef CONFIG_MCU_AT90S8534 +#undef CONFIG_MCU_AT90S8535 +#undef CONFIG_MCU_AT86RF401 +#undef CONFIG_MCU_ATMEGA103 +#undef CONFIG_MCU_ATMEGA603 +#undef CONFIG_MCU_AT43USB320 +#undef CONFIG_MCU_AT43USB355 +#undef CONFIG_MCU_AT76C711 +#undef CONFIG_MCU_ATMEGA8 +#undef CONFIG_MCU_ATMEGA48 +#undef CONFIG_MCU_ATMEGA88 +#undef CONFIG_MCU_ATMEGA8515 +#undef CONFIG_MCU_ATMEGA8535 +#undef CONFIG_MCU_ATTINY13 +#undef CONFIG_MCU_ATTINY2313 +#undef CONFIG_MCU_ATMEGA16 +#undef CONFIG_MCU_ATMEGA161 +#undef CONFIG_MCU_ATMEGA162 +#undef CONFIG_MCU_ATMEGA163 +#undef CONFIG_MCU_ATMEGA165 +#undef CONFIG_MCU_ATMEGA168 +#undef CONFIG_MCU_ATMEGA169 +#undef CONFIG_MCU_ATMEGA32 +#undef CONFIG_MCU_ATMEGA323 +#undef CONFIG_MCU_ATMEGA325 +#undef CONFIG_MCU_ATMEGA3250 +#undef CONFIG_MCU_ATMEGA64 +#undef CONFIG_MCU_ATMEGA645 +#undef CONFIG_MCU_ATMEGA6450 +#define CONFIG_MCU_ATMEGA128 1 +#undef CONFIG_MCU_AT90CAN128 +#undef CONFIG_MCU_AT94K +#undef CONFIG_MCU_AT90S1200 +#define CONFIG_QUARTZ (8000000) + +/* + * Generation options + */ +#define CONFIG_OPTM_0 1 +#undef CONFIG_OPTM_1 +#undef CONFIG_OPTM_2 +#undef CONFIG_OPTM_3 +#undef CONFIG_OPTM_S +#define CONFIG_MATH_LIB 1 +#define CONFIG_FDEVOPEN_COMPAT 1 +#undef CONFIG_MINIMAL_PRINTF +#undef CONFIG_STANDARD_PRINTF +#define CONFIG_ADVANCED_PRINTF 1 +#define CONFIG_FORMAT_IHEX 1 +#undef CONFIG_FORMAT_SREC +#undef CONFIG_FORMAT_BINARY + +/* + * Base modules + */ +#define CONFIG_MODULE_CIRBUF 1 +#undef CONFIG_MODULE_CIRBUF_LARGE +#define CONFIG_MODULE_FIXED_POINT 1 +#define CONFIG_MODULE_VECT2 1 +#define CONFIG_MODULE_SCHEDULER 1 +#define CONFIG_MODULE_SCHEDULER_CREATE_CONFIG 1 +#undef CONFIG_MODULE_SCHEDULER_USE_TIMERS +#define CONFIG_MODULE_SCHEDULER_TIMER0 1 +#undef CONFIG_MODULE_SCHEDULER_MANUAL +#undef CONFIG_MODULE_TIME +#undef CONFIG_MODULE_TIME_CREATE_CONFIG + +/* + * Communication modules + */ +#define CONFIG_MODULE_UART 1 +#define CONFIG_MODULE_UART_CREATE_CONFIG 1 +#undef CONFIG_MODULE_I2C +#undef CONFIG_MODULE_I2C_MASTER +#undef CONFIG_MODULE_I2C_MULTIMASTER +#undef CONFIG_MODULE_I2C_CREATE_CONFIG +#undef CONFIG_MODULE_I2C_EIRBOT +#undef CONFIG_MODULE_I2C_EIRBOT_CREATE_CONFIG +#undef CONFIG_MODULE_MF2_CLIENT +#undef CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER +#undef CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG +#undef CONFIG_MODULE_MF2_SERVER +#undef CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG + +/* + * Hardware modules + */ +#undef CONFIG_MODULE_TIMER +#undef CONFIG_MODULE_TIMER_CREATE_CONFIG +#undef CONFIG_MODULE_TIMER_DYNAMIC +#define CONFIG_MODULE_PWM 1 +#define CONFIG_MODULE_PWM_CREATE_CONFIG 1 +#undef CONFIG_MODULE_ADC +#undef CONFIG_MODULE_ADC_CREATE_CONFIG + +/* + * IHM modules + */ +#undef CONFIG_MODULE_MENU +#undef CONFIG_MODULE_VT100 +#undef CONFIG_MODULE_RDLINE +#undef CONFIG_MODULE_RDLINE_CREATE_CONFIG +#undef CONFIG_MODULE_RDLINE_KILL_BUF +#undef CONFIG_MODULE_RDLINE_HISTORY +#undef CONFIG_MODULE_PARSE + +/* + * external devices modules + */ + +/* + * Crazy Cam Ass Ultra + */ +#undef CONFIG_MODULE_CCAU +#undef CONFIG_MODULE_CCAU_CREATE_CONFIG +#undef CONFIG_MODULE_MOUAIS +#undef CONFIG_MODULE_GP2D12 +#undef CONFIG_MODULE_LCD +#undef CONFIG_MODULE_LCD_CREATE_CONFIG +#undef CONFIG_MODULE_MULTISERVO +#undef CONFIG_MODULE_MULTISERVO_CREATE_CONFIG +#undef CONFIG_MODULE_AX12 +#undef CONFIG_MODULE_AX12_CREATE_CONFIG + +/* + * Brushless motor drivers (you should enable pwm modules to see all) + */ +#undef CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL +#undef CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG +#undef CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE +#undef CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG + +/* + * encoders + */ +#undef CONFIG_MODULE_ENCODERS_MICROB +#undef CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG +#undef CONFIG_MODULE_ENCODERS_EIRBOT +#undef CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG +#define CONFIG_MODULE_ENCODERS_UNIOC 1 +#define CONFIG_MODULE_ENCODERS_UNIOC_CREATE_CONFIG 1 + +/* + * Robot specific modules + */ +#define CONFIG_MODULE_ROBOT_SYSTEM 1 +#define CONFIG_MODULE_POSITION_MANAGER 1 +#define CONFIG_MODULE_TRAJECTORY_MANAGER 1 +#undef CONFIG_MODULE_BLOCKING_DETECTION_MANAGER +#undef CONFIG_MODULE_OBSTACLE_AVOIDANCE + +/* + * Control system modules + */ +#define CONFIG_MODULE_CONTROL_SYSTEM_MANAGER 1 +#define CONFIG_MODULE_PID 1 +#define CONFIG_MODULE_PID_CREATE_CONFIG 1 +#define CONFIG_MODULE_RAMP 1 +#define CONFIG_MODULE_QUADRAMP 1 +#undef CONFIG_MODULE_QUADRAMP_DERIVATE +#undef CONFIG_MODULE_BIQUAD +#undef CONFIG_MODULE_QUADRAMP_DERIVATE +#undef CONFIG_MODULE_BIQUAD + +/* + * Crypto modules + */ +#undef CONFIG_MODULE_AES +#undef CONFIG_MODULE_AES_CTR +#undef CONFIG_MODULE_MD5 +#undef CONFIG_MODULE_MD5_HMAC +#undef CONFIG_MODULE_RC4 + +/* + * Encodings modules + */ +#undef CONFIG_MODULE_BASE64 +#undef CONFIG_MODULE_HAMMING + +/* + * Debug modules + */ +#undef CONFIG_MODULE_DIAGNOSTIC +#undef CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG +#undef CONFIG_MODULE_ERROR +#undef CONFIG_MODULE_ERROR_CREATE_CONFIG + +/* + * Programmer options + */ +#undef CONFIG_AVRDUDE +#define CONFIG_AVARICE 1 + +/* + * Avrdude + */ +#undef CONFIG_AVRDUDE_PROG_FUTURELEC +#undef CONFIG_AVRDUDE_PROG_ABCMINI +#undef CONFIG_AVRDUDE_PROG_PICOWEB +#undef CONFIG_AVRDUDE_PROG_SP12 +#undef CONFIG_AVRDUDE_PROG_ALF +#undef CONFIG_AVRDUDE_PROG_BASCOM +#undef CONFIG_AVRDUDE_PROG_DT006 +#undef CONFIG_AVRDUDE_PROG_PONY_STK200 +#define CONFIG_AVRDUDE_PROG_STK200 1 +#undef CONFIG_AVRDUDE_PROG_PAVR +#undef CONFIG_AVRDUDE_PROG_BUTTERFLY +#undef CONFIG_AVRDUDE_PROG_AVR910 +#undef CONFIG_AVRDUDE_PROG_STK500 +#undef CONFIG_AVRDUDE_PROG_AVRISP +#undef CONFIG_AVRDUDE_PROG_BSD +#undef CONFIG_AVRDUDE_PROG_DAPA +#undef CONFIG_AVRDUDE_PROG_JTAG1 +#define CONFIG_AVRDUDE_PORT "/dev/parport0" + +/* + * Avarice + */ +#define CONFIG_AVARICE_PORT "/dev/ttyS0" +#define CONFIG_AVARICE_DEBUG_PORT (1234) +#define CONFIG_AVARICE_PROG_MKI 1 +#undef CONFIG_AVARICE_PROG_MKII ================================================================ aversive/projects/coupe2008/unioc_asserv/encoders_unioc_config.h (1.1.2.1) ================================================================ @@ -0,0 +1,32 @@ +/** \file encoders_eirbot_unioc_config.h + * \brief configuration du module codeur UNIOC + */ + +#ifndef _ENCODERS_EIRBOT_UNIOC_CONFIG_ +#define _ENCODERS_EIRBOT_UNIOC_CONFIG_ + +/** adresse la plus basse à partir de laquelle se trouvent toutes les autres données codeur. + * Ceci implique des adresses consécutives pour les modules codeurs + * #define BASE_CODEUR 0x1200 +*/ + +#define ENCODERS_BASE_ADDRESS 0x1300 + +/** nombre de codeurs implantés dans le FPGA et utilisés par le µc +*/ +#define ENCODERS_NUMBER 4 + +/** Registre ENCODERS_CONFIG : + * correspondance numero de bit, codeur + */ +#define L_ENGINE_ENCODER 0 +#define R_ENGINE_ENCODER 1 +#define L_SEPARATED_ENCODER 2 +#define R_SEPARATED_ENCODER 3 + + +#define ADDRESS_ENCODERS_CONFIG 0x1400 + +#endif + + =============================================== aversive/projects/coupe2008/unioc_asserv/main.c (1.1.2.1) =============================================== @@ -0,0 +1,522 @@ +/* + * 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 2008-03-23 13:33:48 enseirb Exp $ + * + */ + +#include <aversive.h> +#include <aversive/wait.h> +#include <stdio.h> +#include <uart.h> +#include <scheduler.h> + +#include <pwm.h> +#include <vect2.h> +#include <robot_system.h> +#include <pid.h> +#include <quadramp.h> +#include <control_system_manager.h> +#include <encoders_unioc.h> +#include <position_manager.h> +#include <trajectory_manager.h> + + +/**************************************/ + + +#define ENCODERS_DELTA 1 +#define LOW_ASSERV_DELTA 40 +#define POS_MANAGER_DELTA 400 + +/**************************************/ + +#define TE 40*SCHEDULER_UNIT + +/**************************************/ + +#define RDS_D (PINF&0x01) +#define RDS_G (PINF&0x02) +#define RDS_C (PINF&0x04) + +/**************************************/ + +struct cs csm_distance; +struct cs csm_angle; + +struct pid_filter pid_distance; +struct pid_filter pid_angle; + +struct quadramp_filter qramp_distance; +struct quadramp_filter qramp_angle; + +// + +struct cs csm_motD; +struct cs csm_motG; + +struct pid_filter pid_motD; +struct pid_filter pid_motG; + +struct pid_filter pid_feedback_motD; +struct pid_filter pid_feedback_motG; + +// + +struct trajectory my_traj; + +struct robot_position position; + +struct robot_system robot; + +int32_t pwmG,pwmD; + +int32_t inter_speed = 0; + +//****************************************** + +void update_low_level(void * p); + +void send_motors_consigns(int32_t d, int32_t theta); + +void my_pos_manage(void * p); + +void left_pwm( void* p, int32_t val ); +void right_pwm( void* p, int32_t val ); + +int32_t left_mot_encoder( void* p ); +int32_t right_mot_encoder( void* p ); + +int32_t left_ext_encoder( void* p ); +int32_t right_ext_encoder( void* p ); + +void show( int _a); +void debug( void* p ); + +int32_t encMD, encMG, encSD, encSG; + +int32_t debug1,debug2; + +int32_t pwmMaxD = 4095; +int32_t pwmMaxG = 4095; + +int32_t o_pwmMaxD = 0, o_pwmMaxG = 0; + +uint8_t asserv_angle_active = 1; + +int32_t l_LM = 0; +int32_t l_RM = 0; +int32_t l_LE = 0; +int32_t l_RE = 0; + +int32_t v_LM,v_RM,v_LE,v_RE; + +//*************************** +// MAIN ! GOooo ! +//*************************** + +int main(void) +{ + //************************************************* + //* INIT **************************************** + //************************************************* + + pwmG = pwmD = 0; + + // enable interrupts + sei(); + + uart_init(); + + fdevopen((int(*)(char))uart1_send,(int(*)(void))uart1_recv,0); + + wait_ms(100); + + printf("\n"); + printf("(\\/)\n"); + printf(" O.o\n"); + printf("(U U)\n"); + printf("§§§§§\n"); + + wait_ms(100); + + encoders_init((0 <<L_ENGINE_ENCODER) + | (0 <<R_ENGINE_ENCODER) + | (1 <<L_SEPARATED_ENCODER) + | (1 <<R_SEPARATED_ENCODER)); + + wait_ms(10); + + // Init. les pwms + pwm_init(); + + printf("Scheduler init\n"); + + // ************************************************* + // * TRAJECTORY MANAGER ************************** + // ************************************************* + + // * RTFM le .h (oui... c'est facile... mais bon) + + trajectory_init( &my_traj ); + trajectory_set_cs( &my_traj, &csm_angle, &csm_distance ); + trajectory_set_robot_params( &my_traj, &robot, &position ); + + // Fenetre de fin de trajectoire (distance,angle) et start en deg. + trajectory_set_windows( &my_traj, 2, 5, 45); + + // Vitesse lineaire / angulaire max + trajectory_set_speed( &my_traj, 100.0, 100.0); + + printf("Trajectory\n"); + + // ************************************************* + // * POSITION MANAGER **************************** + // ************************************************* + + position_init(&position); + + position_set_related_robot_system(&position,&robot); + + // X Y Theta avec theta en degrees + position_set(&position,0,0,0); + + position_set_physical_params(&position, + 28.7, // Distance entre les roues + // historique : Reglage qui poutre : 30.76 30.759 + 439.4); // Imp / cm + // historique : 436.0 430.0 445.0 (441) + + + printf("Position manager\n"); + + // ************************************************* + // * ROBOT SYSTEM ******************************** + // ************************************************* + + rs_init( &robot ); + + rs_set_left_pwm( &robot, &left_pwm, NULL); + rs_set_right_pwm( &robot, &right_pwm, NULL); + + rs_set_left_mot_encoder( &robot, &left_mot_encoder, NULL, 1.0); + rs_set_right_mot_encoder( &robot, &right_mot_encoder, NULL, 1.0); + rs_set_left_ext_encoder( &robot, &left_ext_encoder, NULL, 1.0); + rs_set_right_ext_encoder( &robot, &right_ext_encoder, NULL, 1.0); + + printf("Robot system\n"); + + // ************************************************* + // * PID / MOTEURS ******************************** + // ************************************************* + + // + pid_init (&pid_motD); + pid_set_gains(&pid_motD, 200, 0, 0) ; + pid_set_maximums(&pid_motD, 0, 0, 0); + pid_set_out_shift(&pid_motD, 10); + + pid_init (&pid_motG); + pid_set_gains(&pid_motG, 200, 0, 0) ; + pid_set_maximums(&pid_motG, 0, 0, 0); + pid_set_out_shift(&pid_motG, 10); + + // Feedback filters + pid_init (&pid_feedback_motG); + pid_set_gains(&pid_feedback_motG, 0, 0, 1) ; + pid_set_maximums(&pid_feedback_motG, 0, 0, 0); + pid_set_out_shift(&pid_feedback_motG, 1); + + pid_init (&pid_feedback_motD); + pid_set_gains(&pid_feedback_motD, 0, 0, 1) ; + pid_set_maximums(&pid_feedback_motD, 0, 0, 0); + pid_set_out_shift(&pid_feedback_motD, 1); + + printf("PID moteur\n"); + + // ************************************************* + // * CONTROL SYSTEM / MOTEURS ******************** + // ************************************************* + + cs_init( &csm_motD ); + cs_init( &csm_motG ); + + // Filtre en consigne + cs_set_consign_filter( &csm_motD, NULL, NULL ); + cs_set_consign_filter( &csm_motG, NULL, NULL ); + + // Filtre en correction + cs_set_correct_filter( &csm_motD, &pid_do_filter, &pid_motD ); + cs_set_correct_filter( &csm_motG, &pid_do_filter, &pid_motG ); + + // Filtre de retour + cs_set_feedback_filter( &csm_motD, &pid_do_filter, &pid_feedback_motD ); + cs_set_feedback_filter( &csm_motG, &pid_do_filter, &pid_feedback_motG ); + + // Process out + cs_set_process_out( &csm_motD, &right_mot_encoder, NULL ); + cs_set_process_out( &csm_motG, &left_mot_encoder, NULL ); + + // Process in + cs_set_process_in( &csm_motD, &right_pwm, NULL ); + cs_set_process_in( &csm_motG, &left_pwm, NULL ); + + printf("Control system moteur\n"); + + // ************************************************* + // * QUADRAMP ************************************ + // ************************************************* + + quadramp_init( &qramp_distance ); + + quadramp_set_2nd_order_vars( &qramp_distance, 10, 10 ); + quadramp_set_1st_order_vars( &qramp_distance, 300, 300); + + printf("Quadramp distance\n"); + + quadramp_init( &qramp_angle ); + + quadramp_set_2nd_order_vars( &qramp_angle, 10, 10 ); + quadramp_set_1st_order_vars( &qramp_angle, 40,40); + + printf("Quadramp angle\n"); + + // ************************************************* + // * PID / DISTANCE-ANGLE ************************* + // ************************************************* + + pid_init (&pid_distance); + + pid_set_gains(&pid_distance, 3120, 100, 0) ; + pid_set_maximums(&pid_distance, 0, 10000, 0); + pid_set_out_shift(&pid_distance, 11); + + pid_init (&pid_angle); + + pid_set_gains(&pid_angle,1000, 5, 0) ; + pid_set_maximums(&pid_angle, 0, 50000, 0); + pid_set_out_shift(&pid_angle, 11); + + printf("PID dist/angle\n"); + + // ************************************************* + // * CONTROL SYSTEM / DISTANCE-ANGLE ************* + // ************************************************* + + cs_init( &csm_distance ); + cs_init( &csm_angle ); + + // Filtre en consigne + cs_set_consign_filter( &csm_distance, &quadramp_do_filter, &qramp_distance ); + cs_set_consign_filter( &csm_angle, &quadramp_do_filter, &qramp_angle ); + + // Filtre en correction + cs_set_correct_filter( &csm_distance, &pid_do_filter, &pid_distance ); + cs_set_correct_filter( &csm_angle, &pid_do_filter, &pid_angle ); + + // Filtre de retour + cs_set_feedback_filter( &csm_distance, NULL, NULL ); + cs_set_feedback_filter( &csm_angle, NULL, NULL ); + + // Process out + cs_set_process_out( &csm_distance, &rs_get_distance, &robot ); + cs_set_process_out( &csm_angle, &rs_get_angle, &robot ); + + // Process in + cs_set_process_in( &csm_distance, NULL, NULL ); + cs_set_process_in( &csm_angle, NULL, NULL ); + + printf("Control system dist/angle\n"); + + // ************************************************* + // * I2C ******************************************* + // ************************************************* + + //i2cs_init(I2C_ADDR_PROP); + + // ************************************************* + // * SCHEDULERS ********************************** + // ************************************************* + + // NOTE dans la config tel quelle on a une unite timer a 256 us.: + + scheduler_init(); + + scheduler_add_periodical_event_priority(&update_low_level,NULL,40,100); + scheduler_add_periodical_event_priority(&my_pos_manage,(void*)&position,400,50); + //scheduler_add_periodical_event_priority(&show_uart_dbg,NULL,1000,10); + //scheduler_add_periodical_event_priority(&i2c_manage,NULL,1000,40); + scheduler_add_periodical_event_priority(&debug,NULL,2000,0); + + printf("Scheduleurs\n"); + + // *****************scheduler**************** + + cs_set_consign( &csm_distance, 0 ); + cs_set_consign( &csm_angle, 0 ); + + // Initialisation de la bete : X, Y, Theta (en degrés) + position_set(&position,0,0,0); + + DDRB = 0x00; + + // On retire le frein + sbi(DDRB,7); + cbi(PORTB,7); + + printf("Position, uart register, frein out, go !\n"); + + DDRF = 0x00; + + // + + while(1) + { + nop(); + } + + return 0; +} + +/*************************************************************************** + *************************************************************************** + *************************************************************************** + ***************************************************************************/ + +void update_low_level(void * p) +{ + + //PORTC = ~PORTC; + + // Mise à jour de la position. + rs_update(&robot); + + // Moulinage de l'asserv distance/angle + // Calcul de la vitesse à adopter par la rampe. + cs_manage(&csm_distance); + cs_manage(&csm_angle); + + //printf("rp out=%6ld ",cs_get_out(&csm_distance1)); + //printf("rp in=%6ld",cs_get_error(&csm_distance1)); + + // Calcule les ordres (dD,dG) à partir de (d,theta) + // Et envoie les consignes aux asservs moteurs. + send_motors_consigns( cs_get_out(&csm_distance), cs_get_out(&csm_angle)); + + // + cs_manage(&csm_motD); + cs_manage(&csm_motG); + +} + +void send_motors_consigns(int32_t d, int32_t theta) +{ + int32_t d_D,d_G; + + if( !asserv_angle_active ) + theta=0; + + d_D = d - ((position.phys.track_cm)*theta)/2; + d_G = d + ((position.phys.track_cm)*theta)/2; + + cs_set_consign( &csm_motD, d_D ); + cs_set_consign( &csm_motG, d_G ); +} + +void my_pos_manage(void * p) +{ + position_manage((struct robot_position *)p); +} + +void left_pwm( void* p, int32_t val ) +{ + S_MAX(val, pwmMaxG); + pwmG = val; + pwm_set_3A( val ); +} + +void right_pwm( void* p, int32_t val ) +{ + S_MAX(val, pwmMaxD); + pwmD = val; + pwm_set_3C( val ); +} + +int32_t left_mot_encoder( void* p ) // CGM +{ + return (encMG = encoders_get_value(L_ENGINE_ENCODER)); +} + +int32_t right_mot_encoder( void* p ) // CDM +{ + return (encMD = encoders_get_value(R_ENGINE_ENCODER)); +} + +int32_t left_ext_encoder( void* p ) // CGC +{ + return (encSG = encoders_get_value(L_SEPARATED_ENCODER)); +} + +int32_t right_ext_encoder( void* p ) // CGD +{ + return (encSD = encoders_get_value(R_SEPARATED_ENCODER)); +} + +void debug( void* p ) +{ + /* + show( (int)(1*position_get_x_double(&position)) ); + uart0_send('x'); + show( (int)(1*position_get_y_double(&position)) ); + uart0_send('y')/; + show( (int)(180*position_get_a_rad_double(&position)/M_PI) ); + uart0_send('a'); + */ + + + /* printf("%dx%dy%da",position_get_x_int16_t(&position) + ,position_get_y_int16_t(&position) + ,position_get_a_deg_int16_t(&position)); +*/ + /* printf("x = %d y = %d a = %d \n",position_get_x_int16_t(&position) + ,position_get_y_int16_t(&position) + ,position_get_a_deg_int16_t(&position)); */ + + //uart0_send((char)position_get_a_deg_int16_t(&position));*/ + + printf("\r"); + + printf("d=%6ld ",rs_get_distance(&robot)); + printf("derr=%6ld ",cs_get_error(&csm_distance)); + printf("dout=%6ld ",cs_get_out(&csm_distance)); + + //printf("ramp=%6ld ",cs_get_out(&csm_distance1)); + + printf("a=%6ld ",rs_get_angle(&robot)); + printf("aout=%6ld ",cs_get_out(&csm_angle)); + printf("aerr=%6ld ",cs_get_error(&csm_angle)); + + //printf("pwmED=%6ld pwmEG=%6ld ",cs_get_error(&csm_motD),cs_get_error(&csm_motG)); + + //printf("pwminD=%6ld pwminG=%6ld ",cs_get_consign(&csm_motD),cs_get_consign(&csm_motG)); + + printf("D%4ld G%4ld",pwmG,pwmD); + +//printf("\n"); + + return; +} ===================================================== aversive/projects/coupe2008/unioc_asserv/pid_config.h (1.1.2.1) ===================================================== @@ -0,0 +1,30 @@ +/* + * 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 + * + * + * + */ + +#ifndef PID_CONFIG_H +#define PID_CONFIG_H + +/** the derivate term can be filtered to remove the noise. This value + * is the maxium sample count to keep in memory to do this + * filtering. For an instance of pid, this count is defined o*/ +#define PID_DERIVATE_FILTER_MAX_SIZE 4 + +#endif ===================================================== aversive/projects/coupe2008/unioc_asserv/pwm_config.h (1.1.2.1) ===================================================== @@ -0,0 +1,129 @@ +/* + * 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 2008-03-23 13:33:48 enseirb 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_ + +#define _PWM_CONFIG_VERSION_ 2 + +/* 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 (not all possibilities can be used at this time) +#define TIMER0_MODE TIMER_8_MODE_PWM +#define TIMER0_PRESCALE TIMER0_PRESCALER_DIV_64 + +#define TIMER1_MODE TIMER_16_MODE_PWM_10 +#define TIMER1_PRESCALE TIMER1_PRESCALER_DIV_8 + +#define TIMER2_MODE TIMER_8_MODE_PWM +#define TIMER2_PRESCALE TIMER1_PRESCALER_DIV_64 + +#define TIMER3_MODE TIMER_16_MODE_PWM_10 +#define TIMER3_PRESCALE TIMER3_PRESCALER_DIV_8 + + + + +/** 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 + + +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 + +*/ + + + +// example for signed pwm1A +#define PWM3A_MODE (PWM_SIGNED) +#define PWM3A_SIGN_PORT PORTB +#define PWM3A_SIGN_BIT 2 + +#define PWM3C_MODE (PWM_SIGNED) +#define PWM3C_SIGN_PORT PORTB +#define PWM3C_SIGN_BIT 3 + + +/** +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/projects/coupe2008/unioc_asserv/scheduler_config.h (1.1.2.1) =========================================================== @@ -0,0 +1,77 @@ +/* + * 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: scheduler_config.h,v 1.1.2.1 2008-03-23 13:33:48 enseirb Exp $ + * + */ + +#ifndef _SCHEDULER_CONFIG_H_ +#define _SCHEDULER_CONFIG_H_ + +#define _SCHEDULER_CONFIG_VERSION_ 4 + +/** maximum number of allocated events */ +#define SCHEDULER_NB_MAX_EVENT 5 + + +/* define it only if CONFIG_MODULE_SCHEDULER_USE_TIMERS is enabled. In + this case, precaler is defined in timers_config.h in your project + directory. */ +#ifdef CONFIG_MODULE_SCHEDULER_USE_TIMERS +/** the num of the timer to use for the scheduler */ +#define SCHEDULER_TIMER_NUM 0 + +/* or set the prescaler manually (in this case, you use must TIMER0, + and the prescaler must be a correct value regarding the AVR device + you are using (look in include/aversive/parts.h). Obviously, the + values of SCHEDULER_CK and SCHEDULER_CLOCK_PRESCALER must also be + coherent (TIMER0_PRESCALER_DIV_VALUE and VALUE) */ +#endif /* CONFIG_MODULE_SCHEDULER_USE_TIMERS */ + + +#ifdef CONFIG_MODULE_SCHEDULER_TIMER0 +/* The 2 values below MUST be coherent: + * if SCHEDULER_CK = TIMER0_PRESCALER_DIV_x, then + * you must have SCHEDULER_CLOCK_PRESCALER = x too !!! */ +#define SCHEDULER_CK TIMER0_PRESCALER_DIV_8 +#define SCHEDULER_CLOCK_PRESCALER 8 + +#endif /* CONFIG_MODULE_SCHEDULER_TIMER0 */ + +/* last case, the scheduler is called manually. The user has to + define the period here */ +#ifdef CONFIG_MODULE_SCHEDULER_MANUAL + +#define SCHEDULER_UNIT_FLOAT 1000.0 +#define SCHEDULER_UNIT 1000UL + +#endif /* CONFIG_MODULE_SCHEDULER_MANUAL */ + +/** number of allowed imbricated scheduler interrupts. The maximum + * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate + * more than once per event. If it is less, it can avoid to browse the + * event table, events are delayed (we loose precision) but it takes + * less CPU */ +#define SCHEDULER_NB_STACKING_MAX SCHEDULER_NB_MAX_EVENT + +/** define it for debug infos (not recommended, because very slow on + * an AVR, it uses printf in an interrupt). It can be useful if + * prescaler is very high, making the timer interrupt period very + * long in comparison to printf() */ +/* #define SCHEDULER_DEBUG */ + +#endif // _SCHEDULER_CONFIG_H_ ====================================================== aversive/projects/coupe2008/unioc_asserv/uart_config.h (1.1.2.1) ====================================================== @@ -0,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 2008-03-23 13:33:48 enseirb Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* + * UART1 definitions + */ + +/* compile UART1 fonctions, undefine it to pass compilation */ +#define UART1_COMPILE + +/* enable UART1 if == 1, disable if == 0 */ +#define UART1_ENABLED 1 + +/* enable UART1 interrupts if == 1, disable if == 0 */ +#define UART1_INTERRUPT_ENABLED 1 + +#define UART1_BAUDRATE 38400 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART1_USE_DOUBLE_SPEED 0 +//#define UART1_USE_DOUBLE_SPEED 1 + +#define UART1_RX_FIFO_SIZE 4 +#define UART1_TX_FIFO_SIZE 4 +//#define UART1_NBITS 5 +//#define UART1_NBITS 6 +//#define UART1_NBITS 7 +#define UART1_NBITS 8 +//#define UART1_NBITS 9 + +#define UART1_PARITY UART_PARTITY_NONE +//#define UART1_PARITY UART_PARTITY_ODD +//#define UART1_PARITY UART_PARTITY_EVEN + +#define UART1_STOP_BIT UART_STOP_BITS_1 +//#define UART1_STOP_BIT UART_STOP_BITS_2 + + + + +/* .... same for uart 1, 2, 3 ... */ + +#endif +
_______________________________________________ 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