+#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
+
+ if ((cobboard.flags & DO_BD) && (cobboard.flags & DO_POWER)) {