#include <cmd_proc.h>
#include "cmd_pxmc.h"
+#include "timer3.h"
+#include <stdio.h>
/*struktury prikazu cmd*/
extern void _print(char *str);
-
-int main()
+void
+init(void)
{
-
/********************************************************************************/
*DIO_PJDDR=0xff; /*output gate*/
*DIO_PEDDR=0xff; /*output gate*/
/*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;
+}
#include <timer3.h>
#include <mcu_regs.h>
#include <cpu_def.h>
+#include <system_def.h>
static volatile timer_t timer;
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()