James O'Keeffe
/
BeaconDemo_RobotCodeNew
ft. button press reset
Fork of BeaconDemo_RobotCode by
PsiSwarm/led.cpp@6:ff3c66f7372b, 2015-10-22 (annotated)
- Committer:
- jah128
- Date:
- Thu Oct 22 00:46:14 2015 +0000
- Revision:
- 6:ff3c66f7372b
- Parent:
- 0:8a5497a2e366
- Child:
- 10:1b09d4bb847b
Initial version: beacon detection and sync. code, bearing estimation.
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 | 0:8a5497a2e366 | 57 | held_red_states = red_led_states; |
jah128 | 0:8a5497a2e366 | 58 | held_green_states = green_led_states; |
jah128 | 0:8a5497a2e366 | 59 | set_leds(0xFF,0xFF); |
jah128 | 0:8a5497a2e366 | 60 | blink_led_timeout.attach(&IF_restore_led_state, timeout); |
jah128 | 0:8a5497a2e366 | 61 | } |
jah128 | 0:8a5497a2e366 | 62 | |
jah128 | 0:8a5497a2e366 | 63 | void set_center_led(char state){ |
jah128 | 0:8a5497a2e366 | 64 | set_center_led(state, center_led_brightness); |
jah128 | 0:8a5497a2e366 | 65 | } |
jah128 | 0:8a5497a2e366 | 66 | |
jah128 | 0:8a5497a2e366 | 67 | void set_center_led(char state, float brightness){ |
jah128 | 0:8a5497a2e366 | 68 | center_led_brightness = brightness; |
jah128 | 0:8a5497a2e366 | 69 | center_led_state = state; |
jah128 | 0:8a5497a2e366 | 70 | switch(center_led_state){ |
jah128 | 0:8a5497a2e366 | 71 | case 0: center_led_red.write(0); |
jah128 | 0:8a5497a2e366 | 72 | center_led_green.write(0); |
jah128 | 0:8a5497a2e366 | 73 | break; |
jah128 | 0:8a5497a2e366 | 74 | case 1: center_led_red.write(center_led_brightness / 4); |
jah128 | 0:8a5497a2e366 | 75 | center_led_green.write(0); |
jah128 | 0:8a5497a2e366 | 76 | break; |
jah128 | 0:8a5497a2e366 | 77 | case 2: center_led_red.write(0); |
jah128 | 0:8a5497a2e366 | 78 | center_led_green.write(center_led_brightness); |
jah128 | 0:8a5497a2e366 | 79 | break; |
jah128 | 0:8a5497a2e366 | 80 | case 3: center_led_red.write(center_led_brightness / 4); |
jah128 | 0:8a5497a2e366 | 81 | center_led_green.write(center_led_brightness); |
jah128 | 0:8a5497a2e366 | 82 | break; |
jah128 | 0:8a5497a2e366 | 83 | } |
jah128 | 0:8a5497a2e366 | 84 | } |
jah128 | 0:8a5497a2e366 | 85 | |
jah128 | 0:8a5497a2e366 | 86 | void set_center_led_brightness(float brightness){ |
jah128 | 0:8a5497a2e366 | 87 | set_center_led(center_led_state,brightness); |
jah128 | 0:8a5497a2e366 | 88 | } |
jah128 | 0:8a5497a2e366 | 89 | |
jah128 | 0:8a5497a2e366 | 90 | void IF_restore_led_state(){ |
jah128 | 0:8a5497a2e366 | 91 | set_leds(held_green_states,held_red_states); |
jah128 | 0:8a5497a2e366 | 92 | } |
jah128 | 0:8a5497a2e366 | 93 | |
jah128 | 0:8a5497a2e366 | 94 | void IF_init_leds(){ |
jah128 | 0:8a5497a2e366 | 95 | green_led_states = 0; |
jah128 | 0:8a5497a2e366 | 96 | red_led_states = 0; |
jah128 | 0:8a5497a2e366 | 97 | center_led_red.period_us(100); |
jah128 | 0:8a5497a2e366 | 98 | center_led_green.period_us(100); |
jah128 | 0:8a5497a2e366 | 99 | set_center_led(0,0.2); |
jah128 | 0:8a5497a2e366 | 100 | } |
jah128 | 0:8a5497a2e366 | 101 | |
jah128 | 0:8a5497a2e366 | 102 | void IF_update_leds(){ |
jah128 | 0:8a5497a2e366 | 103 | char led_byte_0 = (((green_led_states & (1 << 3)) == 0) << 7) + |
jah128 | 0:8a5497a2e366 | 104 | (((green_led_states & (1 << 2)) == 0) << 5) + |
jah128 | 0:8a5497a2e366 | 105 | (((green_led_states & (1 << 1)) == 0) << 3) + |
jah128 | 0:8a5497a2e366 | 106 | (((green_led_states & (1)) == 0) << 1) + |
jah128 | 0:8a5497a2e366 | 107 | (((red_led_states & (1 << 3)) == 0) << 6) + |
jah128 | 0:8a5497a2e366 | 108 | (((red_led_states & (1 << 2)) == 0) << 4) + |
jah128 | 0:8a5497a2e366 | 109 | (((red_led_states & (1 << 1)) == 0) << 2) + |
jah128 | 0:8a5497a2e366 | 110 | (((red_led_states & (1)) == 0)); |
jah128 | 0:8a5497a2e366 | 111 | char led_byte_1 = (((green_led_states & (1 << 7)) == 0) << 7) + |
jah128 | 0:8a5497a2e366 | 112 | (((green_led_states & (1 << 6)) == 0) << 5) + |
jah128 | 0:8a5497a2e366 | 113 | (((green_led_states & (1 << 5)) == 0) << 3) + |
jah128 | 0:8a5497a2e366 | 114 | (((green_led_states & (1 << 4)) == 0) << 1) + |
jah128 | 0:8a5497a2e366 | 115 | (((red_led_states & (1 << 7)) == 0) << 6) + |
jah128 | 0:8a5497a2e366 | 116 | (((red_led_states & (1 << 6)) == 0) << 4) + |
jah128 | 0:8a5497a2e366 | 117 | (((red_led_states & (1 << 5)) == 0) << 2) + |
jah128 | 0:8a5497a2e366 | 118 | (((red_led_states & (1 << 4)) == 0)); |
jah128 | 0:8a5497a2e366 | 119 | IF_write_to_led_ic(led_byte_0,led_byte_1); |
jah128 | 0:8a5497a2e366 | 120 | } |