speedtable.cc
Go to the documentation of this file.
00001 #include <cstdio>
00002 #include <cstdlib>
00003 #include <cstring>
00004 
00005 #include "kurt.h"
00006 #include "stdoutcomm.h"
00007 #include "mytime.h"
00008 
00009 void usage(char *pgrname)
00010 {
00011   printf("%s: <configfile> <start-pwm-value>\n",pgrname);
00012 }
00013 
00014 int main(int argc, char **argv)
00015 {
00016   FILE *fpr = NULL;
00017   long double Time_Diff1 = 1000.0; // time
00018   long double Time_Diff2 = 0.0; // time
00019   int i,j, k;
00020   double v1=0.0, prev_v1=0.0; // position update
00021   double v1_left = 0.0, v1_right=0.0;  // current speed
00022   double prev_v1_left=0.0, prev_v1_right=0.0;  // previous  speed
00023   int finish = 0;
00024   int startpwm = 1;
00025 
00026   if (argc == 3) {
00027     startpwm = atoi(argv[2]);
00028   }
00029   else {
00030     usage(argv[0]);
00031     return 0;
00032   }
00033 
00034   fpr = fopen(argv[1], "w");
00035 
00036   // fill with zeros until startpwm
00037   for (i = 1; i < startpwm;i++) {
00038     fprintf(fpr,"%Lf 0.0 0.0 0.0 %d\n",Get_mtime_diff(9), i);
00039   }
00040 
00041   //Odometry parameter (defaults for kurt2 indoor)
00042   double wheel_perimeter = 0.379;
00043   double axis_length = 0.28;
00044 
00045   double turning_adaptation = 0.69;
00046   int ticks_per_turn_of_wheel = 21950;
00047 
00048   STDoutComm stdoutcomm;
00049   Kurt kurt(stdoutcomm, wheel_perimeter, axis_length, turning_adaptation, ticks_per_turn_of_wheel);
00050 
00051   i = j = startpwm;
00052   Get_mtime_diff(3);
00053   Get_mtime_diff(2);
00054   int pwm_left, pwm_right;
00055   do {
00056     Time_Diff1 -= Get_mtime_diff(3);
00057     Time_Diff2 -= Get_mtime_diff(2);
00058     //printf("Hallo %Lf %Lf\n",Time_Diff1,Time_Diff2);
00059     if (Time_Diff2 <= 0.0) {
00060       Time_Diff2 = 100; // alle 100 ms senden sonst ausfall
00061       // speed auf die motoren
00062       pwm_left = 1024-i;
00063       pwm_right = 1024-j;
00064       kurt.can_motor(pwm_left, 0, 0, pwm_right, 0, 0);
00065       //        get_multimeter(0);
00066       if (i > 1024) {j  = i = 1024;finish = 1;}
00067     }
00068     // werte von den winkel encodern
00069     while (kurt.can_read_fifo() != CAN_ENCODER) ;
00070     v1 = stdoutcomm.v1();
00071     v1_left = stdoutcomm.v1_left();
00072     v1_right = stdoutcomm.v1_right();
00073 
00074     //  get_current_old(0);
00075     if (Time_Diff1 <= 0.0) { // jede sekunde neuer wert
00076 
00077       if ((v1_left < prev_v1_left)   || (prev_v1_left > 0.0  && 2.0 * prev_v1_left < v1_left)) {
00078         v1_left = prev_v1_left;
00079         if ((prev_v1_left > 0.0  && 2.0 * prev_v1_left < v1_left)) printf("ticks error left\n");
00080       }
00081 
00082       if ((v1_right < prev_v1_right) || (prev_v1_right > 0.0 && 2.0 * prev_v1_right < v1_right)) {
00083         v1_right = prev_v1_right;
00084         if ((prev_v1_right > 0.0 && 2.0 * prev_v1_right < v1_right)) printf("ticks error right \n");
00085       }
00086 
00087       if ((v1 < prev_v1) || (prev_v1 > 0.0 && 2.0 * prev_v1 < v1))
00088         v1 = prev_v1;
00089 
00090       //  get_multimeter(1);
00091       //  get_current_old(1);
00092       printf("t1: %Lf, %Lf, %Lf, v: %lf, vl: %lf, vr: %lf, pwm: %d\n",
00093           Get_mtime_diff(9), Time_Diff1, Time_Diff2,
00094           v1,
00095           v1_left,
00096           v1_right,
00097           i);
00098       fprintf(fpr,"%Lf %lf %lf %lf %d\n",
00099           Get_mtime_diff(9),
00100           v1,
00101           v1_left,
00102           v1_right,
00103           i);
00104       Time_Diff1 += 1000.0;
00105       Time_Diff2 = 0.0;
00106       i++; j++;
00107       prev_v1_left = v1_left;
00108       prev_v1_right = v1_right;
00109       prev_v1 = v1;
00110     }
00111   } while (finish == 0);
00112 
00113   i--;
00114   j--;
00115 
00116   // langsames stoppen ueber 1024 * 20 ms
00117   for (k = 0; k < i; k+=1 ) {
00118     pwm_left = 1024-i+k;
00119     pwm_right = 1024-j+k;
00120     kurt.can_motor(pwm_left, 0, 0, pwm_right, 0, 0);
00121 
00122     /*
00123     // werte von den winkel encodern
00124     if ((K_update_pose_from_encoder (0, &dx, &dy, &dtheta,
00125     &v1, &v1_left, &v1_right, &wL, &wR,
00126     Get_mtime_diff(9))) > 0) {
00127     fprintf(fpr,"%Lf %Lf %Lf %Lf %d\n",
00128     Get_mtime_diff(9),
00129     v1,
00130     v1_left,
00131     v1_right,
00132     i
00133     );
00134     }
00135     */
00136     mydelay(20);
00137   }
00138   fclose(fpr);
00139   printf("Closing the can connection\n");
00140   printf("Bye Bye Spoki\n");
00141   return (0);
00142 }


kurt_base
Author(s): Jochen Sprickerhof
autogenerated on Mon Oct 6 2014 01:39:03