UKESF Headstart Summer School / PsiSwarm-Headstart

Library for the PsiSwarm Robot for Headstart Lab - Version 0.5

Dependents:   UKESF_Lab

Fork of PsiSwarmLibrary by James Hilder

dances.cpp@2:c6986ee3c7c5, 2016-03-11 (annotated)

Committer:
jah128
Date:
Fri Mar 11 13:51:24 2016 +0000
Revision:
2:c6986ee3c7c5
Parent:
0:d6269d17c8cf 
Added Alex' IR functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:d6269d17c8cf 1 /* University of York Robotics Laboratory PsiSwarm Library: Dances Source File
jah128 0:d6269d17c8cf 2 *
jah128 0:d6269d17c8cf 3 * Library of simple predetermined movements
jah128 0:d6269d17c8cf 4 *
jah128 0:d6269d17c8cf 5 * File: dances.cpp
jah128 0:d6269d17c8cf 6 *
jah128 0:d6269d17c8cf 7 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:d6269d17c8cf 8 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
jah128 0:d6269d17c8cf 9 *
jah128 2:c6986ee3c7c5 10 * PsiSwarm Library Version: 0.41
jah128 0:d6269d17c8cf 11 *
jah128 2:c6986ee3c7c5 12 * March 2016
jah128 0:d6269d17c8cf 13 *
jah128 0:d6269d17c8cf 14 *
jah128 0:d6269d17c8cf 15 */
jah128 0:d6269d17c8cf 16
jah128 0:d6269d17c8cf 17
jah128 0:d6269d17c8cf 18 #include "psiswarm.h"
jah128 0:d6269d17c8cf 19
jah128 0:d6269d17c8cf 20 char vibrate_counter = 0;
jah128 0:d6269d17c8cf 21 Timeout dances_timeout;
jah128 0:d6269d17c8cf 22
jah128 0:d6269d17c8cf 23 ///Do a simple wiggle
jah128 0:d6269d17c8cf 24 void vibrate(void)
jah128 0:d6269d17c8cf 25 {
jah128 0:d6269d17c8cf 26 if(vibrate_counter == 0)save_led_states();
jah128 0:d6269d17c8cf 27 if(vibrate_counter % 2 == 0) {
jah128 0:d6269d17c8cf 28 set_leds(0xC7,0x00);
jah128 0:d6269d17c8cf 29 turn(1.0);
jah128 0:d6269d17c8cf 30 } else {
jah128 0:d6269d17c8cf 31 set_leds(0x00,0xC7);
jah128 0:d6269d17c8cf 32 turn(-1.0);
jah128 0:d6269d17c8cf 33 }
jah128 0:d6269d17c8cf 34 vibrate_counter++;
jah128 0:d6269d17c8cf 35
jah128 0:d6269d17c8cf 36 if(vibrate_counter < 14) {
jah128 0:d6269d17c8cf 37 float wiggle_timeout_period = 0.06;
jah128 0:d6269d17c8cf 38 //Move less on first 'wiggle' so that we stay in roughly the same place!
jah128 0:d6269d17c8cf 39 if(vibrate_counter == 0) wiggle_timeout_period = 0.03;
jah128 0:d6269d17c8cf 40 dances_timeout.attach(vibrate, wiggle_timeout_period);
jah128 0:d6269d17c8cf 41 } else {
jah128 0:d6269d17c8cf 42 vibrate_counter = 0;
jah128 0:d6269d17c8cf 43 brake();
jah128 0:d6269d17c8cf 44 restore_led_states();
jah128 0:d6269d17c8cf 45 }
jah128 0:d6269d17c8cf 46 }