// Test program: verifies all motors and servos are functioning.
// Run this on the robot before competition to check hardware.

#include <kipr/wombat.h>

void test_servo(int port) {
    printf("testing servo %d\n", port);
    printf("\tup 2 seconds\n");
    enable_servo(port);
    enable_servos();
    set_servo_position(port, 0);
    msleep(2000);
    set_servo_position(port, 2047);
    msleep(2000);
    disable_servo(port);
    disable_servos();
}

void test_motor(int port) {
    printf("testing motor %d\n", port);
    printf("\tforward 2 seconds\n");
    motor(port, 100);
    msleep(2000);
    printf("\tbackward 2 seconds\n");
    motor(port, -100);
    msleep(2000);
    ao();
}

int main() {
    test_motor(0);
    test_motor(1);
    test_motor(2);
    test_motor(3);
    test_servo(0);
    test_servo(1);
    return 0;
}