Diff: dances.cpp

Revision:

0:d6269d17c8cf

Child:

2:c6986ee3c7c5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dances.cpp	Thu Feb 04 21:48:54 2016 +0000
@@ -0,0 +1,46 @@
+/* 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.4
+ *
+ * February 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();
+    }
+}
\ No newline at end of file