The firmware of the Grove Node
Dependencies: BLE_API color_pixels mbed-src-nrf51822 nRF51822
Fork of BLE_LoopbackUART by
node.cpp@11:c0885b74a63a, 2015-06-04 (annotated)
- Committer:
- yihui
- Date:
- Thu Jun 04 12:39:32 2015 +0000
- Revision:
- 11:c0885b74a63a
- Parent:
- 10:f34ff4e47741
fixed direct control bug
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yihui | 9:84cb66d0375d | 1 | |
yihui | 9:84cb66d0375d | 2 | |
yihui | 10:f34ff4e47741 | 3 | #include "udriver.h" |
yihui | 9:84cb66d0375d | 4 | #include "mbed.h" |
yihui | 10:f34ff4e47741 | 5 | #include "BLEDevice.h" |
yihui | 9:84cb66d0375d | 6 | #include "UARTService.h" |
yihui | 10:f34ff4e47741 | 7 | #include "color_pixels.h" |
yihui | 9:84cb66d0375d | 8 | |
yihui | 11:c0885b74a63a | 9 | #define LOG(...) // { printf(__VA_ARGS__); } |
yihui | 10:f34ff4e47741 | 10 | |
yihui | 10:f34ff4e47741 | 11 | #define MAX_INIT_DATA 4 |
yihui | 10:f34ff4e47741 | 12 | #define MAX_SENOSR_DIMENSION 9 |
yihui | 10:f34ff4e47741 | 13 | #define MAX_ACTUATOR_DATA_BUF 16 |
yihui | 10:f34ff4e47741 | 14 | #define MAX_EQUATION_NUMBER 2 |
yihui | 10:f34ff4e47741 | 15 | #define MAX_TASK_NUMBER 8 |
yihui | 10:f34ff4e47741 | 16 | |
yihui | 10:f34ff4e47741 | 17 | |
yihui | 10:f34ff4e47741 | 18 | typedef struct { |
yihui | 10:f34ff4e47741 | 19 | uint8_t d; // dimension |
yihui | 10:f34ff4e47741 | 20 | uint8_t op; // operator |
yihui | 10:f34ff4e47741 | 21 | uint8_t extra; |
yihui | 10:f34ff4e47741 | 22 | float value; |
yihui | 10:f34ff4e47741 | 23 | } equation_t; |
yihui | 10:f34ff4e47741 | 24 | |
yihui | 10:f34ff4e47741 | 25 | typedef struct { |
yihui | 10:f34ff4e47741 | 26 | equation_t equation_list[MAX_EQUATION_NUMBER]; |
yihui | 10:f34ff4e47741 | 27 | uint8_t output[MAX_ACTUATOR_DATA_BUF]; |
yihui | 10:f34ff4e47741 | 28 | } task_t; |
yihui | 10:f34ff4e47741 | 29 | |
yihui | 10:f34ff4e47741 | 30 | |
yihui | 9:84cb66d0375d | 31 | |
yihui | 9:84cb66d0375d | 32 | extern UARTService *uartServicePtr; |
yihui | 10:f34ff4e47741 | 33 | extern volatile bool ble_is_connected; |
yihui | 9:84cb66d0375d | 34 | |
yihui | 10:f34ff4e47741 | 35 | extern driver_t analog_sensor_driver; |
yihui | 10:f34ff4e47741 | 36 | extern driver_t temperature_sensor_driver; |
yihui | 10:f34ff4e47741 | 37 | extern driver_t time_sensor_driver; |
yihui | 9:84cb66d0375d | 38 | |
yihui | 10:f34ff4e47741 | 39 | extern driver_t relay_driver; |
yihui | 10:f34ff4e47741 | 40 | extern driver_t led_driver; |
yihui | 10:f34ff4e47741 | 41 | extern driver_t color_led_strip_driver; |
yihui | 9:84cb66d0375d | 42 | |
yihui | 10:f34ff4e47741 | 43 | driver_t *sensor_list[] = { |
yihui | 9:84cb66d0375d | 44 | &analog_sensor_driver, |
yihui | 10:f34ff4e47741 | 45 | &temperature_sensor_driver, |
yihui | 9:84cb66d0375d | 46 | &time_sensor_driver, |
yihui | 9:84cb66d0375d | 47 | }; |
yihui | 9:84cb66d0375d | 48 | |
yihui | 10:f34ff4e47741 | 49 | driver_t *actuator_list[] = { |
yihui | 10:f34ff4e47741 | 50 | &relay_driver, |
yihui | 9:84cb66d0375d | 51 | &led_driver, |
yihui | 9:84cb66d0375d | 52 | &color_led_strip_driver |
yihui | 9:84cb66d0375d | 53 | }; |
yihui | 9:84cb66d0375d | 54 | |
yihui | 9:84cb66d0375d | 55 | |
yihui | 9:84cb66d0375d | 56 | driver_t *sensor; |
yihui | 9:84cb66d0375d | 57 | driver_t *actuator; |
yihui | 10:f34ff4e47741 | 58 | int sensor_index = 0; |
yihui | 10:f34ff4e47741 | 59 | int actuator_index = 0; |
yihui | 9:84cb66d0375d | 60 | void *sensor_object; |
yihui | 9:84cb66d0375d | 61 | void *actuator_object; |
yihui | 10:f34ff4e47741 | 62 | float sensor_data[MAX_SENOSR_DIMENSION] = {0,}; |
yihui | 10:f34ff4e47741 | 63 | float actuator_data[MAX_ACTUATOR_DATA_BUF] = {0,}; |
yihui | 9:84cb66d0375d | 64 | |
yihui | 10:f34ff4e47741 | 65 | int sensor_init_data[MAX_INIT_DATA] = {0, }; |
yihui | 10:f34ff4e47741 | 66 | int actuator_init_data[MAX_INIT_DATA] = {0, }; |
yihui | 10:f34ff4e47741 | 67 | |
yihui | 10:f34ff4e47741 | 68 | task_t task_list[MAX_TASK_NUMBER]; |
yihui | 10:f34ff4e47741 | 69 | uint32_t task_mask = 0; |
yihui | 10:f34ff4e47741 | 70 | |
yihui | 10:f34ff4e47741 | 71 | bool equation_test(equation_t *equation, float *data) |
yihui | 9:84cb66d0375d | 72 | { |
yihui | 10:f34ff4e47741 | 73 | bool result = false; |
yihui | 10:f34ff4e47741 | 74 | if (0x01 == equation->op) { // > |
yihui | 10:f34ff4e47741 | 75 | result = data[equation->d] > equation->value; |
yihui | 10:f34ff4e47741 | 76 | } else if (0x02 == equation->op) { // < |
yihui | 10:f34ff4e47741 | 77 | result = data[equation->d] < equation->value; |
yihui | 10:f34ff4e47741 | 78 | } else if (0x03 == equation->op) { // = |
yihui | 10:f34ff4e47741 | 79 | result = data[equation->d] == equation->value; |
yihui | 10:f34ff4e47741 | 80 | } |
yihui | 10:f34ff4e47741 | 81 | |
yihui | 10:f34ff4e47741 | 82 | return true; |
yihui | 10:f34ff4e47741 | 83 | } |
yihui | 9:84cb66d0375d | 84 | |
yihui | 9:84cb66d0375d | 85 | void node_init() |
yihui | 9:84cb66d0375d | 86 | { |
yihui | 10:f34ff4e47741 | 87 | sensor_index = 0; |
yihui | 10:f34ff4e47741 | 88 | sensor = sensor_list[sensor_index]; |
yihui | 11:c0885b74a63a | 89 | sensor_init_data[0] = p1; |
yihui | 11:c0885b74a63a | 90 | sensor_init_data[1] = p2; |
yihui | 9:84cb66d0375d | 91 | sensor->init(&sensor_object, sensor_init_data); |
yihui | 10:f34ff4e47741 | 92 | |
yihui | 10:f34ff4e47741 | 93 | actuator_index = 2; |
yihui | 10:f34ff4e47741 | 94 | actuator = actuator_list[actuator_index]; |
yihui | 11:c0885b74a63a | 95 | actuator_init_data[0] = p3; |
yihui | 11:c0885b74a63a | 96 | actuator_init_data[1] = p4; |
yihui | 10:f34ff4e47741 | 97 | actuator_init_data[2] = 30; |
yihui | 9:84cb66d0375d | 98 | actuator->init(&actuator_object, actuator_init_data); |
yihui | 9:84cb66d0375d | 99 | } |
yihui | 9:84cb66d0375d | 100 | |
yihui | 9:84cb66d0375d | 101 | void node_tick() |
yihui | 9:84cb66d0375d | 102 | { |
yihui | 9:84cb66d0375d | 103 | sensor->read(&sensor_object, sensor_data); |
yihui | 10:f34ff4e47741 | 104 | |
yihui | 10:f34ff4e47741 | 105 | |
yihui | 9:84cb66d0375d | 106 | for (int d = 0; d < sensor->d; d++) { |
yihui | 9:84cb66d0375d | 107 | float value = sensor_data[d]; |
yihui | 10:f34ff4e47741 | 108 | |
yihui | 10:f34ff4e47741 | 109 | if (ble_is_connected) { |
yihui | 10:f34ff4e47741 | 110 | uartServicePtr->printf("i %d %f\n", d, value); |
yihui | 10:f34ff4e47741 | 111 | } |
yihui | 10:f34ff4e47741 | 112 | } |
yihui | 10:f34ff4e47741 | 113 | |
yihui | 10:f34ff4e47741 | 114 | uint32_t mask = task_mask; |
yihui | 10:f34ff4e47741 | 115 | int task_index = 0; |
yihui | 10:f34ff4e47741 | 116 | while (mask) { |
yihui | 10:f34ff4e47741 | 117 | if (mask & 1) { |
yihui | 10:f34ff4e47741 | 118 | task_t *task = task_list + task_index; |
yihui | 10:f34ff4e47741 | 119 | bool result = false; |
yihui | 10:f34ff4e47741 | 120 | for (int i = 0; i < MAX_EQUATION_NUMBER; i++) { |
yihui | 10:f34ff4e47741 | 121 | equation_t *equation = task->equation_list + i; |
yihui | 10:f34ff4e47741 | 122 | if (0 == equation->extra) { |
yihui | 10:f34ff4e47741 | 123 | break; |
yihui | 10:f34ff4e47741 | 124 | } |
yihui | 10:f34ff4e47741 | 125 | |
yihui | 10:f34ff4e47741 | 126 | bool equation_result = equation_test(equation, sensor_data); |
yihui | 10:f34ff4e47741 | 127 | if ('&' == equation->extra) { |
yihui | 10:f34ff4e47741 | 128 | result = result && equation_result; |
yihui | 10:f34ff4e47741 | 129 | } else { |
yihui | 10:f34ff4e47741 | 130 | result = result || equation; |
yihui | 9:84cb66d0375d | 131 | } |
yihui | 9:84cb66d0375d | 132 | } |
yihui | 9:84cb66d0375d | 133 | |
yihui | 9:84cb66d0375d | 134 | if (result) { |
yihui | 10:f34ff4e47741 | 135 | actuator->write(&actuator_object, task->output); |
yihui | 9:84cb66d0375d | 136 | } |
yihui | 9:84cb66d0375d | 137 | } |
yihui | 10:f34ff4e47741 | 138 | |
yihui | 10:f34ff4e47741 | 139 | task_index++; |
yihui | 10:f34ff4e47741 | 140 | mask >>= 1; |
yihui | 9:84cb66d0375d | 141 | } |
yihui | 9:84cb66d0375d | 142 | } |
yihui | 9:84cb66d0375d | 143 | |
yihui | 9:84cb66d0375d | 144 | void node_parse(int argc, char *argv[]) |
yihui | 9:84cb66d0375d | 145 | { |
yihui | 11:c0885b74a63a | 146 | if (0 == strcmp(argv[0], "s")) { |
yihui | 9:84cb66d0375d | 147 | int param1 = atoi(argv[1]); |
yihui | 11:c0885b74a63a | 148 | if (param1 != sensor_index) { |
yihui | 11:c0885b74a63a | 149 | sensor_index = param1; |
yihui | 11:c0885b74a63a | 150 | sensor->fini(&sensor_object); |
yihui | 11:c0885b74a63a | 151 | sensor = sensor_list[param1]; |
yihui | 11:c0885b74a63a | 152 | sensor->init(&sensor_object, sensor_init_data); |
yihui | 10:f34ff4e47741 | 153 | |
yihui | 11:c0885b74a63a | 154 | task_mask = 0; |
yihui | 11:c0885b74a63a | 155 | } |
yihui | 10:f34ff4e47741 | 156 | |
yihui | 11:c0885b74a63a | 157 | LOG("select sensor [%d]\n", sensor_index); |
yihui | 11:c0885b74a63a | 158 | } else if (0 == strcmp(argv[0], "a")) { |
yihui | 11:c0885b74a63a | 159 | int param1 = atoi(argv[1]); |
yihui | 11:c0885b74a63a | 160 | if (param1 != actuator_index) { |
yihui | 11:c0885b74a63a | 161 | actuator_index = param1; |
yihui | 11:c0885b74a63a | 162 | actuator->fini(&actuator_object); |
yihui | 11:c0885b74a63a | 163 | actuator = actuator_list[param1]; |
yihui | 11:c0885b74a63a | 164 | actuator->init(&actuator_object, actuator_init_data); |
yihui | 11:c0885b74a63a | 165 | |
yihui | 11:c0885b74a63a | 166 | task_mask = 0; |
yihui | 11:c0885b74a63a | 167 | } |
yihui | 9:84cb66d0375d | 168 | |
yihui | 11:c0885b74a63a | 169 | LOG("select actuator [%d]\n", actuator_index); |
yihui | 11:c0885b74a63a | 170 | |
yihui | 10:f34ff4e47741 | 171 | |
yihui | 9:84cb66d0375d | 172 | } else if (0 == strcmp(argv[0], "o")) { |
yihui | 9:84cb66d0375d | 173 | |
yihui | 9:84cb66d0375d | 174 | int n = argc - 1; |
yihui | 9:84cb66d0375d | 175 | if (n > 8) { |
yihui | 9:84cb66d0375d | 176 | n = 8; |
yihui | 9:84cb66d0375d | 177 | } |
yihui | 10:f34ff4e47741 | 178 | |
yihui | 10:f34ff4e47741 | 179 | |
yihui | 9:84cb66d0375d | 180 | LOG("set output:"); |
yihui | 9:84cb66d0375d | 181 | for (int i = 0; i < 8; i++) { |
yihui | 9:84cb66d0375d | 182 | if (i < n) { |
yihui | 9:84cb66d0375d | 183 | float value = atof(argv[i + 1]); |
yihui | 9:84cb66d0375d | 184 | actuator_data[i] = value; |
yihui | 9:84cb66d0375d | 185 | LOG(" %f", value); |
yihui | 9:84cb66d0375d | 186 | } else { |
yihui | 9:84cb66d0375d | 187 | actuator_data[i] = 0; |
yihui | 9:84cb66d0375d | 188 | } |
yihui | 9:84cb66d0375d | 189 | } |
yihui | 10:f34ff4e47741 | 190 | |
yihui | 9:84cb66d0375d | 191 | LOG("\n"); |
yihui | 10:f34ff4e47741 | 192 | |
yihui | 9:84cb66d0375d | 193 | actuator->write(&actuator_object, actuator_data); |
yihui | 9:84cb66d0375d | 194 | } |
yihui | 10:f34ff4e47741 | 195 | |
yihui | 9:84cb66d0375d | 196 | } |