#include <kipr/wombat.h>

// ============================================================
// ENABLE / DISABLE SERVOS
// ============================================================
void turn_arm_on() {
    enable_servos();
}

void turn_arm_off() {
    disable_servos();
}

// ============================================================
// SET START POSITION
// ============================================================
void set_start_position() {
    set_servo_position(0, 734);
}

// ============================================================
// ARM ROTATION
// ============================================================
void rotate_up() {
    set_servo_position(0, 724);
    msleep(1000);
}

void rotate_down() {
    set_servo_position(0, 1824);
    msleep(1000);
}

// ============================================================
// CLAW
// ============================================================
void claw_open() {
    set_servo_position(3, 724);
    msleep(1000);
}

void claw_close() {
    set_servo_position(3, 1933);
    msleep(1000);
}

// ============================================================
// HELPER: RUN SERVO TO POSITION
// ============================================================
// Moves servo on 'port' to 'position', then stops all motors
void RunStart(int port, int position) {
    enable_servos();
    set_servo_position(port, position);
    ao();
    msleep(100);
}