Colony

#include "Forklift.h"

#include "twi.h"

extern "C"

{

#include <dragonfly_lib.h>

}

#define SPD 190 //200

#define TURN_TIME 1050 //950

using namespace Forklift;

int main()

{

        sei();

        dragonfly_init(ALL_ON);

        orb_init();

        forklift_init();

  orb1_set_color(BLUE);

  orb2_set_color(YELLOW);

        set_height_setpoint(128);

  orb1_set_color(PURPLE);

  orb2_set_color(RED);

  /*int high = 1;

  while(true)

  {

                set_height_setpoint(high? 100 : 150);

                high = !high;

                delay_ms(4000);

        }*/

        while(true)

        {

                while(!button2_read());

                motor_l_set(BACKWARD, SPD);

                motor_r_set(BACKWARD, SPD);

                delay_ms(800);

                motor_l_set(BACKWARD, 0);

                motor_r_set(BACKWARD, 0);

                set_height_setpoint(30);

                delay_ms(1000);

                motor_l_set(BACKWARD, SPD);

                motor_r_set(BACKWARD, SPD);

                delay_ms(200);

                set_height_setpoint(230);

                delay_ms(600);

                motor_l_set(BACKWARD, 0);

                motor_r_set(BACKWARD, 0);

                delay_ms(1200);

                motor_l_set(BACKWARD, SPD);

                motor_r_set(FORWARD, SPD);

                delay_ms(TURN_TIME);

                motor_l_set(BACKWARD, SPD);

                motor_r_set(BACKWARD, SPD);

                delay_ms(800);

                motor_l_set(BACKWARD, SPD);

                motor_r_set(FORWARD, SPD);

                delay_ms(TURN_TIME);

                motor_l_set(BACKWARD, 0);

                motor_r_set(BACKWARD, 0);

                set_height_setpoint(128);

        }

}