modules/devices/control_system/filters/quadramp_derivate/test/main.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: main.c,v 1.2.2.3 2007-05-23 17:18:13 zer0 Exp $
  19  *
  20  */
  21
  22 #include <avr/io.h>
  23 #include <aversive/wait.h>
  24 #include <aversive.h>
  25
  26 #include <brushless.h>
  27 #include <biquad.h>
  28
  29 #include <pwm.h>
  30
  31
  32
  33 #include <uart.h>
  34 #include <stdio.h>
  35 #include <aversive/pgmspace.h>
  36
  37
  38
  39
  40 #include <pid.h>
  41 #include <control_system_manager.h>
  42 #include <quadramp_derivate.h>
  43
  44
  45
  46 struct cs                          speed;
  47 struct pid_filter                  speed_pid;
  48 struct biquad_filter               speed_derivation;
  49
  50
  51 struct cs                          position;
  52 struct quadramp_derivate_filter    position_quadr_derv;
  53
  54
  55
  56 // periodical 10ms execution
  57 void asserv_rapide_manage(brushless data_moteur)
  58 {
  59
  60         cs_manage(&position);
  61
  62         cs_manage(&speed);
  63
  64
  65
  66         // for debug graphs
  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);
  69
  70         //printf_P(PSTR("i%lio%liv%li\n"), speed_pid.prev_in, speed_pid.prev_out,  vitesse);
  71 }
  72
  73
  74 int main(void)
  75 {
  76
  77
  78
  79   // init motors and PWMs
  80   brushless_init();
  81
  82
  83   // enable power bridges
  84   sbi(DDRG, 1);
  85   sbi(PORTG, 1);
  86
  87
  88   // init uart
  89   uart_init();
  90   fdevopen((void *)uart0_send,NULL,0);
  91
  92
  93
  94   //printf_P(PSTR("\nbonjour\n"));
  95
  96
  97
  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
 100
 101
 102
 103   /** speed PID stuff */
 104
 105   // PID
 106   pid_init(&speed_pid);
 107
 108   pid_set_gains(&speed_pid,     180, 70, 40); // sur alim
 109
 110   pid_set_maximums(&speed_pid,  0, 80, PWM_MAX*4/5);
 111   pid_set_out_shift(&speed_pid, 0);
 112
 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
 117
 118   // control system speed
 119   cs_init(&speed);
 120
 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);
 126
 127
 128
 129   /** ramp generator parameters */
 130
 131   quadramp_derivate_init(&position_quadr_derv);
 132
 133   quadramp_derivate_set_gain_anticipation(&position_quadr_derv, 256 *3);// some anticipation : 3.0 (this is a fixed point value *1/256)
 134
 135   quadramp_derivate_set_goal_window(&position_quadr_derv, 5);           // algorithm shut down window
 136
 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
 139
 140   quadramp_derivate_set_divisor(&position_quadr_derv, 2);               // divisor, for precision
 141
 142
 143   // control system position
 144   cs_init(&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);
 149
 150
 151   /** begin */
 152
 153   brushless_set_speed((void *)0 , BRUSHLESS_MAX_SPEED); // init speed
 154
 155   sei();
 156
 157
 158
 159
 160   // some simple trajectories (enable one )
 161
 162   while(1)
 163   {
 164   wait_ms(3500);
 165   cs_set_consign(&position, 400);
 166   wait_ms(500);
 167   cs_set_consign(&position, 0);
 168   }
 169
 170
 171   /*
 172   while(1)
 173   {
 174   wait_ms(2500);
 175   cs_set_consign(&position, 2000);
 176   wait_ms(2500);
 177   cs_set_consign(&position, 0);
 178   }
 179
 180
 181
 182   // test of speed pid only, deactivate the position.
 183   while(1)
 184   {
 185   wait_ms(300);
 186   cs_set_consign(&speed, 10);
 187   wait_ms(300);
 188   cs_set_consign(&speed, -10);
 189   } */
 190
 191
 192
 193
 194   while(1);
 195
 196
 197   return 0;
 198 }
 199