laser-io: works now well the new laser. Two ways of measurement. One uses the

IRC and the other uses the TPU.

diff --git a/src/laser-io/laser-rotaring.c b/src/laser-io/laser-rotaring.c
index 1cf8dc772e93d5994cffcf855b428eb8b9d07273..0eecdd325fc4099dc6a08d71f886a24b300412cd 100644 (file)
--- a/src/laser-io/laser-rotaring.c
+++ b/src/laser-io/laser-rotaring.c
@@ -38,11 +38,21 @@ enum {
        EMPTY = 0,
        INT_IR,
        INT_LASER,
+       SET_REF_DONE
 };
 
+/* starting and end irc point of the rotation */
+#define IRC_START_POINT        0
+#define DIRECTION      (-1)
+/* define USE_IRC to use IRC to measure angles else the TPU will be used */
+#define USE_IRC
+#undef USE_IRC
+
 static int prog_flag = EMPTY;
+static int set_ref_done = 0;
 /* measures */
 static __u16 time[LAS_CNT];
+static __u16 tmp;
 
 /* Funtions prototypes */
 void unhandled_exception(void) __attribute__ ((interrupt_handler));
@@ -197,17 +207,38 @@ pxmc_state_list_t pxmc_main_list = {
 void isr_rotation_finished(void) __attribute__ ((interrupt_handler));
 void isr_rotation_finished(void)
 { 
-       led = 0;
-       deb_led_out(led);
-       prog_flag = INT_IR;
+#ifdef USE_IRC
+       long ap;
+       ap = (mcs0.pxms_ap>>PXMC_SUBDIV(&mcs0)) % (long)(14746);
+       tmp = (ap < 0) ? (__u16)((-1)*(ap)) : (__u16)(ap); 
+#else
+       tmp = (__u16)*TPU_TCNT3;
+#endif
+
+       /* insensitive zone, used to eliminate interrupt noises */
+#ifdef USE_IRC
+       if (meas_cnt <= 0)
+#else
+       if (tmp < 10000)
+#endif
+               goto out;
 
        time[LAS_MEAS_CNTI] = meas_cnt;
-       time[LAS_MEAS_PERI] = (__u16)((mcs0.pxms_ap % (long)(2048 * 7.2 * 256))/100);
+       time[LAS_MEAS_PERI] = tmp;
+
        meas_cnt = 0;
        /* store measured times from the previous cycle */
        memcpy(meas, time, LAS_CNT*sizeof(time[0]));
        memset(time, 0, LAS_CNT*sizeof(time[0]));
        
+#ifndef USE_IRC
+       *TPU_TCNT3 = 0;
+#endif
+       led = 0;
+       deb_led_out(led);
+
+out:
+       prog_flag = INT_IR;
        /* Clean interrupt source */
        *TPU_TSR3 &= ~TSR3_TGFAm;
 }
@@ -216,17 +247,24 @@ void isr_rotation_finished(void)
 void isr_beacon(void) __attribute__ ((interrupt_handler));
 void isr_beacon(void)
 { 
-       led++;
-       deb_led_out(led);
-//     prog_flag = INT_LASER;
+#ifdef USE_IRC
+       long ap;
+       ap = (mcs0.pxms_ap>>PXMC_SUBDIV(&mcs0)) % (long)(14746);
+       tmp = (ap < 0) ? (__u16)((-1)*(ap)) : (__u16)(ap); 
+#else
+       tmp = (__u16)*TPU_TGR3B;
+#endif
 
        /* store measurements */
        if (meas_cnt < (LAS_CNT - LAS_MEAS_DATAI)) {
-               time[LAS_MEAS_DATAI + meas_cnt] = 
-                       (__u16)((mcs0.pxms_ap % (long)(2048 * 7.2 * 256))/100);
+               time[LAS_MEAS_DATAI + meas_cnt] = tmp;
                meas_cnt++;
+               led++;
        }
+
+       deb_led_out(led);
        
+       /*prog_flag = INT_LASER;*/
        /* clean interrupt source */
        *TPU_TSR3 &= ~TSR3_TGFBm;
 }
@@ -238,8 +276,9 @@ void init_rot_timer(void)
        
        /*CNTN cleared by by TGRA, rising edge; source TGRA compare match/input 
         * capture, prescaler 1024  */
-//     *TPU_TCR3=(TPCR_TPSC_F64 | TPCR_CKEG_FAL | TPCR_CCLR_TGRA); 
-       *TPU_TCR3=(TPCR_TPSC_3F4096 | TPCR_CKEG_FAL | TPCR_CCLR_TGRA); 
+//     *TPU_TCR3=(TPCR_TPSC_F64 | TPCR_CKEG_FAL); 
+       *TPU_TCR3=(TPCR_TPSC_135F256 | TPCR_CKEG_FAL); 
+//     *TPU_TCR3=(TPCR_TPSC_3F4096 | TPCR_CKEG_FAL | TPCR_CCLR_TGRA); 
        /*TGRA is input capture registr at rising edge*/
        *TPU_TIOR3H|=TIOR3H_IOA3m;
        /*TGRB is input capture registr at rising edge*/
@@ -271,7 +310,11 @@ void init_engine(void)
        pxmc_spd(&mcs1, rot_per_sec*(long)14746<<PXMC_SUBDIV(mcs1)/pxmc_get_sfi_hz(&mcs1), 0);
        */
        /* set up engine speed */
-       pxmc_spd(&mcs0, 5400, 0);
+#ifdef USE_IRC
+       pxmc_spd(&mcs0, (-1)*DIRECTION*1800, 0);
+#else
+       pxmc_spd(&mcs0, DIRECTION*5400, 0);
+#endif
 //     pxmc_spd(&mcs1, -1800, 0);
 }
 
@@ -380,20 +423,38 @@ int main()
 #endif
        /* initialization of the laser`s beacon engine */
        init_engine();
-//     /* TPU initialization */
+
+       /* TPU initialization */
        init_rot_timer();
+       deb_led_out(led);
 
        /* the main loop */
        do {
                /* turn on command line processor */
                cmd_processor_run(&cmd_io_rs232_line, cmd_list_default);
-               deb_led_out(led);
 
                if (!prog_flag) {
-//                     printf("%lu\n", ((mcs0.pxms_ap % (long)(2048 * 7.2 * 256))/100));
-//                     printf("%lu\n", (mcs0.pxms_ap % (long)(2048 * 7.2 * 256)));
+//                     printf("tmp=%u\n", tmp);
+//                     printf("TCNT3=%u\n", *TPU_TCNT3);
+//                     printf("ap: %ld meas_cnt: %d\n",
+//                             ((mcs0.pxms_ap>>PXMC_SUBDIV(&mcs0)) % (long)(2048 * 7.2)), 
+//                             meas_cnt);
+//                     printf("request pos is: %ld\n",mcs0.pxms_rp>>PXMC_SUBDIV(&mcs0));
                        continue;
                }
+
+#ifdef USE_IRC
+               /* set the starting position of the irc */
+               if (set_ref_done != SET_REF_DONE) {
+                       printf("setting pos\n");
+                       pxmc_set_const_out(&mcs0, 0);
+                       pxmc_axis_set_pos(&mcs0, (long)(IRC_START_POINT));
+                       mcs0.pxms_ptofs = 0;
+                       pxmc_spd(&mcs0, DIRECTION*5400, 0);
+                       set_ref_done = SET_REF_DONE;
+               }
+#endif
+
 #ifdef LAS_CAN
                can_laser_send();
 #else