-#ifdef DEBUG_CPLD
- {
- extern int16_t g_encoders_spi_previous[4];
- int32_t ls, rs, sh;
-
- ls = (cobboard.left_spickle.prev - cobboard.left_spickle.cs.filtered_feedback_value);
- rs = (cobboard.right_spickle.prev - cobboard.right_spickle.cs.filtered_feedback_value);
- sh = (cobboard.shovel.prev - cobboard.shovel.cs.filtered_feedback_value);
- if (ls < -2000 || ls > 2000 ||
- rs < -2000 || rs > 2000 ||
- sh < -2000 || sh > 2000) {
- printf_P(PSTR("left_spickle %ld "), ls);
- printf_P(PSTR("right_spickle %ld "), rs);
- printf_P(PSTR("shovel %ld "), sh);
- printf_P(PSTR("/ %d %d %d %d\r\n"),
- g_encoders_spi_previous[0],
- g_encoders_spi_previous[1],
- g_encoders_spi_previous[2],
- g_encoders_spi_previous[3]);
- BRAKE_ON();
- while (1);
- }
- }
-#endif