00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <korebot/korebot.h>
00020
00021 static knet_dev_t * dsPic;
00022 static knet_dev_t * mot1;
00023 static knet_dev_t * mot2;
00024
00025
00026
00027 #define PULSE_TO_MM_FIRMWARE_S_3 0.04629 // for version <3.0
00028 #define PULSE_TO_MM_FIRMWARE_BE_3 0.03068
00029
00030
00031 #define MM_S_TO_SPEED_FIRMWARE_S_3 144.01 // for version <3.0
00032 #define MM_S_TO_SPEED_FIRMWARE_BE_3 218.72
00033
00034
00035 #define IR_BAR_LEN 15
00036
00037 int main()
00038 {
00039
00040 float fpos;
00041 long lpos,rpos;
00042 char Buffer[100],bar[11][IR_BAR_LEN+5];
00043 int sensors[11],i,n,revision,version;
00044 short index, value;
00045 char usvalues[5];
00046 long motspeed;
00047
00048 float pulsestomm=PULSE_TO_MM_FIRMWARE_S_3 ;
00049 float mmstospeed=MM_S_TO_SPEED_FIRMWARE_S_3 ;
00050
00051 kh3_init();
00052
00053
00054 dsPic = knet_open( "Khepera3:dsPic" , KNET_BUS_I2C , 0 , NULL );
00055 mot1 = knet_open( "Khepera3:mot1" , KNET_BUS_I2C , 0 , NULL );
00056 mot2 = knet_open( "Khepera3:mot2" , KNET_BUS_I2C , 0 , NULL );
00057
00058
00059 kmot_SetMode( mot1 , kMotModeIdle );
00060 kmot_SetSampleTime( mot1 , 1550 );
00061 kmot_SetMargin( mot1 , 6 );
00062 kmot_SetOptions( mot1 , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk
00063 | kMotSWOptDirectionInv );
00064 kmot_ResetError( mot1 );
00065 kmot_SetBlockedTime( mot1 , 10);
00066 kmot_ConfigurePID( mot1 , kMotRegSpeed , 620 , 3 , 10 );
00067 kmot_ConfigurePID( mot1 ,kMotRegPos,600,20,30);
00068 kmot_SetSpeedProfile(mot1 ,15000,30);
00069
00070
00071 kmot_SetMode( mot2 , kMotModeIdle );
00072 kmot_SetSampleTime( mot2 , 1550 );
00073 kmot_SetMargin( mot2 , 6 );
00074 kmot_SetOptions( mot2 , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00075 kmot_ResetError( mot2 );
00076 kmot_SetBlockedTime( mot2 , 10);
00077 kmot_ConfigurePID( mot2 , kMotRegSpeed , 620 , 3 , 10 );
00078 kmot_ConfigurePID( mot2 ,kMotRegPos,600,20,30);
00079 kmot_SetSpeedProfile(mot2 ,15000,30);
00080
00081
00082 if(kh3_revision((char *)Buffer, dsPic)){
00083
00084
00085 version=(Buffer[1] | Buffer[2]<<8);
00086 revision=(Buffer[3] | Buffer[4]<<8);
00087
00088 printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
00089 Buffer[0],version ,revision,version ,revision);
00090
00091
00092
00093 if (version <3)
00094 {
00095 pulsestomm=PULSE_TO_MM_FIRMWARE_S_3;
00096 mmstospeed=MM_S_TO_SPEED_FIRMWARE_S_3;
00097 }
00098 else
00099 {
00100 pulsestomm=PULSE_TO_MM_FIRMWARE_BE_3;
00101 mmstospeed=MM_S_TO_SPEED_FIRMWARE_BE_3;
00102 }
00103
00104 }
00105
00106
00107 if(kh3_battery_voltage((char *)Buffer, 0, dsPic)){
00108 printf("\r\n%c,%3.3u,%3.3u => battery voltage = %u.%uV\r\n",
00109 Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
00110 (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
00111 }
00112
00113
00114
00115 while(1)
00116 {
00117
00118 sleep(2);
00119
00120
00121
00122
00123 lpos = kmot_GetMeasure(mot1, kMotRegPos);
00124 rpos = kmot_GetMeasure(mot2, kMotRegPos);
00125
00126 fpos = 20.0*10.0/pulsestomm;
00127 printf("\nMoving forward %.1f cm in %.1f pulses with position control (encoders: left %ld | right%ld)\n",20.0,fpos,lpos+(long)fpos,rpos+(long)fpos);
00128
00129
00130 kmot_SetPoint(mot1, kMotRegPosProfile, lpos+(long)fpos);
00131 kmot_SetPoint(mot2, kMotRegPosProfile, rpos+(long)fpos);
00132
00133
00134 printf("\n wait 10s \n");
00135 sleep(10);
00136
00137
00138
00139 motspeed= (long)(-40.0*mmstospeed);
00140 kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
00141 kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
00142
00143 printf("\nMoving backward %.1f cm at %.1f mm/s (internal speed %d) with speed control\n",20.0,40.0,motspeed);
00144
00145
00146 sleep(5);
00147
00148
00149 kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
00150 kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
00151
00152
00153 sleep(5);
00154
00155
00156
00157 if(kh3_proximity_ir((char *)Buffer, dsPic))
00158 {
00159 for (i=0;i<11;i++)
00160 {
00161 sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
00162
00163 n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
00164
00165 if (n==0)
00166 sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
00167 else
00168 if (n>=IR_BAR_LEN-1)
00169 sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
00170 else
00171 sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
00172
00173 }
00174 printf("\n near far\nback left : %4.4u %s\nleft 90 : %4.4u %s\
00175 \nleft 45 : %4.4u %s\nfront left : %4.4u %s\nfront right : %4.4u %s\
00176 \nright 45 : %4.4u %s\nright 90 : %4.4u %s\nback right : %4.4u %s\
00177 \nback : %4.4u %s\nbottom right: %4.4u %s\nbottom left : %4.4u %s\ntime stamp : %lu\r\n",
00178 sensors[0],bar[0], sensors[1],bar[1],
00179 sensors[2],bar[2], sensors[3],bar[3],
00180 sensors[4],bar[4], sensors[5],bar[5],
00181 sensors[6],bar[6], sensors[7],bar[7],
00182 sensors[8], bar[8], sensors[9], bar[9],sensors[10], bar[10],
00183 ((Buffer[19] | Buffer[20]<<8) | (Buffer[21] | Buffer[22]<<8)<<16));
00184 }
00185
00186
00187
00188 index=0;
00189 value=31;
00190
00191 if(kh3_configure_os((char *)Buffer, index, value, dsPic))
00192 printf("\r\n US sensors configured.\r\n");
00193 else
00194 printf("\r\nconfigure OS error!\r\n");
00195
00196
00197 for (i=0;i<5;i++)
00198 {
00199
00200 if(kh3_measure_us((char *)Buffer, i+1, dsPic))
00201 {
00202 usvalues[i] = (Buffer[0*8+3] | Buffer[0*8+4]<<8);
00203 }
00204 else
00205 printf("\r\ng, error...");
00206 }
00207
00208 printf("US sensors : distance [cm]\nleft 90 : %3d\nleft 45 : %3d \
00209 \nfront : %3d\nright 45 : %3d\nright 90 : %3d\n",
00210 usvalues[0],usvalues[1],usvalues[2],usvalues[3],usvalues[4]);
00211 }
00212 }