2 * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: main.c,v 1.2.2.3 2007-05-23 17:18:13 zer0 Exp $
23 #include <aversive/wait.h>
26 #include <brushless.h>
35 #include <aversive/pgmspace.h>
41 #include <control_system_manager.h>
42 #include <quadramp_derivate.h>
47 struct pid_filter speed_pid;
48 struct biquad_filter speed_derivation;
52 struct quadramp_derivate_filter position_quadr_derv;
56 // periodical 10ms execution
57 void asserv_rapide_manage(brushless data_moteur)
67 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
68 , brushless_get_speed((void*)0), speed.consign_value);
70 //printf_P(PSTR("i%lio%liv%li\n"), speed_pid.prev_in, speed_pid.prev_out, vitesse);
79 // init motors and PWMs
83 // enable power bridges
90 fdevopen((void *)uart0_send,NULL,0);
94 //printf_P(PSTR("\nbonjour\n"));
98 /** replaces the scheduler. This is directly derived from the interrupt which runs the brushless motors, for convenience */
99 brushless_0_register_periodic_event(asserv_rapide_manage); // 10 ms
103 /** speed PID stuff */
106 pid_init(&speed_pid);
108 pid_set_gains(&speed_pid, 180, 70, 40); // sur alim
110 pid_set_maximums(&speed_pid, 0, 80, PWM_MAX*4/5);
111 pid_set_out_shift(&speed_pid, 0);
113 // derivation (This could alternatively be skipped if we use the brushless peed info directly)
114 biquad_init(&speed_derivation);
115 biquad_set_numerator_coeffs(&speed_derivation, 1,-1,0); // this is a discrete derivation : O(n) = I(n) - I(n-1)
116 // no need to initialize denominator coeffs to 0, init has done it
118 // control system speed
121 cs_set_correct_filter(&speed, pid_do_filter, &speed_pid);
122 cs_set_process_in(&speed, brushless_set_torque, (void *)0 );
123 cs_set_process_out(&speed,brushless_get_pos , (void *)0 );
124 cs_set_feedback_filter(&speed, biquad_do_filter, &speed_derivation);
125 cs_set_consign(&speed, 0);
129 /** ramp generator parameters */
131 quadramp_derivate_init(&position_quadr_derv);
133 quadramp_derivate_set_gain_anticipation(&position_quadr_derv, 256 *3);// some anticipation : 3.0 (this is a fixed point value *1/256)
135 quadramp_derivate_set_goal_window(&position_quadr_derv, 5); // algorithm shut down window
137 quadramp_derivate_set_2nd_order_vars(&position_quadr_derv, 1 , 1); // max acceleration : 1
138 quadramp_derivate_set_1st_order_vars(&position_quadr_derv, 12, 12); // max speed is 12
140 quadramp_derivate_set_divisor(&position_quadr_derv, 2); // divisor, for precision
143 // control system position
145 cs_set_correct_filter(&position, quadramp_derivate_do_filter, &position_quadr_derv);
146 cs_set_process_in(&position, (void *)cs_set_consign, &speed );
147 cs_set_process_out(&position,brushless_get_pos , (void *)0 );
148 cs_set_consign(&position, 0);
153 brushless_set_speed((void *)0 , BRUSHLESS_MAX_SPEED); // init speed
160 // some simple trajectories (enable one )
165 cs_set_consign(&position, 400);
167 cs_set_consign(&position, 0);
175 cs_set_consign(&position, 2000);
177 cs_set_consign(&position, 0);
182 // test of speed pid only, deactivate the position.
186 cs_set_consign(&speed, 10);
188 cs_set_consign(&speed, -10);