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 00036 /* ---- Include Files ---------------------------------------------------- */ 00037 #include "koala.h" 00038 00039 #include <stdio.h> 00040 #include <string.h> 00041 #include <errno.h> 00042 00043 /* ---- Private Constants and Types -------------------------------------- */ 00044 00045 koala_rs232_t *koala_rs232; // serial port device 00046 00047 00048 /*---- private Functions --------------------------------------------------*/ 00049 00061 int process_param(char *source,int *destination,int nb_arg) 00062 { 00063 int narg=0; 00064 char *pch; 00065 00066 pch = strtok (source,KOALA_DELIM); 00067 while (pch != NULL) 00068 { 00069 if (sscanf(pch,"%d",&destination[narg])!=1) 00070 return -1; 00071 00072 pch = strtok (NULL, KOALA_DELIM); 00073 narg++; 00074 00075 if (narg == nb_arg) 00076 break; 00077 } 00078 00079 return 0; 00080 } 00081 00082 /* ---- Exported Functions------------------------------------------------ */ 00083 00084 00085 const char *KOALA_US_SENSOR_NAMES[]= {"Left rear","Left front","Front left","Front","Front right","Right front","Right rear","Back right","Back left"}; 00086 00098 int koala_robot_init( void ) 00099 { 00100 char version; 00101 int rc; 00102 00103 // open serial port port for communicating with the robot 00104 if ((koala_rs232=koala_rs232_open(KOALA_SERIAL_PORT_NAME,KOALA_SERIAL_PORT_BAUDRATE))== NULL) 00105 { 00106 //printf("\n libkoala ERROR: could not open serial port!\n\n"); 00107 return -1; 00108 } 00109 00110 return 0; 00111 } 00112 00113 00124 int koala_robot_release( void ) 00125 { 00126 koala_rs232_close(koala_rs232); 00127 return 0; 00128 } 00129 00145 int koala_getcommand(char *command,int read_len) 00146 { 00147 int rc=0; 00148 00149 rc=koala_rs232_read(koala_rs232, command ,read_len ); 00150 00151 return rc; 00152 } 00153 00168 int koala_getcommand_line(char *command) 00169 { 00170 int rc; 00171 00172 rc=koala_rs232_readLine(koala_rs232,command); 00173 00174 return rc; 00175 } 00176 00177 00178 00179 int koala_getcommand_line_nowait(char *command) 00180 00181 { 00182 int rc; 00183 00184 rc=koala_rs232_readLine_nowait(koala_rs232,command); 00185 00186 return rc; 00187 } 00188 00204 int koala_sendcommand(char *command,int send_len) 00205 { 00206 int rc=0; 00207 00208 rc=koala_rs232_write(koala_rs232,command,send_len); 00209 00210 return rc; 00211 } 00212 00232 int koala_configure_auto_monitoring_mode(unsigned int bit_config) 00233 { 00234 char buffer[KOALA_MAX_BUFFER]; 00235 int rc=0; 00236 00237 sprintf(buffer,"A,%d\r",bit_config); 00238 00239 koala_sendcommand(buffer,strlen(buffer)); 00240 00241 koala_getcommand_line(buffer); 00242 00243 if (buffer[0] != 'a') 00244 return -1; 00245 00246 return 0; 00247 } 00248 00258 int koala_get_firmware_version_revision(char *version,char *revision) 00259 { 00260 int rc=0; 00261 char buffer[7]; 00262 00263 buffer[0]='B'; 00264 buffer[1]='\r'; 00265 00266 koala_sendcommand(buffer,2); 00267 rc=koala_getcommand(buffer,7); 00268 00269 if (version == NULL) 00270 return -1; 00271 00272 if (revision == NULL) 00273 return -2; 00274 00275 if (buffer[0]!='b') 00276 return -3; 00277 00278 if (rc!=7) 00279 return -4; 00280 00281 *version=buffer[2]; 00282 00283 *revision=atoi(buffer+4); 00284 00285 return 0; 00286 } 00287 00316 int koala_configure_us_io(int us_mask,unsigned char io_dir) 00317 { 00318 char buffer[KOALA_MAX_BUFFER]; 00319 int rc=0; 00320 00321 sprintf(buffer,"C,%d,%d\r",us_mask,io_dir); 00322 00323 koala_sendcommand(buffer,strlen(buffer)); 00324 00325 koala_getcommand_line(buffer); 00326 00327 if (buffer[0] != 'c') 00328 return -1; 00329 00330 return 0; 00331 } 00332 00333 00343 int koala_set_motor_speed(int left, int right) 00344 { 00345 char buffer[KOALA_MAX_BUFFER]; 00346 int rc=0; 00347 00348 sprintf(buffer,"D,%d,%d\r",left,right); 00349 00350 koala_sendcommand(buffer,strlen(buffer)); 00351 00352 koala_getcommand_line(buffer); 00353 00354 if (buffer[0] != 'd') 00355 return -1; 00356 00357 return 0; 00358 } 00359 00369 int koala_read_motor_speed(int *left, int *right) 00370 { 00371 char buffer[KOALA_MAX_BUFFER]; 00372 int rc=0; 00373 00374 buffer[0]='E'; 00375 buffer[1]='\r'; 00376 00377 koala_sendcommand(buffer,2); 00378 rc=koala_getcommand_line(buffer); 00379 00380 if (buffer[0]!='e') 00381 return -1; 00382 00383 if (sscanf(buffer,"%*c,%d,%d",left,right)!=2) 00384 return -2; 00385 00386 return 0; 00387 } 00388 00398 int koala_set_motor_target_position(int left, int right) 00399 { 00400 char buffer[KOALA_MAX_BUFFER]; 00401 int rc=0; 00402 00403 sprintf(buffer,"F,%d,%d\r",left,right); 00404 00405 koala_sendcommand(buffer,strlen(buffer)); 00406 00407 koala_getcommand_line(buffer); 00408 00409 if (buffer[0] != 'f') 00410 return -1; 00411 00412 return 0; 00413 } 00414 00427 int koala_read_us_sensors(int *values_array) 00428 { 00429 char buffer[KOALA_MAX_BUFFER]; 00430 int rc=0; 00431 int narg=0; 00432 char * pch; 00433 00434 buffer[0]='G'; 00435 buffer[1]='\r'; 00436 00437 koala_sendcommand(buffer,2);; 00438 rc=koala_getcommand_line(buffer); 00439 00440 if (buffer[0]!='g') 00441 return -1; 00442 00443 if (process_param(buffer+2,values_array,KOALA_US_SENSORS_NUMBER)!=0) 00444 return -2; 00445 00446 return 0; 00447 } 00448 00449 00459 int koala_read_magnetometer(int *values_array) 00460 { 00461 char buffer[KOALA_MAX_BUFFER]; 00462 int rc=0; 00463 int narg=0; 00464 char * pch; 00465 00466 buffer[0]='&'; 00467 buffer[1]='\r'; 00468 00469 koala_sendcommand(buffer,2);; 00470 rc=koala_getcommand_line(buffer); 00471 00472 if (buffer[0]!='@') 00473 return -1; 00474 00475 if (process_param(buffer+2,values_array,KOALA_MAGNE_VALUES_NUMBER)!=0) 00476 return -2; 00477 00478 return 0; 00479 } 00480 00491 int koala_configure_pid(int kp, int ki,int kd) 00492 { 00493 char buffer[KOALA_MAX_BUFFER]; 00494 int rc=0; 00495 00496 sprintf(buffer,"H,%d,%d,%d\r",kp,ki,kd); 00497 00498 koala_sendcommand(buffer,strlen(buffer)); 00499 00500 koala_getcommand_line(buffer); 00501 00502 if (buffer[0] != 'h') 00503 return -1; 00504 00505 return 0; 00506 } 00507 00521 int koala_set_position_encoders(int left, int right) 00522 { 00523 char buffer[KOALA_MAX_BUFFER]; 00524 int rc=0; 00525 00526 sprintf(buffer,"I,%d,%d\r",left,right); 00527 00528 koala_sendcommand(buffer,strlen(buffer)); 00529 00530 koala_getcommand_line(buffer); 00531 00532 if (buffer[0] != 'i') 00533 return -1; 00534 00535 return 0; 00536 } 00537 00553 int koala_set_speed_profile(int acc_inc,int acc_div,int min_speed,int cst_speed,int pos_margin,int max_current) 00554 { 00555 char buffer[KOALA_MAX_BUFFER]; 00556 int rc=0; 00557 00558 sprintf(buffer,"J,%d,%d,%d,%d,%d,%d\r",acc_inc,acc_div,min_speed,cst_speed,pos_margin,max_current); 00559 00560 koala_sendcommand(buffer,strlen(buffer)); 00561 00562 koala_getcommand_line(buffer); 00563 00564 if (buffer[0] != 'j') 00565 return -1; 00566 00567 return 0; 00568 } 00569 00582 int koala_set_motor_speed_accel(int left, int right) 00583 { 00584 char buffer[KOALA_MAX_BUFFER]; 00585 int rc=0; 00586 00587 sprintf(buffer,"K,%d,%d\r",left,right); 00588 00589 koala_sendcommand(buffer,strlen(buffer)); 00590 00591 koala_getcommand_line(buffer); 00592 00593 if (buffer[0] != 'k') 00594 return -1; 00595 00596 return 0; 00597 } 00598 00610 int koala_set_motor_speed_open_loop(int left, int right) 00611 { 00612 char buffer[KOALA_MAX_BUFFER]; 00613 int rc=0; 00614 00615 sprintf(buffer,"L,%d,%d\r",left,right); 00616 00617 koala_sendcommand(buffer,strlen(buffer)); 00618 00619 koala_getcommand_line(buffer); 00620 00621 if (buffer[0] != 'l') 00622 return -1; 00623 00624 return 0; 00625 } 00626 00635 int koala_read_accelerometer(int *values_array) 00636 { 00637 char buffer[KOALA_MAX_BUFFER]; 00638 int rc=0,i; 00639 00640 buffer[0]='M'; 00641 buffer[1]='\r'; 00642 00643 koala_sendcommand(buffer,2); 00644 rc=koala_getcommand_line(buffer); 00645 00646 if (buffer[0]!='m') 00647 return -1; 00648 00649 if (process_param(buffer+2,values_array,KOALA_ACCEL_VALUES_NUMBER)!=0) 00650 return -2; 00651 00652 00653 return 0; 00654 } 00655 00664 int koala_read_gyroscope(int *values_array) 00665 { 00666 char buffer[KOALA_MAX_BUFFER]; 00667 int rc=0,i; 00668 00669 buffer[0]='N'; 00670 buffer[1]='\r'; 00671 00672 koala_sendcommand(buffer,2); 00673 rc=koala_getcommand_line(buffer); 00674 00675 if (buffer[0]!='n') 00676 return -1; 00677 00678 if (process_param(buffer+2,values_array,KOALA_GYRO_VALUES_NUMBER)!=0) 00679 return -2; 00680 00681 return 0; 00682 } 00683 00693 int koala_read_motor_current(int *left, int *right) 00694 { 00695 char buffer[KOALA_MAX_BUFFER]; 00696 int rc=0; 00697 00698 buffer[0]='O'; 00699 buffer[1]='\r'; 00700 00701 koala_sendcommand(buffer,2); 00702 rc=koala_getcommand_line(buffer); 00703 00704 if (buffer[0]!='o') 00705 return -1; 00706 00707 if (sscanf(buffer,"%*c,%d,%d",left,right)!=2) 00708 return -2; 00709 00710 return 0; 00711 } 00712 00722 int koala_read_motor_position(int *left, int *right) 00723 { 00724 char buffer[KOALA_MAX_BUFFER]; 00725 int rc=0; 00726 00727 buffer[0]='P'; 00728 buffer[1]='\r'; 00729 00730 koala_sendcommand(buffer,2); 00731 rc=koala_getcommand_line(buffer); 00732 00733 if (buffer[0]!='p') 00734 return -1; 00735 00736 if (sscanf(buffer,"%*c,%d,%d",left,right)!=2) 00737 return -2; 00738 00739 return 0; 00740 } 00741 00742 00760 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) 00761 { 00762 char buffer[KOALA_MAX_BUFFER]; 00763 int rc=0,i; 00764 char *bptr; 00765 int narg; 00766 00767 buffer[0]='Q'; 00768 buffer[1]='\r'; 00769 00770 koala_sendcommand(buffer,2); 00771 rc=koala_getcommand_line(buffer); 00772 00773 if (buffer[0]!='q') 00774 return -1; 00775 00776 if (sscanf(buffer,"%*c,%c,%d,%lf%c,%lf%c,%d:%d:%d,%d.%d.%d,%lf,%d",valid_sat,sat_nb,lat_val,lat_car,long_val,long_car,&date_time->tm_hour,&date_time->tm_min,&date_time->tm_sec,&date_time->tm_mday,&date_time->tm_mon,&date_time->tm_year,speed,altitude)!=14) 00777 return -2; 00778 00779 00780 return 0; 00781 } 00782 00783 00795 int koala_read_i2c(int i2c_add,int i2c_reg, int nb_read,int *data) 00796 { 00797 char buffer[KOALA_MAX_BUFFER]; 00798 int rc=0; 00799 int narg=0; 00800 char *pch; 00801 00802 sprintf(buffer,"R,%d,%d,%d\r",i2c_add,i2c_reg,nb_read); 00803 koala_sendcommand(buffer,strlen(buffer)); 00804 rc=koala_getcommand_line(buffer); 00805 00806 if (buffer[0]!='r') 00807 return -1; 00808 00809 00810 pch = strtok (buffer+2,KOALA_DELIM); 00811 while (pch != NULL) 00812 { 00813 00814 if (sscanf(pch,"%d",&data[narg])!=1) 00815 return -2; 00816 00817 pch = strtok (NULL, KOALA_DELIM); 00818 narg++; 00819 00820 if (narg == KOALA_MAX_I2C_DATA) 00821 break; 00822 00823 if (narg==nb_read) 00824 break; 00825 } 00826 00827 if (narg!=nb_read) 00828 return -3; 00829 00830 return 0; 00831 } 00832 00833 00851 int koala_set_pwr_io_output(int power_out,int IO0, int IO1, int IO2, int IO3) 00852 { 00853 char buffer[KOALA_MAX_BUFFER]; 00854 int rc=0; 00855 00856 sprintf(buffer,"S,%d,%d,%d,%d,%d\r",power_out,IO0,IO1,IO2,IO3); 00857 00858 koala_sendcommand(buffer,strlen(buffer)); 00859 00860 koala_getcommand_line(buffer); 00861 00862 if (buffer[0] != 's') 00863 return -1; 00864 00865 return 0; 00866 } 00867 00881 int koala_read_io(int *io_state, int *in_state) 00882 { 00883 char buffer[KOALA_MAX_BUFFER]; 00884 int rc=0; 00885 00886 buffer[0]='T'; 00887 buffer[1]='\r'; 00888 00889 koala_sendcommand(buffer,2); 00890 rc=koala_getcommand_line(buffer); 00891 00892 if (buffer[0]!='t') 00893 return -1; 00894 00895 if (sscanf(buffer,"%*c,%d,%d",io_state,in_state)!=2) 00896 return -2; 00897 00898 return 0; 00899 } 00900 00910 int koala_read_ad(int *ad_0, int *ad_1) 00911 { 00912 char buffer[KOALA_MAX_BUFFER]; 00913 int rc=0; 00914 00915 buffer[0]='U'; 00916 buffer[1]='\r'; 00917 00918 koala_sendcommand(buffer,2); 00919 rc=koala_getcommand_line(buffer); 00920 00921 if (buffer[0]!='u') 00922 return -1; 00923 00924 if (sscanf(buffer,"%*c,%d,%d",ad_0,ad_1)!=2) 00925 return -2; 00926 00927 return 0; 00928 } 00929 00941 int koala_get_battery_data(int *bat_type,int *bat_voltage, int *bat_current, int *chrg_current) 00942 { 00943 00944 char buffer[KOALA_MAX_BUFFER]; 00945 int rc=0; 00946 00947 buffer[0]='V'; 00948 buffer[1]='\r'; 00949 00950 koala_sendcommand(buffer,2); 00951 rc=koala_getcommand_line(buffer); 00952 00953 if (buffer[0]!='v') 00954 return -1; 00955 00956 if (sscanf(buffer,"%*c,%d,%d,%d,%d",bat_type,bat_voltage,bat_current,chrg_current)!=4) 00957 return -2; 00958 00959 return 0; 00960 } 00961 00973 int koala_write_i2c(int i2c_add,int i2c_reg,int nb_data,int *data) 00974 { 00975 char buffer[KOALA_MAX_BUFFER]; 00976 char cdata[8]; 00977 int rc=0,i; 00978 00979 // test buffer space 00980 if (nb_data>((KOALA_MAX_BUFFER-10)/4)) 00981 return -1; 00982 00983 sprintf(buffer,"W,%d,%d",i2c_add,i2c_reg); 00984 00985 for (i=0;i<nb_data;i++) 00986 { 00987 sprintf(cdata,",%d",data[i]); 00988 strcat(buffer,cdata); 00989 } 00990 00991 strcat(buffer,"\r"); 00992 00993 koala_sendcommand(buffer,strlen(buffer)); 00994 00995 koala_getcommand_line(buffer); 00996 00997 if (buffer[0]!='w') 00998 return -2; 00999 01000 01001 return 0; 01002 } 01003 01012 int koala_send_gps_cmd(char *gps_cmd) 01013 { 01014 char buffer[KOALA_MAX_BUFFER]; 01015 01016 if (strlen(gps_cmd)>KOALA_MAX_BUFFER-4) 01017 return -1; 01018 01019 sprintf(buffer,"X,%s\r",gps_cmd); 01020 01021 koala_sendcommand(buffer,strlen(buffer)); 01022 01023 koala_getcommand_line(buffer); 01024 01025 if (buffer[0]!='x') 01026 return -2; 01027 01028 return 0; 01029 } 01030 01053 int koala_get_motor_status(int *left_status, int *right_status,int *left_pos, int *right_pos) 01054 { 01055 01056 char buffer[KOALA_MAX_BUFFER]; 01057 int rc=0; 01058 01059 buffer[0]='Y'; 01060 buffer[1]='\r'; 01061 01062 koala_sendcommand(buffer,2); 01063 rc=koala_getcommand_line(buffer); 01064 01065 if (buffer[0]!='y') 01066 return -1; 01067 01068 if (sscanf(buffer,"%*c,%d,%d,%d,%d",left_status,right_status,left_pos,right_pos)!=4) 01069 return -2; 01070 01071 return 0; 01072 } 01073 01081 int koala_reset_microcontroller() 01082 { 01083 char buffer[KOALA_MAX_BUFFER]; 01084 01085 sprintf(buffer,"Z\r"); 01086 01087 koala_sendcommand(buffer,strlen(buffer)); 01088 01089 koala_getcommand_line(buffer); 01090 01091 if (buffer[0]!='z') 01092 return -1; 01093 01094 return 0; 01095 } 01096 01105 #define MAX_I2C_ADDRESS 128 01106 int koala_scan_i2c(int *nb_devices,int *address) 01107 { 01108 char buffer[KOALA_MAX_BUFFER]; 01109 int narg=0,nb; 01110 char delim[2]= {',' , '\0'}; 01111 char * pch; 01112 01113 *nb_devices=0; 01114 01115 sprintf(buffer,"?\r"); 01116 01117 koala_sendcommand(buffer,strlen(buffer)); 01118 01119 koala_getcommand_line(buffer); 01120 01121 if (buffer[0]!='!') 01122 return -1; 01123 01124 01125 pch = strtok (buffer+2,delim); 01126 01127 if (pch == NULL) 01128 return -2; 01129 01130 if (sscanf(pch,"%d",nb_devices)!=1) 01131 return -3; 01132 01133 pch = strtok (NULL, delim); 01134 01135 while (pch != NULL) 01136 { 01137 01138 if (sscanf(pch,"%d",&address[narg])!=1) 01139 return -4; 01140 01141 pch = strtok (NULL, delim); 01142 narg++; 01143 01144 if (narg == MAX_I2C_ADDRESS) 01145 break; 01146 } 01147 01148 if (narg != *nb_devices) 01149 return -5; 01150 01151 return 0; 01152 } 01153 01154 01164 int koala_get_from_auto_mode(koala_auto_data_t *data) 01165 { 01166 char buffer[KOALA_MAX_BUFFER]; 01167 int rc=0; 01168 char *pch; 01169 int narg; 01170 01171 rc=koala_getcommand_line_nowait(buffer); 01172 01173 if (rc<=0) 01174 { 01175 return -1; 01176 } 01177 01178 data->mode=buffer[0]; 01179 01180 switch(buffer[0]) 01181 { 01182 case 'e' : // Motor Speed 01183 if (sscanf(buffer,"%*c,%d,%d",&data->left_speed,&data->right_speed)!=2) 01184 return -2; 01185 break; 01186 case 'g' : // US sensor 01187 if (process_param(buffer+2,data->us,KOALA_US_SENSORS_NUMBER)!=0) 01188 return -3; 01189 break; 01190 case 'm' : // Accelerometer value 01191 if (process_param(buffer+2,data->accel,KOALA_ACCEL_VALUES_NUMBER)!=0) 01192 return -4; 01193 break; 01194 case 'n' : // Gyroscope value 01195 if (process_param(buffer+2,data->gyro,KOALA_GYRO_VALUES_NUMBER)!=0) 01196 return -5; 01197 break; 01198 case 'o' : // Motor Current 01199 if (sscanf(buffer,"%*c,%d,%d",&data->left_current,&data->right_current)!=2) 01200 return -6; 01201 break; 01202 case 'p' : // Motor Position 01203 if (sscanf(buffer,"%*c,%d,%d",&data->left_position,&data->right_position)!=2) 01204 return -7; 01205 break; 01206 case 'q' : // GPS data 01207 if (sscanf(buffer,"%*c,%c,%d,%lf%c,%lf%c,%d:%d:%d,%d.%d.%d,%lf,%d",&(data->gps.valid_sat),&(data->gps.sat_nb),&(data->gps.lat_val),&(data->gps.lat_car),&(data->gps.long_val),&(data->gps.long_car),&(data->gps.date_time.tm_hour),&(data->gps.date_time.tm_min),&(data->gps.date_time.tm_sec),&(data->gps.date_time.tm_mday),&(data->gps.date_time.tm_mon),&(data->gps.date_time.tm_year),&(data->gps.speed),&(data->gps.altitude))!=14) 01208 return -8; 01209 break; 01210 case '$' : // GPS raw data 01211 strcpy(data->gps_raw,buffer); 01212 break; 01213 case '@' : // GPS raw data 01214 if (process_param(buffer+2,data->magne,KOALA_MAGNE_VALUES_NUMBER)!=0) 01215 return -9; 01216 break; 01217 default: 01218 return -10; 01219 01220 } 01221 01222 return 0; 01223 }