Colony

#include "Forklift.h"

extern "C"

{

#include <dragonfly_lib.h>

}

#define SPD 200

using namespace Forklift;

int main()

{

	dragonfly_init(ALL_ON);

	orb_init();

	forklift_init();

	set_height_setpoint(128);

	while(true)

	{

		while(!button2_read());

		motor_l_set(FORWARD, SPD);

		motor_r_set(FORWARD, SPD);

		delay_ms(1500);

		motor_l_set(FORWARD, 0);

		motor_r_set(FORWARD, 0);

		set_height_setpoint(0);

		delay_ms(1000);

		motor_l_set(FORWARD, SPD);

		motor_r_set(FORWARD, SPD);

		delay_ms(200);

		set_height_setpoint(255);

		motor_l_set(FORWARD, SPD);

		motor_r_set(BACKWARD, SPD);

		delay_ms(500);

		motor_l_set(FORWARD, SPD);

		motor_r_set(FORWARD, SPD);

		delay_ms(1500);

		motor_l_set(FORWARD, SPD);

		motor_r_set(BACKWARD, SPD);

		delay_ms(500);

		motor_l_set(FORWARD, 0);

		motor_r_set(FORWARD, 0);

		set_height_setpoint(128);

	}

}