Autodemo updated, timer routeines calculate proper values for given timer period.

[mirosot.git] / autodemo / mirosot_autodemo.c

diff --git a/autodemo/mirosot_autodemo.c b/autodemo/mirosot_autodemo.c
index 44942b9c21b96becfa6bc0b55633c00b7b84989e..b512e1fb3eb4055e97f87fd16a9e03e2e936085f 100644 (file)
--- a/autodemo/mirosot_autodemo.c
+++ b/autodemo/mirosot_autodemo.c
@@ -24,6 +24,8 @@
 
 #include <cmd_proc.h>
 #include "cmd_pxmc.h"
+#include "timer3.h"
+#include <stdio.h>
 
 
 /*struktury prikazu cmd*/
@@ -147,10 +149,9 @@ int cmd_rs232_processor_run(void)
 
 extern void _print(char *str);
 
-
-int main()
+void
+init(void)
 {
-
   /********************************************************************************/
   *DIO_PJDDR=0xff;     /*output gate*/
   *DIO_PEDDR=0xff;     /*output gate*/
@@ -183,66 +184,78 @@ int main()
   /*nastaveni DC motoru*/
   pxmc_add_pservice_and_mode(4); /*Macro -  mod=4 tj. all motors are DC*/
 
-  /*nekonecna smycka obsluhujici bth, pc ...*/
-/*   do{ */
-/*     cmd_rs232_processor_run();  /\*sber + odesilani cmd prikazu PC*\/ */
-/*   }while(1); */
-#include "timer3.h"
+  if (init_timer3((long long)200/*ms*/*1000*1000) < 0) {
+    /* ERROR */
+    DEB_LED_OFF(0);
+  }
+}
+
+
+// Distance of wheels - d
+#define WHEEL_DIST 74 /* mm */
+#define MAX_R 300 /* mm */
+#define MIN_R (WHEEL_DIST/2) /* mm */
+
+void move(int speed, int r)
+{
+  int sl, sr;
+  int d = WHEEL_DIST;
 
-  init_timer3();
 
-  long int before = get_timer();
-  int a=1;
+  if (r != 0) {
+    sr = speed + (long long)speed * (d/2) / r;
+    sl = speed - (long long)speed * (d/2) / r;
+  } else {
+    sr = speed;
+    sl = speed;
+  }
+  pxmc_spd(&mcsX0, +sl, 0);
+  pxmc_spd(&mcsX1, -sr, 0);
+  //printf("speed=%5d, r=%5d, sl=%5d, sr=%5d\n", speed, r, sl, sr);
+}
+
+void
+main_loop(void)
+{
+  long int now = get_timer();
+  long int before = now;
+  long int next = now;
+  int speed = 3000;
+  int radius = 0;
   while (1) {
     cmd_rs232_processor_run();  /*sber + odesilani cmd prikazu PC*/
 
     long int now = get_timer();
 
-    if (now != before)
-      switch (now % 10) {
-        case 0:
-          pxmc_spd(&mcsX0,a*400,0);
-          pxmc_spd(&mcsX1,-a*700,0);
-          break;
-        case 1:
-          pxmc_spd(&mcsX0,a*700,0);
-          pxmc_spd(&mcsX1,-a*400,0);
-          break;
-        case 2:
-          pxmc_spd(&mcsX0,a*2000,0);
-          pxmc_spd(&mcsX1,-a*1200,0);
-          break;
-        case 3:
-          pxmc_spd(&mcsX0,a*1200,0);
-          pxmc_spd(&mcsX1,-a*2000,0);
-          break;
-        case 4:
-          pxmc_spd(&mcsX0,a*1300,0);
-          pxmc_spd(&mcsX1,-a*1000,0);
-          break;
-        case 5:
-          pxmc_spd(&mcsX0,a*1300,0);
-          pxmc_spd(&mcsX1,-a*1000,0);
-          break;
-        case 6:
-          pxmc_spd(&mcsX0,a*100,0);
-          pxmc_spd(&mcsX1,-a*300,0);
-          break;
-        case 7:
-          pxmc_spd(&mcsX0,a*300,0);
-          pxmc_spd(&mcsX1,-a*100,0);
-          break;
-        case 8:
-          pxmc_spd(&mcsX0,a*2000,0);
-          pxmc_spd(&mcsX1,-a*100,0);
-          break;
-        case 9:
-          pxmc_spd(&mcsX0,a*4000,0);
-          pxmc_spd(&mcsX1,-a*4000,0);
-          if(a>0) a=-1;
-          else a=1;
-          break;
-          
+    if (now > before) {
+      before = now;
+      DEB_LED_XOR(1);
+    }
+    if (now >= next) {
+      DEB_LED_XOR(3);
+      next = now + 1 + (rand() % 5);
+      //printf("%5ld->%5ld ", now, next);
+      if (rand() % 100 < 20)
+        speed = -speed;         /* changing direction */
+      if (rand() % 100 < 50) 
+        move(speed, 0);         /* go stright ahaed */
+      else {
+        radius = (rand() % (2*(MAX_R-MIN_R))) - (MAX_R-MIN_R);
+        if (radius >= 0) radius += MIN_R;
+        else radius -= MIN_R;
+        move(speed, radius);    /* turn */
+/*         if (abs(radius) < WHEEL_DIST)  */
+/*           next = now + 1;/\* sharp cuves are short *\/ */
       }
+    }
   }
 }
+
+
+int main()
+{
+  init();
+  /* TODO initialize rand accoring to voltage read from AD6. */
+  main_loop();
+  return 0;
+}