#include <kipr/wombat.h>

// ============================================================
// PAUSE
// ============================================================
// Stop all motors and wait 2 seconds
void pause_example() {
    ao();
    msleep(2000);
}

// ============================================================
// FORWARD
// ============================================================
void forward_example() {
    motor(0, 100);
    motor(3, 100);
    msleep(250);
}

// Forward using MAV (Motor Absolute Velocity)
void Move(int l_power, int r_power, int time) {
    mav(0, l_power);
    mav(3, r_power);
    msleep(time);
    ao();
    sleep(100);
}

// ============================================================
// BACKWARD
// ============================================================
void backward_example() {
    motor(0, -100);
    motor(3, -100);
    msleep(250);
}

// ============================================================
// TURN 90 DEGREES RIGHT
// ============================================================
void turn_right_90() {
    motor(0, 0);
    motor(3, 100);
    msleep(1500);
}

// ============================================================
// TURN 90 DEGREES LEFT
// ============================================================
void turn_left_90() {
    motor(0, 100);
    motor(3, 0);
    msleep(1500);
}

// ============================================================
// MOTOR POSITION COUNTER
// ============================================================
// Counts "ticks" - one complete wheel rotation
// Motors have approximately 1820 ticks per revolution
void motor_position_counter_example() {
    clear_motor_position_counter(3);
    while (get_motor_position_counter(3) < 4000) {
        motor(3, 50);
        motor(0, 50);
    }
    ao();
}