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

darcs-hash:20061113211822-f2ef6-3b58bd97d06617478ff64728c6397d2458c38f5a.gz

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;
+}

diff --git a/autodemo/timer3.c b/autodemo/timer3.c
index 259a3ece8704c8ed73bf3c01886e95e1beb6b450..a531debe274ff6f730fa278176046ea850d1f95a 100644 (file)
--- a/autodemo/timer3.c
+++ b/autodemo/timer3.c
@@ -1,6 +1,7 @@
 #include <timer3.h>
 #include <mcu_regs.h>
 #include <cpu_def.h>
+#include <system_def.h>
 
 static volatile timer_t timer;
 
@@ -10,23 +11,44 @@ static void
 timer_isr(void)
 {
     timer++;
-    *TPU_TSR3 &= ~TSR2_TCFVm ; //reset overflow flag (clear interrupt)
+    //*TPU_TSR3 &= ~TSR2_TCFVm ; //reset overflow flag (clear interrupt)
+    *TPU_TSR3 &= ~TSR3_TCFVm & ~TSR3_TGFAm; //reset overflow and comare match flags
 }    
 
 //timer initialisation
 /*free running counter*/
-void init_timer3()
+int init_timer3(long long nsec)
 {
-    *SYS_MSTPCRA &= ~MSTPCRA_TPUm; // power TPU unit
-
-    *TPU_TCR3 =0x00 | 0x06;     //rising edge, f divided by 256
-    *TPU_TMDR3 =0x00;           // normal mode
-    *TPU_TSR3 &= ~TSR3_TCFVm ;  //reset overflow flag
-    *TPU_TIER3 |=TIER3_TCIEVm;  //enable overflow interrupt
-
-    *TPU_TSTR |=TSTR_CST3m;     //start timer
-
-    excptvec_set(52, timer_isr); 
+#define CPU_CYCLE_NSEC (1000000000/CPU_SYS_HZ)
+  long long timer_tick = CPU_CYCLE_NSEC;
+  int presc_ind = 0;
+  const signed char presc_tab[] = {0,1,2,3,6,5,7,-1};
+  long long nsec_scaled = nsec >> 16;
+  while (presc_ind < 8 &&
+         (timer_tick < nsec_scaled
+          || presc_tab[presc_ind] < 0)) {
+    presc_ind++;
+    timer_tick <<= 2;
+  }
+  if (timer_tick < nsec_scaled)
+    return -1;
+
+
+  *SYS_MSTPCRA &= ~MSTPCRA_TPUm; // power TPU unit
+  
+  *TPU_TCR3 = TCR3_CCLR0m | presc_tab[presc_ind]; //rising edge, cleared by TGA, prescaler is calculated
+  *TPU_TMDR3 =0x00;             // normal mode
+  *TPU_TIOR3L = TIOR3L_IOC1m;   // output 1 at compare match
+  *TPU_TSR3 &= ~TSR3_TCFVm & ~TSR3_TGFAm; //reset overflow and comare match flags
+  //*TPU_TIER3 |=TIER3_TCIEVm;    //enable overflow interrupt
+  *TPU_TIER3 |=TIER3_TGIEAm;    //enable compare match interrupt
+  *TPU_TGR3A = (unsigned)(nsec/timer_tick);
+
+  *TPU_TSTR |=TSTR_CST3m;       //start timer
+  
+  //excptvec_set(EXCPTVEC_TCI3V, timer_isr);  /* handle overflow interrupt */
+  excptvec_set(EXCPTVEC_TGI3A, timer_isr); /* handle TGRA match interrupt */
+  return 0;
 }
 
 timer_t get_timer()

diff --git a/autodemo/timer3.h b/autodemo/timer3.h
index 0bee22d3a160aca0015080fbce4b028e9dacaef8..9bdf2d26f16c69edf7262912105bff8edd04971f 100644 (file)
--- a/autodemo/timer3.h
+++ b/autodemo/timer3.h
@@ -2,6 +2,6 @@
 
 typedef long long timer_t;
 
-void init_timer3();
+int init_timer3(long long nsec);
 timer_t get_timer();