summaryrefslogtreecommitdiff
path: root/n/asserv/src/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/main.c')
-rw-r--r--n/asserv/src/main.c25
1 files changed, 22 insertions, 3 deletions
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 9eeca45..1715d5f 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -27,6 +27,7 @@
#include <n/avr/proto/proto.h>
#include <n/avr/utils/utils.h>
#include <n/avr/utils/byte.h>
+#include <n/avr/twi-master/twi_master.h>
#include <avr/interrupt.h>
#include <avr/io.h>
@@ -67,6 +68,10 @@ uint8_t motor_stat_timer, motor_stat_timer_cpt;
/** Print PINC. */
uint8_t motor_print_pinc, motor_print_pinc_cpt;
+/** Print Sharps. */
+uint8_t motor_print_sharps, motor_print_sharps_cpt;
+uint16_t motor_sharps[3];
+
/** Record timer value at different stage of computing. Used for performance
* analisys. */
uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3,
@@ -91,6 +96,7 @@ main (void)
speed_init ();
postrack_init ();
rs232_init ();
+ twi_master_init ();
proto_send0 ('z');
sei ();
main_loop ();
@@ -116,9 +122,7 @@ main_loop (void)
{
goto_update ();
if (goto_finish)
- {
proto_send0 ('F');
- }
}
motor_timer_2 = timer_read ();
/* Speed control. */
@@ -168,9 +172,19 @@ main_loop (void)
}
if (motor_print_pinc && !--motor_print_pinc_cpt)
{
- proto_send1b ('P', PINC);
+ proto_send1b ('P', PINA);
motor_print_pinc_cpt = motor_print_pinc;
}
+ if (motor_print_sharps && !--motor_print_sharps_cpt)
+ {
+ if (twi_master_is_finished ())
+ {
+ proto_send3w ('H', motor_sharps[0], motor_sharps[1],
+ motor_sharps[2]);
+ twi_master_read (0x02, (uint8_t *) motor_sharps, 6);
+ }
+ motor_print_sharps_cpt = motor_print_sharps;
+ }
/* Misc. */
if ((motor_loop_cpt & 7) == 0)
{
@@ -294,6 +308,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Input port stats. */
motor_print_pinc_cpt = motor_print_pinc = args[0];
break;
+ case c ('H', 1):
+ /* Sharps stats. */
+ twi_master_read (0x02, (uint8_t *) motor_sharps, 6);
+ motor_print_sharps_cpt = motor_print_sharps = args[0];
+ break;
default:
/* Params. */
if (cmd == 'p')