From ed961665ec4856083d979f22dad7287c49a48944 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 18 Apr 2004 16:48:11 +0000 Subject: Changement de l'acceleration. Changement du système de stat. Retardement des int_enable. --- 2004/n/asserv/src/main.c | 10 +++++++++- 2004/n/asserv/src/motor.c | 44 ++++++++++++++++++++++---------------------- 2004/n/asserv/src/serial.c | 12 +++++++++--- 2004/n/asserv/src/serial.h | 4 ++++ 4 files changed, 44 insertions(+), 26 deletions(-) (limited to '2004/n/asserv') diff --git a/2004/n/asserv/src/main.c b/2004/n/asserv/src/main.c index d8f0622..6a6ddaa 100644 --- a/2004/n/asserv/src/main.c +++ b/2004/n/asserv/src/main.c @@ -55,7 +55,7 @@ A1-5 B2-7 C4-5 D2-6 E0-2 /* Configuration générale. */ #fuses H4,WDT,WDT128,PUT,NOBROWNOUT,NOLVP #use delay(clock=40000000) -#use rs232(baud=9600,xmit=PIN_C6,parity=N,rcv=PIN_C7,bits=8) +#use rs232(baud=115200,xmit=PIN_C6,parity=N,rcv=PIN_C7,bits=8) #priority RB,RDA,TBE,TIMER2 #include "motor.c" @@ -90,6 +90,12 @@ main_init (void) setup_ccp2 (CCP_PWM); set_pwm1_duty (0); set_pwm2_duty (0); +} + +/* Initialise les interruptions. */ +void +irq_init (void) +{ /* Configuration des interruptions activées. */ enable_interrupts (INT_TIMER2); enable_interrupts (INT_RDA); @@ -105,5 +111,7 @@ main (void) main_init (); motor_init (); serial_init (); + irq_init (); + serial_send_rezet (); motor_main (); } diff --git a/2004/n/asserv/src/motor.c b/2004/n/asserv/src/motor.c index 095d132..2f05c3a 100644 --- a/2004/n/asserv/src/motor.c +++ b/2004/n/asserv/src/motor.c @@ -40,12 +40,12 @@ unsigned int motor_ttl; /* Time to live : arr 0 en mode asservissement position. */ unsigned int motor_kp, motor_ki, motor_kd; /* Coefficients du PID. */ unsigned int motor_a; /* Acceleration. */ +unsigned int motor_a_cpt; /* Compteur d'acceleration. */ signed long motor_int_max; /* Terme intégral maximum. */ unsigned int motor_pid_int; /* Compteur d'interruptions timer2 entre deux calculs de PID. */ /* Statistiques, etc... */ -short motor_stat; /* Report motor stats. */ unsigned int motor_stat_delay; /* Delay between stats. */ unsigned int motor_stat_delay_cpt; /* Delay counter. */ short motor_cpt; /* Report motors counters. */ @@ -76,11 +76,11 @@ motor_init (void) motor_pos_asserv = 0; motor_ttl = 10; motor_kp = 10; motor_ki = 5; motor_kd = 0; - motor_a = 2; + motor_a = 8; + motor_a_cpt = 8; motor_int_max = 1000; motor_pid_int = 0; - motor_stat = 0; - motor_stat_delay = 101; + motor_stat_delay = 0; motor_stat_delay_cpt = 0; motor_cpt = 0; motor_g_vdes = 0; motor_g_vacc = 0; @@ -183,16 +183,12 @@ motor_update_left_speed (void) if (motor_g_vacc < motor_g_vdes) { /* Accélère. */ - motor_g_vacc += motor_a; - if (motor_g_vacc > motor_g_vdes) - motor_g_vacc = motor_g_vdes; + motor_g_vacc++; } else { /* Freine. */ - motor_g_vacc -= motor_a; - if (motor_g_vacc < motor_g_vdes) - motor_g_vacc = motor_g_vdes; + motor_g_vacc--; } } } @@ -206,16 +202,12 @@ motor_update_right_speed (void) if (motor_d_vacc < motor_d_vdes) { /* Accélère. */ - motor_d_vacc += motor_a; - if (motor_d_vacc > motor_d_vdes) - motor_d_vacc = motor_d_vdes; + motor_d_vacc++; } else { /* Freine. */ - motor_d_vacc -= motor_a; - if (motor_d_vacc < motor_d_vdes) - motor_d_vacc = motor_d_vdes; + motor_d_vacc--; } } } @@ -398,9 +390,9 @@ motor_main (void) motor_compute_left_pid (); motor_compute_right_pid (); /* Information de PWM. */ - if (motor_stat) + if (motor_stat_delay) { - if (!motor_stat_delay_cpt--) + if (!--motor_stat_delay_cpt) { serial_send_motor_stat ('l', motor_g_vacc, motor_g_e_old, motor_g_pwm_old); @@ -410,19 +402,25 @@ motor_main (void) motor_stat_delay_cpt = motor_stat_delay; } } + /* Accélère. */ + if (!--motor_a_cpt) + { + motor_update_left_speed (); + motor_update_right_speed (); + motor_a_cpt = motor_a; + } } /* Le reste. */ if ((motor_pid_int & 63) == 0) { /* Gestion du ttl. */ - if (motor_pos_asserv && --motor_ttl == 0) + if (motor_pos_asserv && (motor_g_vdes || motor_d_vdes) && + --motor_ttl == 0) { motor_g_vdes = 0; motor_d_vdes = 0; serial_send_motor_ttl (); } - motor_update_left_speed (); - motor_update_right_speed (); serial_parse (); if (motor_cpt) { @@ -459,6 +457,7 @@ motor_parse (void) return 1; case 'a': serial_recv_int (motor_a); + motor_a_cpt = motor_a; serial_send_ok (); return 1; case 'p': @@ -474,7 +473,8 @@ motor_parse (void) serial_send_ok (); return 1; case 'm': - serial_recv_bool (motor_stat); + serial_recv_int (motor_stat_delay); + motor_stat_delay_cpt = motor_stat_delay; serial_send_ok (); return 1; case 'c': diff --git a/2004/n/asserv/src/serial.c b/2004/n/asserv/src/serial.c index 5b48cc9..add7904 100644 --- a/2004/n/asserv/src/serial.c +++ b/2004/n/asserv/src/serial.c @@ -61,9 +61,6 @@ serial_init (void) serial_send_full = 0; serial_unreliable_send_buf_n = 0; serial_unreliable_send_full = 0; - serial_send_char ('!'); - serial_send_char ('z'); - serial_send_char (CRLF); } /* Recois des caractères. */ @@ -169,6 +166,15 @@ serial_parse (void) serial_recv_full = 0; } +/* Envoie un code rezet. */ +void +serial_send_rezet (void) +{ + serial_send_char ('!'); + serial_send_char ('z'); + serial_send_char (CRLF); +} + /* Envoie un code d'erreur. */ void serial_send_error (char c) diff --git a/2004/n/asserv/src/serial.h b/2004/n/asserv/src/serial.h index f371239..12fb564 100644 --- a/2004/n/asserv/src/serial.h +++ b/2004/n/asserv/src/serial.h @@ -65,6 +65,10 @@ serial_send (); void serial_parse (void); +/* Envoie un code rezet. */ +void +serial_send_rezet (void); + /* Envoie un code d'erreur. */ void serial_send_error (char c); -- cgit v1.2.3