![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
PsiSwarm/led.cpp@27:7eb032772bc2, 2016-01-31 (annotated)
- Committer:
- alanmillard
- Date:
- Sun Jan 31 15:14:54 2016 +0000
- Revision:
- 27:7eb032772bc2
- Parent:
- 10:1b09d4bb847b
Flocking seems to work better now, however the robot controller sometimes "crashes" (unsure why).
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jah128 | 0:8a5497a2e366 | 1 | /* University of York Robotics Laboratory PsiSwarm Library: LED Functions Source File |
jah128 | 0:8a5497a2e366 | 2 | * |
jah128 | 0:8a5497a2e366 | 3 | * File: led.cpp |
jah128 | 0:8a5497a2e366 | 4 | * |
jah128 | 6:ff3c66f7372b | 5 | * (C) Dept. Electronics & Computer Science, University of York |
jah128 | 6:ff3c66f7372b | 6 | * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis |
jah128 | 0:8a5497a2e366 | 7 | * |
jah128 | 6:ff3c66f7372b | 8 | * PsiSwarm Library Version: 0.3 |
jah128 | 0:8a5497a2e366 | 9 | * |
jah128 | 6:ff3c66f7372b | 10 | * October 2015 |
jah128 | 0:8a5497a2e366 | 11 | * |
jah128 | 0:8a5497a2e366 | 12 | */ |
jah128 | 0:8a5497a2e366 | 13 | |
jah128 | 0:8a5497a2e366 | 14 | #include "psiswarm.h" |
jah128 | 0:8a5497a2e366 | 15 | |
jah128 | 0:8a5497a2e366 | 16 | char green_led_states; |
jah128 | 0:8a5497a2e366 | 17 | char red_led_states; |
jah128 | 0:8a5497a2e366 | 18 | char center_led_state; |
jah128 | 0:8a5497a2e366 | 19 | |
jah128 | 0:8a5497a2e366 | 20 | char held_red_states; |
jah128 | 0:8a5497a2e366 | 21 | char held_green_states; |
jah128 | 0:8a5497a2e366 | 22 | Timeout blink_led_timeout; |
jah128 | 0:8a5497a2e366 | 23 | |
jah128 | 0:8a5497a2e366 | 24 | unsigned short get_led_states(){ |
jah128 | 0:8a5497a2e366 | 25 | return (green_led_states << 8) + red_led_states; |
jah128 | 0:8a5497a2e366 | 26 | } |
jah128 | 0:8a5497a2e366 | 27 | |
jah128 | 0:8a5497a2e366 | 28 | void set_leds(char green, char red){ |
jah128 | 0:8a5497a2e366 | 29 | green_led_states = green; |
jah128 | 0:8a5497a2e366 | 30 | red_led_states = red; |
jah128 | 0:8a5497a2e366 | 31 | IF_update_leds(); |
jah128 | 0:8a5497a2e366 | 32 | } |
jah128 | 0:8a5497a2e366 | 33 | |
jah128 | 0:8a5497a2e366 | 34 | void set_base_led(char state){ |
jah128 | 0:8a5497a2e366 | 35 | IF_set_base_LED(state); |
jah128 | 0:8a5497a2e366 | 36 | } |
jah128 | 0:8a5497a2e366 | 37 | |
jah128 | 0:8a5497a2e366 | 38 | void set_green_leds(char green){ |
jah128 | 0:8a5497a2e366 | 39 | green_led_states = green; |
jah128 | 0:8a5497a2e366 | 40 | IF_update_leds(); |
jah128 | 0:8a5497a2e366 | 41 | } |
jah128 | 0:8a5497a2e366 | 42 | |
jah128 | 0:8a5497a2e366 | 43 | void set_red_leds(char red){ |
jah128 | 0:8a5497a2e366 | 44 | red_led_states = red; |
jah128 | 0:8a5497a2e366 | 45 | IF_update_leds(); |
jah128 | 0:8a5497a2e366 | 46 | } |
jah128 | 0:8a5497a2e366 | 47 | |
jah128 | 0:8a5497a2e366 | 48 | void set_led(char led, char state){ |
jah128 | 0:8a5497a2e366 | 49 | if(state % 2 == 1) red_led_states |= 1 << led; |
jah128 | 0:8a5497a2e366 | 50 | else red_led_states &= ~(1 << led); |
jah128 | 0:8a5497a2e366 | 51 | if(state / 2) green_led_states |= 1 << led; |
jah128 | 0:8a5497a2e366 | 52 | else green_led_states &= ~(1 << led); |
jah128 | 0:8a5497a2e366 | 53 | IF_update_leds(); |
jah128 | 0:8a5497a2e366 | 54 | } |
jah128 | 0:8a5497a2e366 | 55 | |
jah128 | 0:8a5497a2e366 | 56 | void blink_leds(float timeout){ |
jah128 | 10:1b09d4bb847b | 57 | save_led_states(); |
jah128 | 0:8a5497a2e366 | 58 | set_leds(0xFF,0xFF); |
jah128 | 10:1b09d4bb847b | 59 | blink_led_timeout.attach(&restore_led_states, timeout); |
jah128 | 0:8a5497a2e366 | 60 | } |
jah128 | 0:8a5497a2e366 | 61 | |
jah128 | 0:8a5497a2e366 | 62 | void set_center_led(char state){ |
jah128 | 0:8a5497a2e366 | 63 | set_center_led(state, center_led_brightness); |
jah128 | 0:8a5497a2e366 | 64 | } |
jah128 | 0:8a5497a2e366 | 65 | |
jah128 | 0:8a5497a2e366 | 66 | void set_center_led(char state, float brightness){ |
jah128 | 0:8a5497a2e366 | 67 | center_led_brightness = brightness; |
jah128 | 0:8a5497a2e366 | 68 | center_led_state = state; |
jah128 | 0:8a5497a2e366 | 69 | switch(center_led_state){ |
jah128 | 0:8a5497a2e366 | 70 | case 0: center_led_red.write(0); |
jah128 | 0:8a5497a2e366 | 71 | center_led_green.write(0); |
jah128 | 0:8a5497a2e366 | 72 | break; |
jah128 | 0:8a5497a2e366 | 73 | case 1: center_led_red.write(center_led_brightness / 4); |
jah128 | 0:8a5497a2e366 | 74 | center_led_green.write(0); |
jah128 | 0:8a5497a2e366 | 75 | break; |
jah128 | 0:8a5497a2e366 | 76 | case 2: center_led_red.write(0); |
jah128 | 0:8a5497a2e366 | 77 | center_led_green.write(center_led_brightness); |
jah128 | 0:8a5497a2e366 | 78 | break; |
jah128 | 0:8a5497a2e366 | 79 | case 3: center_led_red.write(center_led_brightness / 4); |
jah128 | 0:8a5497a2e366 | 80 | center_led_green.write(center_led_brightness); |
jah128 | 0:8a5497a2e366 | 81 | break; |
jah128 | 0:8a5497a2e366 | 82 | } |
jah128 | 0:8a5497a2e366 | 83 | } |
jah128 | 0:8a5497a2e366 | 84 | |
jah128 | 0:8a5497a2e366 | 85 | void set_center_led_brightness(float brightness){ |
jah128 | 0:8a5497a2e366 | 86 | set_center_led(center_led_state,brightness); |
jah128 | 0:8a5497a2e366 | 87 | } |
jah128 | 0:8a5497a2e366 | 88 | |
jah128 | 10:1b09d4bb847b | 89 | void save_led_states(){ |
jah128 | 10:1b09d4bb847b | 90 | held_red_states = red_led_states; |
jah128 | 10:1b09d4bb847b | 91 | held_green_states = green_led_states; |
jah128 | 10:1b09d4bb847b | 92 | } |
jah128 | 10:1b09d4bb847b | 93 | |
jah128 | 10:1b09d4bb847b | 94 | void restore_led_states(){ |
jah128 | 0:8a5497a2e366 | 95 | set_leds(held_green_states,held_red_states); |
jah128 | 0:8a5497a2e366 | 96 | } |
jah128 | 0:8a5497a2e366 | 97 | |
jah128 | 0:8a5497a2e366 | 98 | void IF_init_leds(){ |
jah128 | 0:8a5497a2e366 | 99 | green_led_states = 0; |
jah128 | 0:8a5497a2e366 | 100 | red_led_states = 0; |
jah128 | 0:8a5497a2e366 | 101 | center_led_red.period_us(100); |
jah128 | 0:8a5497a2e366 | 102 | center_led_green.period_us(100); |
jah128 | 0:8a5497a2e366 | 103 | set_center_led(0,0.2); |
jah128 | 0:8a5497a2e366 | 104 | } |
jah128 | 0:8a5497a2e366 | 105 | |
jah128 | 0:8a5497a2e366 | 106 | void IF_update_leds(){ |
jah128 | 0:8a5497a2e366 | 107 | char led_byte_0 = (((green_led_states & (1 << 3)) == 0) << 7) + |
jah128 | 0:8a5497a2e366 | 108 | (((green_led_states & (1 << 2)) == 0) << 5) + |
jah128 | 0:8a5497a2e366 | 109 | (((green_led_states & (1 << 1)) == 0) << 3) + |
jah128 | 0:8a5497a2e366 | 110 | (((green_led_states & (1)) == 0) << 1) + |
jah128 | 0:8a5497a2e366 | 111 | (((red_led_states & (1 << 3)) == 0) << 6) + |
jah128 | 0:8a5497a2e366 | 112 | (((red_led_states & (1 << 2)) == 0) << 4) + |
jah128 | 0:8a5497a2e366 | 113 | (((red_led_states & (1 << 1)) == 0) << 2) + |
jah128 | 0:8a5497a2e366 | 114 | (((red_led_states & (1)) == 0)); |
jah128 | 0:8a5497a2e366 | 115 | char led_byte_1 = (((green_led_states & (1 << 7)) == 0) << 7) + |
jah128 | 0:8a5497a2e366 | 116 | (((green_led_states & (1 << 6)) == 0) << 5) + |
jah128 | 0:8a5497a2e366 | 117 | (((green_led_states & (1 << 5)) == 0) << 3) + |
jah128 | 0:8a5497a2e366 | 118 | (((green_led_states & (1 << 4)) == 0) << 1) + |
jah128 | 0:8a5497a2e366 | 119 | (((red_led_states & (1 << 7)) == 0) << 6) + |
jah128 | 0:8a5497a2e366 | 120 | (((red_led_states & (1 << 6)) == 0) << 4) + |
jah128 | 0:8a5497a2e366 | 121 | (((red_led_states & (1 << 5)) == 0) << 2) + |
jah128 | 0:8a5497a2e366 | 122 | (((red_led_states & (1 << 4)) == 0)); |
jah128 | 0:8a5497a2e366 | 123 | IF_write_to_led_ic(led_byte_0,led_byte_1); |
jah128 | 0:8a5497a2e366 | 124 | } |