avr/modules_avr/asserv/microb/trajectory/trajectory.c

Go to the documentation of this file.

/*  
 *  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: trajectory.c,v 1.7 2005/05/02 09:10:56 zer0 Exp $
 *
 */

#include <utils/utils.h>
#include <time/scheduler/scheduler.h>
#include <asserv/microb/asserv_main/asserv_main.h>
#include <position/xy/xy.h>
#include <asserv/eirbot/vect2/vect2.h>
#include <asserv/microb/trajectory/trajectory.h>
#include <trajectory_config.h>

#define TRAJECTORY_XY_ANGLE_END_RAD (TRAJECTORY_XY_ANGLE_END*M_PI/180.0)
#define TRAJECTORY_XY_ANGLE_MIN_MOVE_RAD (TRAJECTORY_XY_ANGLE_MIN_MOVE*M_PI/180.0)
#define TRAJECTORY_XY_FORWARD_BACKWARD_PREFERENCY_ANGLE_RAD (TRAJECTORY_XY_FORWARD_BACKWARD_PREFERENCY_ANGLE*M_PI/180.0)

#define DEBUG 0

#include <stdio.h>

#define debug_printf(args...) do { if(DEBUG) printf(args); } while(0)

static s08 id=-1;
extern struct asserv_config asserv;

void trajectory_gotoxy_update(void);
void trajectory_gotoa_update(void);

void trajectory_init(void)
{
}

void trajectory_del_cur_traj_event(void)
{
  if(id != -1) {
  IRQ_LOCK(); 
  scheduler_del_event(id);    
  id=-1;
  IRQ_UNLOCK(); 
  }
}

void trajectory_set_speed_alpha(u16 speed_deg_s)
{
  float s = (float)speed_deg_s * asserv.imp_alpha_per_deg * asserv.period;
  IRQ_LOCK();
  ramp_generator_set_speed(&asserv.ramp_alpha, s, -s);
  IRQ_UNLOCK();
}

void trajectory_set_speed_theta(u16 speed_cm_s)
{
  float s = (float)speed_cm_s * asserv.imp_theta_per_cm * asserv.period;
  IRQ_LOCK();
  ramp_generator_set_speed(&asserv.ramp_theta, s, -s);
  IRQ_UNLOCK();
}

void trajectory_straight(s16 pos_cm)
{
  double destx, desty, a;

  destx = xy_get_x_float(&asserv.pos);
  desty = xy_get_y_float(&asserv.pos);
  a     = xy_get_a_rad_float(&asserv.pos);

  debug_printf("%2.2f %2.2f %2.2f\r\n", destx, desty, a);

  destx = destx + cos(a)*(double)pos_cm ;
  desty = desty + sin(a)*(double)pos_cm ;
  
  debug_printf("%2.2f %2.2f\r\n", destx, desty);

  trajectory_gotoxy(destx, desty);
}


void __trajectory_gotoxy(s16 x, s16 y, u08 update)
{
  static vect2 dest;
  vect2 cur,diff;
  double dest_angle;
  s32 ramp_alpha, ramp_theta;

  /* init all values */
  if(!update) {
    IRQ_LOCK();
    trajectory_del_cur_traj_event();
    asserv_reset_block(&asserv);
    asserv.status &= (~ASSERV_STATUS_BIT_END_TRAJ);
    IRQ_UNLOCK();
    dest.cart.type = VECT2_CARTESIEN ;
    dest.cart.x = (double)x;
    dest.cart.y = (double)y;
    id=scheduler_add_periodical_event_priority(trajectory_gotoxy_update, 100000l/SCHEDULER_UNIT, 50);
    debug_printf("EVT\r\n");
  }

  /* get cur pos */
  cur.cart.x = xy_get_x_float(&asserv.pos);
  cur.cart.y = xy_get_y_float(&asserv.pos);
  
  /* process difference diff=dest-cur*/
  diff = dest;
  vect2_sub(&diff, &cur, 2 * M_PI);
  vect2_2pol(&diff, 2 * M_PI);

  /* process dest_angle */
  dest_angle = diff.pol.teta-(double)xy_get_a_rad_float(&asserv.pos) ;
  if(dest_angle < -M_PI)
    dest_angle += (M_PI*2);
  else if (dest_angle > (M_PI))
    dest_angle -= (M_PI*2);
  
  if (diff.pol.u < TRAJECTORY_XY_DIST_END) {
    /* end of traj */
    debug_printf("Fin\r\n");
    trajectory_del_cur_traj_event();
    asserv.status |= (ASSERV_STATUS_BIT_END_TRAJ);
  }
  
  if (ABS(dest_angle) < TRAJECTORY_XY_ANGLE_MIN_MOVE_RAD) {
    /* start moving forward to the dest */
    debug_printf("Avance ");
    ramp_theta = asserv.ta_pos.theta + (s32)(diff.pol.u * asserv.imp_theta_per_cm);
    ramp_alpha = asserv.ta_pos.alpha + (s32)(dest_angle * asserv.imp_alpha_per_deg * 180.0/M_PI);
  }
  else if (ABS(dest_angle) < TRAJECTORY_XY_FORWARD_BACKWARD_PREFERENCY_ANGLE_RAD) {
    /* turn to have the dest point forward */
    debug_printf("Tourne AV ");
    ramp_theta = asserv.ta_pos.theta;
    ramp_alpha = asserv.ta_pos.alpha + (s32)(dest_angle * asserv.imp_alpha_per_deg * 180.0/M_PI);
  }
  else if (ABS(dest_angle) < M_PI - TRAJECTORY_XY_ANGLE_MIN_MOVE_RAD) {
    /* turn to have the dest point backward */
    debug_printf("Tourne AR ");
    ramp_theta = asserv.ta_pos.theta;
    if(dest_angle < 0) {
      ramp_alpha = asserv.ta_pos.alpha + (s32)((dest_angle + M_PI)* asserv.imp_alpha_per_deg * 180.0/M_PI);
    }
    else {
      ramp_alpha = asserv.ta_pos.alpha + (s32)((dest_angle - M_PI)* asserv.imp_alpha_per_deg * 180.0/M_PI);
    }
  }
  else {
    /* start moving backward to the dest */
    debug_printf("Recule ");
    ramp_theta = asserv.ta_pos.theta - (s32)(diff.pol.u * asserv.imp_theta_per_cm);
    if(dest_angle < 0) {
      ramp_alpha = asserv.ta_pos.alpha + (s32)((dest_angle + M_PI)* asserv.imp_alpha_per_deg * 180.0/M_PI);
    }
    else {
      ramp_alpha = asserv.ta_pos.alpha + (s32)((dest_angle - M_PI)* asserv.imp_alpha_per_deg * 180.0/M_PI);
    }
  }

  debug_printf("theta %ld, alpha %ld, cur_theta %ld\r\n", ramp_theta, ramp_alpha, asserv.ta_pos.theta);

  IRQ_LOCK(); 
  if(update) {
    ramp_generator_update_pos(&asserv.ramp_theta, ramp_theta);
    ramp_generator_update_pos(&asserv.ramp_alpha, ramp_alpha);
  }
  else {
    ramp_generator_new(&asserv.ramp_theta, asserv.ta_pos.theta,
                       ramp_theta, asserv.ta_speed.theta);
    ramp_generator_new(&asserv.ramp_alpha, asserv.ta_pos.alpha, 
                       ramp_alpha, asserv.ta_speed.alpha);
  }
  IRQ_UNLOCK(); 

  debug_printf("d%2.2f a%2.2f\r\n", diff.pol.u, dest_angle);
}

