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
[email protected]
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
