From f06a9614fe1ae7e1721d68824bbfc315ca19cc84 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Mon, 13 Nov 2006 21:18:00 +0000 Subject: [PATCH] Autodemo updated, timer routeines calculate proper values for given timer period. darcs-hash:20061113211822-f2ef6-3b58bd97d06617478ff64728c6397d2458c38f5a.gz --- autodemo/mirosot_autodemo.c | 125 ++++++++++++++++++++---------------- autodemo/timer3.c | 46 +++++++++---- autodemo/timer3.h | 2 +- 3 files changed, 104 insertions(+), 69 deletions(-) diff --git a/autodemo/mirosot_autodemo.c b/autodemo/mirosot_autodemo.c index 44942b9..b512e1f 100644 --- a/autodemo/mirosot_autodemo.c +++ b/autodemo/mirosot_autodemo.c @@ -24,6 +24,8 @@ #include #include "cmd_pxmc.h" +#include "timer3.h" +#include /*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 259a3ec..a531deb 100644 --- a/autodemo/timer3.c +++ b/autodemo/timer3.c @@ -1,6 +1,7 @@ #include #include #include +#include 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 0bee22d..9bdf2d2 100644 --- 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(); -- 2.39.2