Colony

#include <dragonfly_lib.h>

#define OFF         140

#define SLOW         160

#define        GO                220

int testmotors(void) {

    //usb_init();

    //usb_puts("usb turned on\n");

        //forward

        orb_set_color(RED);

        motors_off();

    delay_ms(3000);

    orb_set_color(YELLOW);

        motor_l_set(1, SLOW);

    motor_r_set(1, SLOW);

    delay_ms(3000);

    orb_set_color(GREEN);

        motor_l_set(1, GO);

    motor_r_set(1, GO);

    delay_ms(3000);

        //backward

    orb_set_color(RED);

        motors_off();

    delay_ms(3000);

    orb_set_color(YELLOW);

        motor_l_set(0, SLOW);

    motor_r_set(0, SLOW);

    delay_ms(3000);

    orb_set_color(GREEN);

        motor_l_set(0, GO);

    motor_r_set(0, GO);

    delay_ms(3000);

        //spin

        orb_set_color(RED);

        motors_off();

    delay_ms(3000);

    orb_set_color(YELLOW);

        motor_l_set(1, SLOW);

    motor_r_set(0, SLOW);

    delay_ms(3000);

    orb_set_color(GREEN);

        motor_l_set(1, GO);

    motor_r_set(0, GO);

    delay_ms(3000);

        //spin other way

        orb_set_color(RED);

        motors_off();

    delay_ms(3000);

    orb_set_color(YELLOW);

        motor_l_set(0, SLOW);

    motor_r_set(1, SLOW);

    delay_ms(3000);

    orb_set_color(GREEN);

        motor_l_set(0, GO);

    motor_r_set(1, GO);

    delay_ms(3000);

        //while(1){;}

    motors_off();

    return 0;

}

/*test data

left >154 forward

right >150 forward

both >152 backward

*/