/* this is the callback function for gotoxy */
void trajectory_gotoxy_update(void)
{
  __trajectory_gotoxy(0, 0, 1);
}

inline void trajectory_gotoxy(s16 x, s16 y)
{
  __trajectory_gotoxy(x,y,0);
}

void __trajectory_gotoa(s16 a, u08 update)
{
  static double dest_angle;
  s32 ramp_alpha, ramp_theta;
  double diff_angle;
  double cur_angle=xy_get_a_rad_float(&asserv.pos);

  if(!update) {
    /* init all values */
    IRQ_LOCK();
    trajectory_del_cur_traj_event();
    asserv_reset_block(&asserv);
    asserv.status &= (~ASSERV_STATUS_BIT_END_TRAJ);
    dest_angle = a * (M_PI/180.0);
    if(dest_angle < -M_PI)
      dest_angle += (M_PI*2);
    else if (dest_angle > (M_PI))
      dest_angle -= (M_PI*2);
    id=scheduler_add_periodical_event_priority(trajectory_gotoa_update, 100000l/SCHEDULER_UNIT, 50);
    IRQ_UNLOCK();
  }

  /* process diff_angle */
  diff_angle = (double)dest_angle - cur_angle ;
  if(diff_angle < -M_PI)
    diff_angle += (M_PI*2);
  else if (diff_angle > (M_PI))
    diff_angle -= (M_PI*2);
  
  if(update)
    debug_printf("Running gotoa : dest=%2.2f cur=%2.2f diff=%2.2f\r\n", dest_angle, cur_angle, diff_angle);  
  else
    debug_printf("Init gotoa : dest=%2.2f cur=%2.2f diff=%2.2f\r\n", dest_angle, cur_angle, diff_angle);  
    

  /* end of traj ? */
  if (ABS(diff_angle) < TRAJECTORY_XY_ANGLE_END_RAD) {
    /* end of traj */
    debug_printf("Fin\r\n");
    trajectory_del_cur_traj_event();
    asserv.status |= (ASSERV_STATUS_BIT_END_TRAJ);
  }

  
  /* turn to have the dest point forward */
  ramp_theta = asserv.ta_pos.theta;
  ramp_alpha = asserv.ta_pos.alpha + (s32)(diff_angle * asserv.imp_alpha_per_deg * 180.0/M_PI);
  
  IRQ_LOCK(); 
  if(update) {
    ramp_generator_update_pos(&asserv.ramp_theta, ramp_theta);
    ramp_generator_update_pos(&asserv.ramp_alpha, ramp_alpha);
  }
  else {
    ramp_generator_new(&asserv.ramp_theta, asserv.ta_pos.theta,
                       ramp_theta, asserv.ta_speed.theta);
    ramp_generator_new(&asserv.ramp_alpha, asserv.ta_pos.alpha, 
                       ramp_alpha, asserv.ta_speed.alpha);
  }
  IRQ_UNLOCK(); 
  
  

  

}

inline void trajectory_gotoa(s16 a)
{
  __trajectory_gotoa(a, 0);
}

/* this is the callback function for gotoa */
void trajectory_gotoa_update(void)
{
  __trajectory_gotoa(0, 1);
}


inline void trajectory_gotoa_fromxy(s16 x, s16 y)
{
  vect2 dest, cur;

  dest.cart.type = VECT2_CARTESIEN ;
  dest.cart.x = (double)x;
  dest.cart.y = (double)y;

  /* get cur pos */
  cur.cart.x = xy_get_x_float(&asserv.pos);
  cur.cart.y = xy_get_y_float(&asserv.pos);
  
  dest = dest;
  vect2_sub(&dest, &cur, 360.0);
  vect2_2pol(&dest, 360.0);

  trajectory_gotoa(dest.pol.teta);
}

---