Koala Library
|
00001 //--------------------------------------------------------------------------------// 00002 //- KOALA( Koala extension board ) -// 00003 // -// 00004 //- Copyright (C) Julien Tharin, K-Team S.A. 2013 -// 00005 //- This library is free software; you can redistribute it and/or -// 00006 //- modify it under the terms of the GNU Lesser General Public -// 00007 //- License as published by the Free Software Foundation; either -// 00008 //- version 2.1 of the License, or any later version. -// 00009 //- -// 00010 //- This library is distributed in the hope that it will be useful, -// 00011 //- but WITHOUT ANY WARRANTY; without even the implied warranty of -// 00012 //- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// 00013 //- Lesser General Public License for more details. -// 00014 //- -// 00015 //- You should have received a copy of the GNU Lesser General Public -// 00016 //- License along with this library; if not, write to the Free Software -// 00017 //- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -// 00018 //- -// 00019 //- __ __ ________ -// 00020 //- K-Team S.A. | |/ /|__ __|___ _____ ___ ___ -// 00021 //- Chemin des Plans-Praz 28, | / __ | | _____|/ _ \| \/ | -// 00022 //- 1337 Vallorbe | | \ | | ____|/ /_\ | | -// 00023 //- Switzerland |__|\__\ |__|______|_/ \_|__|\/|__| -// 00024 //- jtharin@k-team.com tel:+41 24 423 89 56 fax:+41 24 423 8960 -// 00025 //- -// 00026 //--------------------------------------------------------------------------------// 00027 00029 00032 00033 00034 00035 #ifndef __koala_robot__ 00036 #define __koala_robot__ 00037 00038 #include <time.h> 00039 #include <math.h> 00040 00045 #define KOALA_SERIAL_PORT_NAME "/dev/ttyS1" 00046 #define KOALA_SERIAL_PORT_BAUDRATE B115200 00047 00048 #define KOALA_MAX_BUFFER 1024 // max input/output buffer for sending/receiving data 00049 00050 #define KOALA_DELIM "," // delimiter for parameters 00051 00052 // bit configuration for auto monitoring mode 00053 #define KOALA_AUTOM_US_SENSOR_BIT 1<<0 // US sensor 00054 #define KOALA_AUTOM_MOTOR_SPEED 1<<1 // Motor Speed 00055 #define KOALA_AUTOM_MOTOR_POSITION 1<<2 // Motor Position 00056 #define KOALA_AUTOM_MOTOR_CURRENT 1<<3 // Motor Current 00057 #define KOALA_AUTOM_ACCEL_VALUE 1<<4 // Accelerometer value 00058 #define KOALA_AUTOM_GYRO_VALUE 1<<5 // Gyroscope value 00059 #define KOALA_AUTOM_GPS_DATA 1<<6 // GPS data 00060 #define KOALA_AUTOM_GPS_NEMA 1<<7 // GPS NEMA Data (will return all GPS raw data) 00061 #define KOALA_AUTOM_MAGNE_VALUE 1<<8 // Magnometer value 00062 #define KOALA_AUTOM_ALL 0x01FF // all options activated 00063 #define KOALA_AUTOM_NONE 0x0000 // all options desactivated 00064 00065 00066 // bit configuration for US sensors mask 00067 #define KOALA_US_LEFT_REAR 1<<0 // Left rear 00068 #define KOALA_US_LEFT_FRONT 1<<1 // Left front 00069 #define KOALA_US_FRONT_LEFT 1<<2 // Front left 00070 #define KOALA_US_FRONT 1<<3 // Front 00071 #define KOALA_US_FRONT_RIGHT 1<<4 // Front left 00072 #define KOALA_US_RIGHT_FRONT 1<<5 // Right front 00073 #define KOALA_US_RIGHT_REAR 1<<6 // Right rear 00074 #define KOALA_US_BACK_RIGHT 1<<7 // Back right 00075 #define KOALA_US_BACK_LEFT 1<<8 // Back left 00076 #define KOALA_US_ALL 511 // all US activated 00077 #define KOALA_US_NONE 0 // all US desactivated 00078 00079 extern const char *KOALA_US_SENSOR_NAMES[]; 00080 00081 00082 #define KOALA_US_SENSORS_NUMBER 9 // number of US sensors 00083 00084 // bit configuration for Input/Output ports mask 00085 #define KOALA_IO_0_OUTPUT 0b00000000 // Port 0 in output 00086 #define KOALA_IO_0_INPUT 0b00000001 // Port 0 in input 00087 #define KOALA_IO_0_PWMS 0b00000010 // Port 0 in PWM servo 00088 #define KOALA_IO_1_OUTPUT 0b00000000 // Port 1 in output 00089 #define KOALA_IO_1_INPUT 0b00000100 // Port 1 in input 00090 #define KOALA_IO_1_PWMS 0b00001000 // Port 1 in PWM servo 00091 #define KOALA_IO_2_OUTPUT 0b00000000 // Port 2 in output 00092 #define KOALA_IO_2_INPUT 0b00010000 // Port 2 in input 00093 #define KOALA_IO_2_PWMS 0b00100000 // Port 2 in PWM servo 00094 #define KOALA_IO_3_OUTPUT 0b00000000 // Port 2 in output 00095 #define KOALA_IO_3_INPUT 0b01000000 // Port 2 in input 00096 #define KOALA_IO_3_PWMS 0b10000000 // Port 2 in PWM servo 00097 #define KOALA_IO_ALL_OUTPUT 0b00000000 // All IO ports in output 00098 00099 00100 // bit configuration for POWER Input/Output ports mask 00101 #define KOALA_PWR_IO_0_0 0b00000000 // Power Port 0 to 0 00102 #define KOALA_PWR_IO_0_1 0b00000001 // Power Port 0 to 1 00103 #define KOALA_PWR_IO_1_0 0b00000000 // Power Port 1 to 0 00104 #define KOALA_PWR_IO_1_1 0b00000010 // Power Port 1 to 1 00105 #define KOALA_PWR_IO_2_0 0b00000000 // Power Port 2 to 0 00106 #define KOALA_PWR_IO_2_1 0b00000100 // Power Port 2 to 1 00107 #define KOALA_PWR_IO_3_0 0b00000000 // Power Port 3 to 0 00108 #define KOALA_PWR_IO_3_1 0b00001000 // Power Port 3 to 1 00109 00110 00111 00112 #define KOALA_ACCEL_G (1.0/16384.0) // convert to [g] 00113 #define KOALA_ACCEL_VALUES_NUMBER 30 // number of values from the accelerometer: 3 axes * 10 values 00114 00115 #define KOALA_NEW_ACCEL_X 0 // position of newest X acceleration in the buffer 00116 #define KOALA_NEW_ACCEL_Y 1 // position of newest Y acceleration in the buffer 00117 #define KOALA_NEW_ACCEL_Z 2 // position of newest Z acceleration in the buffer 00118 00119 #define KOALA_GYRO_DEG_S (66.0/1000.0) // convert to [deg/s] 00120 #define KOALA_GYRO_VALUES_NUMBER 30 // number of values from the gyrometer: 3 axes * 10 values 00121 #define KOALA_NEW_GYRO_X 0 // position of newest X speed in the buffer 00122 #define KOALA_NEW_GYRO_Y 1 // position of newest Y speed in the buffer 00123 #define KOALA_NEW_GYRO_Z 2 // position of newest Z speed in the buffer 00124 00125 #define KOALA_MAGNE_VALUES_NUMBER 3 // x, y ,z 00126 00127 #define KOALA_MAX_I2C_DATA 256 // max i2c data to be send/received at one time 00128 00129 00130 // robot hardware constants 00131 00132 #define KOALA_WHEELS_DISTANCE 250.0 // distance between wheel, for rotation calculus [mm] 00133 00134 #define KOALA_WHEEL_DIAM 82.5 // wheel diameter [mm] (real 85; without load 82.5, with 3kg load: 80.5) 00135 00136 #define KOALA_PULSE_TO_MM (M_PI*KOALA_WHEEL_DIAM/23400.0) // motor position factor to convert from pulse to mm 00137 00138 #define KOALA_TIME_BTWN 10 // [ms] time for speed computation 00139 00140 #define KOALA_SPEED_TO_MM_S (KOALA_PULSE_TO_MM/(KOALA_TIME_BTWN/1000.0)) // motor speed factor to convert from speed units to mm/s 00141 00142 #define KOALA_US_DISABLED_SENSOR 2000 // disabled sensor 00143 #define KOALA_US_NO_OBJECT_IN_RANGE 1000 // no object in range 25..250cm 00144 #define KOALA_US_OBJECT_NEAR 0 // object at less 25cm 00145 00146 // default motor parameters 00147 #define KOALA_MOTOR_P 10 00148 #define KOALA_MOTOR_I 3 00149 #define KOALA_MOTOR_D 1 00150 #define KOALA_MOTOR_ACC_INC 5 00151 #define KOALA_MOTOR_ACC_DIV 1 00152 #define KOALA_MOTOR_MIN_SPACC 10 00153 #define KOALA_MOTOR_CST_SPEED 200 00154 #define KOALA_MOTOR_POS_MARGIN 10 00155 #define KOALA_MOTOR_MAX_CURRENT 10 00156 00157 00158 #define KOALA_AD_TO_V (3.3/1024) // convert AD valu to Volt 00159 00164 #define KOALA_RS232_MESSAGE_ERROR_CHAR '#' 00165 00171 // koala struct type for gps data 00172 typedef struct gps_data_s 00173 { 00174 char valid_sat; // valid data flag 00175 int sat_nb; // number of satellites used 00176 double lat_val; // latitude 00177 char lat_car; // latitude N or S 00178 double long_val; // longitude 00179 char long_car; // longitude W or E 00180 struct tm date_time; // UTC date and time of the last fix 00181 double speed; // speed in knots 00182 int altitude; // altitude in meter 00183 } gps_data_t; 00184 00185 // koala struct type for auto mode 00186 typedef struct auto_struct_s 00187 { 00188 // for speed, current, position 00189 int left_speed; // motor left speed 00190 int right_speed; // motor right speed 00191 int left_position; // motor left position 00192 int right_position; // motor right position 00193 int left_current; // motor left current 00194 int right_current; // motor right current 00195 int us[KOALA_US_SENSORS_NUMBER]; // us sensors 00196 int accel[KOALA_ACCEL_VALUES_NUMBER]; // accelerometer 00197 int gyro[KOALA_GYRO_VALUES_NUMBER]; // gyrometer 00198 int magne[KOALA_MAGNE_VALUES_NUMBER]; // magnometer 00199 char gps_raw[KOALA_MAX_BUFFER]; 00200 char mode; // type of the data received 00201 gps_data_t gps; 00202 } koala_auto_data_t; 00203 00204 00210 // Is called by koala_init 00211 extern int koala_robot_init( void ); 00212 extern int koala_robot_release( void ); 00213 00214 00215 // "Low level" function to communicate with the KOALA via serial 00216 extern int koala_sendcommand(char *command,int write_len); 00217 extern int koala_getcommand(char *command,int read_len); 00218 extern int koala_getcommand_line(char *command); 00219 00220 // "High level" function that let user to simply retrieve various informations from the robot 00221 extern int koala_get_firmware_version_revision(char *version,char *revision); 00222 00223 extern int koala_get_battery_data(int *bat_type,int *bat_voltage, int *bat_current,int *chrg_current); 00224 extern int koala_configure_auto_monitoring_mode(unsigned int bit_config); 00225 extern int koala_get_from_auto_mode(koala_auto_data_t *data); 00226 extern int koala_configure_us_io(int us_mask,unsigned char io_dir); 00227 00228 extern int koala_configure_pid(int kp, int ki,int kd); 00229 extern int koala_set_speed_profile(int acc_inc,int acc_div,int min_speed,int cst_speed,int pos_margin,int max_current); 00230 extern int koala_set_position_encoders(int left, int right); 00231 00232 00233 extern int koala_set_motor_speed(int left, int right); 00234 extern int koala_set_motor_speed_accel(int left, int right); 00235 extern int koala_set_motor_speed_open_loop(int left, int right); 00236 extern int koala_read_motor_speed(int *left, int *right); 00237 extern int koala_set_motor_target_position(int left, int right); 00238 extern int koala_read_motor_current(int *left, int *right); 00239 extern int koala_read_motor_position(int *left, int *right); 00240 int koala_get_motor_status(int *left_status, int *right_status,int *left_pos, int *right_pos); 00241 00242 extern int koala_read_us_sensors(int *values_array); 00243 extern int koala_read_accelerometer(int *values_array); 00244 extern int koala_read_gyroscope(int *values_array); 00245 extern int koala_read_magnetometer(int *values_array); 00246 00247 extern int koala_gps_data(char *valid_sat, int *sat_nb,double *lat_val,char *lat_car,double *long_val,char *long_car,struct tm *date_time,double *speed,int *altitude); 00248 extern int koala_send_gps_cmd(char *gps_cmd); 00249 00250 extern int koala_read_i2c(int i2c_add,int i2c_reg, int nb_read,int *data); 00251 extern int koala_write_i2c(int i2c_add,int i2c_reg,int nb_data,int *data); 00252 extern int koala_scan_i2c(int *nb_devices,int *address); 00253 00254 extern int koala_set_pwr_io_output(int power_out,int IO0, int IO1, int IO2, int IO3); 00255 extern int koala_read_io(int *io_state, int *in_state); 00256 extern int koala_read_ad(int *ad_0, int *ad_1); 00257 00258 extern int koala_reset_microcontroller(); 00259 00260 #endif /* __koala_robot__ */