File content as of revision 6:b340a527add9:

/* University of York Robotics Laboratory PsiSwarm Library: Dances Source File
 * 
 * Copyright 2016 University of York
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. 
 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
 * See the License for the specific language governing permissions and limitations under the License.
 *
 * 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.7
 *
 * October 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();
    }
}