+ state_mode = I2C_COBBOARD_MODE_HARVEST;
+ }
+
+ if (HARVEST(state_mode)) {
+ /* init for each loop */
+ shovel_down();
+ servo_carry_close();
+ servo_door_close();
+
+ /* pack/deply spickles, enable/disable roller */
+ cobroller_off(I2C_LEFT_SIDE);
+ cobroller_off(I2C_RIGHT_SIDE);
+ spickle_prepare(I2C_LEFT_SIDE);
+ spickle_prepare(I2C_RIGHT_SIDE);
+
+ /* harvest if not many cobs */
+ if (cob_count < 5) {
+ if (state_spicklemode_deployed(I2C_LEFT_SIDE) &&
+ state_spicklemode_autoharvest(I2C_LEFT_SIDE))
+ state_do_harvest(I2C_LEFT_SIDE);
+ if (state_spicklemode_deployed(I2C_RIGHT_SIDE) &&
+ state_spicklemode_autoharvest(I2C_RIGHT_SIDE))
+ state_do_harvest(I2C_RIGHT_SIDE);
+ }
+ }
+
+ /* help to climb the hill */
+ if (KICKSTAND_UP(state_mode)) {
+ state_status = I2C_COBBOARD_STATUS_KICKSTAND_UP;
+ servo_carry_open();
+ servo_door_open();
+ shovel_kickstand_up();