devthrottle.h
Go to the documentation of this file.
00001 /*
00002  *  ART throttle servo controller device interface
00003  *
00004  *  Copyright (C) 2007, 2009 Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  * 
00008  *  $Id: devthrottle.h 2 2010-01-17 01:54:03Z jack.oquin $
00009  */
00010 
00011 #ifndef _DEVTHROTTLE_H_
00012 #define _DEVTHROTTLE_H_
00013 
00014 #include <stdio.h>
00015 #include <stdint.h>
00016 #include <math.h>
00017 
00018 #include <ros/ros.h>
00019 
00020 #include "../servo.h"
00021 #include "avr_controller.h"
00022 
00023 #define DBG(format,args...) ROS_DEBUG(format, ## args)
00024 
00025 // One per throttle hardware device.
00026 class devthrottle: Servo
00027 {
00028 public:
00029 
00030   /* These public data are initialized by throttle.cc before calling
00031    * Open(), so initial values can be set from the .cfg file.
00032    */
00033   double rpm_redline;                   /* engine rev limit (RPM) */
00034   double throttle_limit;                /* throttle position limit (%) */
00035   int avr_kp;                           /* AVR proportional PID */
00036   int avr_ki;                           /* AVR integral PID */
00037   int avr_kd;                           /* AVR derivative PID */
00038   int avr_out_max;                      /* AVR max PWM output */
00039 
00040   /* Actual sensor limit positions are determined during Open(),
00041    * having been set to reasonable initial values by .cfg.
00042    */
00043   double avr_pos_max;
00044   int    avr_pos_min;
00045   double avr_pos_range;
00046   int    avr_pos_epsilon;               /* trivial difference value */
00047 
00048   devthrottle(bool train);
00049   ~devthrottle() {};
00050 
00051   int Open(const char *device);
00052   int Close(void);
00053 
00054   int64_t GetTime();
00055 
00056   // accessor method for current position
00057   float get_position(void) {return cur_position;}
00058 
00059   // query methods for current status
00060   bool  query_estop(void);
00061   int   query_pid(float *pwm, float *dstate, float *istate);
00062   int   query_rpms(float *data);
00063   int   query_status();
00064 
00065   // throttle command methods
00066   int   throttle_absolute(float position);
00067   int   throttle_relative(float delta);
00068 
00069  private:
00070 
00071   bool  training;                       // use training mode
00072   bool  already_configured;             // throttle already configured once
00073 
00074   struct avr_cmd stat;                  /* latest throttle status */
00075   struct avr_cmd cmd;                   /* outgoing AVR command */
00076   struct avr_cmd resp;                  /* AVR command response */
00077 
00078   /* byte pointers to these structures */
00079   uint8_t *cmd_p;
00080   uint8_t *resp_p;
00081 
00082   float cur_position;                   // last position read
00083   float last_req;                       // last position requested
00084 
00085   int     calibrate_idle(void);
00086   uint8_t cmd_compute_csum(uint8_t *buffer, int len);
00087   int     configure_controller(void);
00088   void    decode_char(char c, int *resp_bytes, int *resp_digits);
00089   int     format_cmd(char *cmdstr);
00090   int     read_byte(int linelen);
00091   int     send_cmd(int ccode);
00092   int     send_cmd08(int ccode, uint8_t data);
00093   int     send_cmd16(int ccode, uint16_t data);
00094   int     send_cmd32(int ccode, uint32_t data);
00095   int     send_goto(uint8_t pos);
00096   int     servo_cmd(void);
00097   int     validate_response(int resp_bytes, int linelen);
00098 
00099   // return position valued constrained to permitted range
00100   inline float limit_travel(float position)
00101     {
00102       if (position > throttle_limit)    position = throttle_limit;
00103       else if (position < 0.0)          position = 0.0;
00104       return position;
00105     }
00106 
00107   // Conversions between throttle percentages and controller values.
00108   inline uint8_t pos2avr(float position)
00109     {return (int) rintf(avr_pos_min + position * avr_pos_range);}
00110 
00111   inline float avr2pos(uint8_t avr_val)
00112     {return (avr_val - avr_pos_min) / avr_pos_range;}
00113 };
00114 
00115 #endif // _DEVTHROTTLE_H_


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12