dances.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: Dances Source File
00002  *
00003  * Library of simple predetermined movements
00004  *
00005  * File: dances.cpp
00006  *
00007  * (C) Dept. Electronics & Computer Science, University of York
00008  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
00009  *
00010  * PsiSwarm Library Version: 0.41
00011  *
00012  * March 2016
00013  *
00014  *
00015  */
00016 
00017 
00018 #include "psiswarm.h"
00019 
00020 char vibrate_counter = 0;
00021 Timeout dances_timeout;
00022 
00023 ///Do a simple wiggle
00024 void vibrate(void)
00025 {
00026     if(vibrate_counter == 0)save_led_states();
00027     if(vibrate_counter % 2 == 0) {
00028         set_leds(0xC7,0x00);
00029         turn(1.0);
00030     } else {
00031         set_leds(0x00,0xC7);
00032         turn(-1.0);
00033     }
00034     vibrate_counter++;
00035 
00036     if(vibrate_counter < 14) {
00037         float wiggle_timeout_period = 0.06;
00038         //Move less on first 'wiggle' so that we stay in roughly the same place!
00039         if(vibrate_counter == 0) wiggle_timeout_period = 0.03;
00040         dances_timeout.attach(vibrate, wiggle_timeout_period);
00041     } else {
00042         vibrate_counter = 0;
00043         brake();
00044         restore_led_states();
00045     }
00046 }