File content as of revision 5:f6be169e465b:

/* University of York Robotics Laboratory PsiSwarm Library: Dances Source File
 *
 * Library of simple predetermined movements
 *
 * File: dances.cpp
 *
 * (C) Dept. Electronics & Computer Science, University of York
 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
 *
 * PsiSwarm Library Version: 0.41
 *
 * March 2016
 *
 *
 */


#include "psiswarm.h"

char vibrate_counter = 0;
Timeout dances_timeout;

///Do a simple wiggle
void vibrate(void)
{
    if(vibrate_counter == 0)save_led_states();
    if(vibrate_counter % 2 == 0) {
        set_leds(0xC7,0x00);
        turn(1.0);
    } else {
        set_leds(0x00,0xC7);
        turn(-1.0);
    }
    vibrate_counter++;

    if(vibrate_counter < 14) {
        float wiggle_timeout_period = 0.06;
        //Move less on first 'wiggle' so that we stay in roughly the same place!
        if(vibrate_counter == 0) wiggle_timeout_period = 0.03;
        dances_timeout.attach(vibrate, wiggle_timeout_period);
    } else {
        vibrate_counter = 0;
        brake();
        restore_led_states();
    }
}