kurt_base: kurt_base: countticks.cc Source File

00001 #include <cstdio>
00002 #include <cstdlib>
00003 
00004 #include "kurt.h"
00005 #include "stdoutcomm.h"
00006 #include "mytime.h"
00007 
00008 void usage(char *pgrname)
00009 {
00010   printf("%s: <pwml> <pwmr> <nr_turns> <nr_ticks> (radumdrehungen zaehlen)\n",pgrname);
00011   printf("e.g. %s 400 400 10 2050\n",pgrname);
00012 }
00013 
00014 int main(int argc, char **argv)
00015 {
00016   FILE *fpr = NULL;
00017   long double Time_Diff1 = 101.0; // time
00018   long double Time_Diffa = 0.0; // time
00019   long double Time_Diffb = 0.0; // time
00020   int i,j, k;
00021   int startpwml = 400, startpwmr = 400, nr_turns = 10, nr_ticks = 2050;
00022 
00023   if (argc < 5) {
00024     usage(argv[0]);
00025     return 0;
00026   }
00027   else {
00028     startpwml = atoi(argv[1]);
00029     startpwmr = atoi(argv[2]);
00030     nr_turns = atoi(argv[3]);
00031     nr_ticks = atoi(argv[4]);
00032   }
00033 
00034   fpr = fopen("distance-test.dat","w");
00035 
00036   mydelay(200);
00037 
00038   //Odometry parameter (defaults for kurt2 indoor)
00039   double wheel_perimeter = 0.379;
00040   double axis_length = 0.28;
00041 
00042   double turning_adaptation = 0.69;
00043   int ticks_per_turn_of_wheel = 21950;
00044 
00045   STDoutComm stdoutcomm;
00046   Kurt kurt(stdoutcomm, wheel_perimeter, axis_length, turning_adaptation, ticks_per_turn_of_wheel);
00047 
00048   Get_mtime_diff(2);
00049   Get_mtime_diff(3);
00050   Get_mtime_diff(4);
00051 
00052   i = startpwml;
00053   j = startpwmr;
00054 
00055   int pwm_left, pwm_right;
00056   do {
00057 
00058     // speed auf die motoren
00059     pwm_left = 1024-i;
00060     pwm_right = 1024-j;
00061     Time_Diff1 += Get_mtime_diff(3);
00062     if (Time_Diff1 > 100.0) {
00063       kurt.can_motor(pwm_left, 0, 0, pwm_right, 0, 0);
00064       Time_Diff1 = 0.0;
00065     }
00066     //printf("out %d\n",i);
00067     // werte von den winkel encodern
00068     while (kurt.can_read_fifo() != CAN_ENCODER) ;
00069 
00070     fprintf(fpr,"%Lf %lf %lf %lf %lld %lld\n",
00071         Get_mtime_diff(9),
00072         stdoutcomm.v1(),stdoutcomm.v1_left(),stdoutcomm.v1_right(),
00073         stdoutcomm.K_get_sum_ticks_a(),
00074         stdoutcomm.K_get_sum_ticks_b());
00075 
00076     // xx umdrehungen pro rad zaehlen
00077     if (abs(stdoutcomm.K_get_sum_ticks_a()) > nr_ticks*nr_turns-200) {
00078       Time_Diffa += Get_mtime_diff(2);
00079       if ( i > 0) {
00080         i = 0;
00081         pwm_left = 1024-i;
00082         kurt.can_motor(pwm_left, 0, 0, pwm_right, 0, 0);
00083       }
00084 
00085     }
00086     else {
00087       Get_mtime_diff(2);
00088       Time_Diffa = 0.0;
00089     }
00090     // fuer kurt2 mit 90 watt motoren und 1:14 getriebe
00091     // encoder 500 oder 1000 / umdrehung
00092     // umsetzung kette ??
00093     if (abs(stdoutcomm.K_get_sum_ticks_b()) > nr_ticks*nr_turns-200) {
00094       Time_Diffb += Get_mtime_diff(4);
00095       if (j > 0) {
00096         j = 0;
00097         pwm_right = 1024-j;
00098         kurt.can_motor(pwm_left, 0, 0, pwm_right, 0, 0);
00099       }
00100 
00101     }
00102     else {
00103       Get_mtime_diff(4);
00104       Time_Diffb = 0.0;
00105     }
00106 
00107   } while ((Time_Diffa < 3000.0) || (Time_Diffb < 3000.0));
00108   fclose(fpr);
00109 
00110   // langsames stoppen ueber 1024 / 2 * 10 ms
00111   for (k = 0; k < i; k+=2 ) {
00112     pwm_left = 1024-i+k;
00113     pwm_right = 1024-j+k;
00114     kurt.can_motor(pwm_left, 0, 0, pwm_right, 0, 0);
00115     mydelay(10);
00116   }
00117 
00118   printf("Closing the can connection\n");
00119   printf("Bye Bye Spoki\n");
00120   return (0);
00121 }