LE KICKASS TEAM / Mbed 2 deprecated RTOS_Lights_Master

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

main.cpp@4:b3c3bca604a6, 2015-12-12 (annotated)

Committer:
alecselfridge
Date:
Sat Dec 12 01:21:09 2015 +0000
Revision:
4:b3c3bca604a6
Parent:
3:25eba54bace2
Child:
5:8ae5aca33233
Added Pack, Unpack and checkSSEL_isr functions.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alecselfridge 2:f3ae10decc2f 1 /************************************************************************
alecselfridge 2:f3ae10decc2f 2 * Authors: Alec Selfridge/Dylan Smith
alecselfridge 2:f3ae10decc2f 3 * Date: 12/4/2015
alecselfridge 2:f3ae10decc2f 4 * Rev: 1.4
alecselfridge 2:f3ae10decc2f 5 * Project: RTOS Light Animations
alecselfridge 2:f3ae10decc2f 6 * Notes: The z-axis of the accelerometer is used to calculate the
alecselfridge 2:f3ae10decc2f 7 * "effective" angle that the board is tilted. First, the Z data
alecselfridge 2:f3ae10decc2f 8 * (0-65) is converted to a (0-90) scale. Then the actual
alecselfridge 2:f3ae10decc2f 9 * manipulation occurs by injecting the acc data into a response
alecselfridge 2:f3ae10decc2f 10 * formula in the form of ax^2 - by + c. The resulting angle has
alecselfridge 2:f3ae10decc2f 11 * no sign but it's magnitude is used to determine the rate at
alecselfridge 2:f3ae10decc2f 12 * which LEDs will "fill", thus simulating a gravity effect. To
alecselfridge 2:f3ae10decc2f 13 * account for both directions of tilt we check the polarity of
alecselfridge 2:f3ae10decc2f 14 * the x-axis. A negative value indicates a tilt to the right
alecselfridge 2:f3ae10decc2f 15 * and vice versa.
alecselfridge 2:f3ae10decc2f 16 * A 5-axis joystick is used to mix R, G, and B values to create
alecselfridge 2:f3ae10decc2f 17 * a custom color. The RGB LED is controled via PWM with a value
alecselfridge 2:f3ae10decc2f 18 * of 1.0 indicating "off" and vice versa.
alecselfridge 2:f3ae10decc2f 19 *
alecselfridge 2:f3ae10decc2f 20 ************************************************************************/
alecselfridge 2:f3ae10decc2f 21
alecselfridge 0:4555c427d7a8 22 #include "mbed.h"
alecselfridge 1:0a93e9e88ad3 23 #include "rtos.h"
alecselfridge 0:4555c427d7a8 24 #include "MMA7455.h"
alecselfridge 0:4555c427d7a8 25
alecselfridge 2:f3ae10decc2f 26 // response formula coefficients
alecselfridge 2:f3ae10decc2f 27 #define X2 0.0038
alecselfridge 2:f3ae10decc2f 28 #define X 0.0299
alecselfridge 2:f3ae10decc2f 29 #define C 0.1883
alecselfridge 0:4555c427d7a8 30
alecselfridge 4:b3c3bca604a6 31 #define UART0 0x4000C000
alecselfridge 4:b3c3bca604a6 32 #define UART0CTL (UART0 + 0x30)
alecselfridge 4:b3c3bca604a6 33
alecselfridge 0:4555c427d7a8 34 /*
alecselfridge 0:4555c427d7a8 35 **************************
alecselfridge 0:4555c427d7a8 36 Display Functions
alecselfridge 0:4555c427d7a8 37 **************************
alecselfridge 0:4555c427d7a8 38 */
alecselfridge 0:4555c427d7a8 39 void displayBanner();
alecselfridge 0:4555c427d7a8 40 void newline();
alecselfridge 0:4555c427d7a8 41
alecselfridge 0:4555c427d7a8 42 /*
alecselfridge 0:4555c427d7a8 43 **************************
alecselfridge 0:4555c427d7a8 44 Utility Functions
alecselfridge 0:4555c427d7a8 45 **************************
alecselfridge 0:4555c427d7a8 46 */
alecselfridge 2:f3ae10decc2f 47 void SerialInit(void);
alecselfridge 2:f3ae10decc2f 48 void AccInit(void);
alecselfridge 2:f3ae10decc2f 49 void LEDsInit(void);
alecselfridge 2:f3ae10decc2f 50 void RgbInit(void);
alecselfridge 2:f3ae10decc2f 51 void accFeed(void);
alecselfridge 0:4555c427d7a8 52 void sampleAcc(int period, int32_t data[3]);
alecselfridge 2:f3ae10decc2f 53 double getAngle(void);
alecselfridge 2:f3ae10decc2f 54 unsigned char mapLEDS(double v);
alecselfridge 2:f3ae10decc2f 55 void LEDPush(void const *args);
alecselfridge 2:f3ae10decc2f 56 void RGBpicker(void const *args);
alecselfridge 2:f3ae10decc2f 57 void testLED(void);
alecselfridge 2:f3ae10decc2f 58 void testRGB(void);
alecselfridge 4:b3c3bca604a6 59 int testSlave(void);
alecselfridge 4:b3c3bca604a6 60 void checkSSEL_isr(void);
alecselfridge 4:b3c3bca604a6 61 void unpackRGB (int RGB);
alecselfridge 4:b3c3bca604a6 62 void packRGB (int R, int G, int B);
alecselfridge 0:4555c427d7a8 63 /*
alecselfridge 0:4555c427d7a8 64 **************************
alecselfridge 0:4555c427d7a8 65 Global Data
alecselfridge 0:4555c427d7a8 66 **************************
alecselfridge 0:4555c427d7a8 67 */
alecselfridge 0:4555c427d7a8 68 const char banner[37] = "Serial Comm Established with LPC4088";
alecselfridge 0:4555c427d7a8 69 // x, y, z
alecselfridge 0:4555c427d7a8 70 // holds current position
alecselfridge 0:4555c427d7a8 71 int32_t accPos[3] = {};
alecselfridge 0:4555c427d7a8 72 // holds up to 20 samples of data
alecselfridge 0:4555c427d7a8 73 int32_t accData[20][20][20] = {};
alecselfridge 0:4555c427d7a8 74 // holds calibration offsets
alecselfridge 0:4555c427d7a8 75 int32_t accCal[3] = {};
alecselfridge 2:f3ae10decc2f 76 // holds last acc value
alecselfridge 2:f3ae10decc2f 77 int32_t acc_old = 0;
alecselfridge 2:f3ae10decc2f 78 // holds most recent, adjusted, acc value
alecselfridge 2:f3ae10decc2f 79 double acc_adj = 0.0;
alecselfridge 2:f3ae10decc2f 80 // a value representing what's shown on the LEDs
alecselfridge 2:f3ae10decc2f 81 uint8_t LEDvals = 0;
alecselfridge 2:f3ae10decc2f 82 // tracker of how many times the joystick was used
alecselfridge 2:f3ae10decc2f 83 uint8_t tick = 0;
alecselfridge 2:f3ae10decc2f 84 // red value of RGB
alecselfridge 2:f3ae10decc2f 85 float r = 0;
alecselfridge 2:f3ae10decc2f 86 // green value of RGB
alecselfridge 2:f3ae10decc2f 87 float g = 0;
alecselfridge 2:f3ae10decc2f 88 // blue value of RGB
alecselfridge 2:f3ae10decc2f 89 float b = 0;
alecselfridge 0:4555c427d7a8 90
alecselfridge 4:b3c3bca604a6 91 int RGB = 0;
alecselfridge 4:b3c3bca604a6 92
alecselfridge 4:b3c3bca604a6 93 int RGBtemp = 0;
alecselfridge 4:b3c3bca604a6 94
alecselfridge 4:b3c3bca604a6 95 int reply = 0;
alecselfridge 4:b3c3bca604a6 96
alecselfridge 0:4555c427d7a8 97 /*
alecselfridge 0:4555c427d7a8 98 **************************
alecselfridge 0:4555c427d7a8 99 Objects
alecselfridge 0:4555c427d7a8 100 **************************
alecselfridge 0:4555c427d7a8 101 */
alecselfridge 0:4555c427d7a8 102 // UART connection to PC
alecselfridge 0:4555c427d7a8 103 Serial terminal(USBTX, USBRX);
alecselfridge 0:4555c427d7a8 104 // accelerometer on I2C bus
alecselfridge 0:4555c427d7a8 105 MMA7455 acc(P0_27, P0_28);
alecselfridge 4:b3c3bca604a6 106 // SPI interface for 8 LEDs & slave processor
alecselfridge 4:b3c3bca604a6 107 SPI leds(p5, p6, p7);
alecselfridge 1:0a93e9e88ad3 108 // load for shift register
alecselfridge 1:0a93e9e88ad3 109 DigitalOut ld(p30);
alecselfridge 4:b3c3bca604a6 110 // cs for SPI slave board
alecselfridge 4:b3c3bca604a6 111 DigitalOut cs(p8);
alecselfridge 2:f3ae10decc2f 112 // R of RGB LED
alecselfridge 2:f3ae10decc2f 113 PwmOut R(p25);
alecselfridge 2:f3ae10decc2f 114 // G of RGB LED
alecselfridge 2:f3ae10decc2f 115 PwmOut G(p28);
alecselfridge 2:f3ae10decc2f 116 // B of RGB LED
alecselfridge 2:f3ae10decc2f 117 PwmOut B(p26);
alecselfridge 2:f3ae10decc2f 118 // active-low 5-axis joystick
alecselfridge 2:f3ae10decc2f 119 // center, left_hi, right_hi, left_lo, right_lo
alecselfridge 2:f3ae10decc2f 120 BusIn joystick(p31, p32, p37, p39, p38);
alecselfridge 0:4555c427d7a8 121
alecselfridge 4:b3c3bca604a6 122 Ticker tock;
alecselfridge 4:b3c3bca604a6 123
alecselfridge 0:4555c427d7a8 124 int main() {
alecselfridge 0:4555c427d7a8 125 /*
alecselfridge 0:4555c427d7a8 126 **************************
alecselfridge 0:4555c427d7a8 127 Initializations
alecselfridge 0:4555c427d7a8 128 **************************
alecselfridge 0:4555c427d7a8 129 */
alecselfridge 0:4555c427d7a8 130 SerialInit();
alecselfridge 2:f3ae10decc2f 131 AccInit();
alecselfridge 1:0a93e9e88ad3 132 LEDsInit();
alecselfridge 2:f3ae10decc2f 133 testLED();
alecselfridge 2:f3ae10decc2f 134 RgbInit();
alecselfridge 2:f3ae10decc2f 135 testRGB();
alecselfridge 1:0a93e9e88ad3 136
alecselfridge 4:b3c3bca604a6 137 terminal.printf("Initializing slave...........");
alecselfridge 4:b3c3bca604a6 138 if(testSlave()) {
alecselfridge 4:b3c3bca604a6 139 terminal.printf("done.");
alecselfridge 4:b3c3bca604a6 140 newline();
alecselfridge 4:b3c3bca604a6 141 }
alecselfridge 4:b3c3bca604a6 142 else {
alecselfridge 4:b3c3bca604a6 143 terminal.printf("failed.");
alecselfridge 4:b3c3bca604a6 144 newline();
alecselfridge 4:b3c3bca604a6 145 }
alecselfridge 2:f3ae10decc2f 146 terminal.printf("Initializing timers..........");
alecselfridge 2:f3ae10decc2f 147 // timer responsible for updating the LEDs
alecselfridge 2:f3ae10decc2f 148 RtosTimer refresh_timer(LEDPush, osTimerPeriodic, (void *)0);
alecselfridge 2:f3ae10decc2f 149 // 16.7Hz timer (60ms or ~1/2 of 30fps)
alecselfridge 1:0a93e9e88ad3 150 refresh_timer.start(60);
alecselfridge 2:f3ae10decc2f 151 RtosTimer update_timer(RGBpicker, osTimerPeriodic, (void *)0);
alecselfridge 2:f3ae10decc2f 152 // 10Hz timer (100ms)
alecselfridge 2:f3ae10decc2f 153 update_timer.start(100);
alecselfridge 2:f3ae10decc2f 154 terminal.printf("done.");
alecselfridge 1:0a93e9e88ad3 155
alecselfridge 4:b3c3bca604a6 156 newline();
alecselfridge 4:b3c3bca604a6 157 tock.attach(checkSSEL_isr, 0.03);
alecselfridge 4:b3c3bca604a6 158 terminal.printf("Ticker started.");
alecselfridge 4:b3c3bca604a6 159 newline();
alecselfridge 4:b3c3bca604a6 160 newline();
alecselfridge 2:f3ae10decc2f 161 terminal.printf("Initialization complete.");
alecselfridge 2:f3ae10decc2f 162 newline();
alecselfridge 0:4555c427d7a8 163 /*
alecselfridge 0:4555c427d7a8 164 **************************
alecselfridge 0:4555c427d7a8 165 Main Execution
alecselfridge 0:4555c427d7a8 166 **************************
alecselfridge 0:4555c427d7a8 167 */
alecselfridge 2:f3ae10decc2f 168 while(true) {
alecselfridge 0:4555c427d7a8 169 acc.read(accPos[0], accPos[1], accPos[2]);
alecselfridge 2:f3ae10decc2f 170 acc_adj = getAngle();
alecselfridge 2:f3ae10decc2f 171 LEDvals = mapLEDS(acc_adj);
alecselfridge 0:4555c427d7a8 172 }
alecselfridge 0:4555c427d7a8 173 }
alecselfridge 0:4555c427d7a8 174
alecselfridge 0:4555c427d7a8 175 /*
alecselfridge 0:4555c427d7a8 176 **************************
alecselfridge 0:4555c427d7a8 177 Function Definitions
alecselfridge 0:4555c427d7a8 178 **************************
alecselfridge 0:4555c427d7a8 179 */
alecselfridge 0:4555c427d7a8 180 void SerialInit()
alecselfridge 0:4555c427d7a8 181 {
alecselfridge 2:f3ae10decc2f 182 // initialize connection to PC. default: 8N1
alecselfridge 0:4555c427d7a8 183 terminal.baud(19200);
alecselfridge 0:4555c427d7a8 184 displayBanner();
alecselfridge 0:4555c427d7a8 185 newline();
alecselfridge 0:4555c427d7a8 186 }
alecselfridge 0:4555c427d7a8 187
alecselfridge 0:4555c427d7a8 188 void AccInit()
alecselfridge 0:4555c427d7a8 189 {
alecselfridge 0:4555c427d7a8 190 // configure accelerometer for 2G range
alecselfridge 0:4555c427d7a8 191 acc.setMode(MMA7455::ModeMeasurement);
alecselfridge 0:4555c427d7a8 192 acc.setRange(MMA7455::Range_2g);
alecselfridge 4:b3c3bca604a6 193 terminal.printf("Calibrating accelerometer....");
alecselfridge 0:4555c427d7a8 194
alecselfridge 0:4555c427d7a8 195 // if we can successfully calibrate the accelerometer...
alecselfridge 0:4555c427d7a8 196 if(acc.calibrate()) {
alecselfridge 0:4555c427d7a8 197 newline();
alecselfridge 0:4555c427d7a8 198 acc.getCalibrationOffsets(accCal[0], accCal[1], accCal[2]);
alecselfridge 2:f3ae10decc2f 199 terminal.printf(" Offsets are (x,y,z): (%d, %d, %d)", accCal[0], accCal[1], accCal[2]);
alecselfridge 0:4555c427d7a8 200 newline(); newline();
alecselfridge 0:4555c427d7a8 201 }
alecselfridge 0:4555c427d7a8 202 else {
alecselfridge 0:4555c427d7a8 203 terminal.printf("failed.");
alecselfridge 0:4555c427d7a8 204 newline(); newline();
alecselfridge 0:4555c427d7a8 205 }
alecselfridge 0:4555c427d7a8 206 }
alecselfridge 0:4555c427d7a8 207
alecselfridge 2:f3ae10decc2f 208 void LEDsInit()
alecselfridge 2:f3ae10decc2f 209 {
alecselfridge 2:f3ae10decc2f 210 terminal.printf("Initializing LED array.......");
alecselfridge 2:f3ae10decc2f 211 leds.format(8, 3); // 8-bit packet, polarity & phase mode 3
alecselfridge 2:f3ae10decc2f 212 leds.frequency(100000); // 1MHz SPI
alecselfridge 2:f3ae10decc2f 213 ld = 1;
alecselfridge 2:f3ae10decc2f 214 LEDvals = 0x00;
alecselfridge 2:f3ae10decc2f 215 terminal.printf("done.");
alecselfridge 2:f3ae10decc2f 216 newline();
alecselfridge 2:f3ae10decc2f 217 }
alecselfridge 2:f3ae10decc2f 218
alecselfridge 2:f3ae10decc2f 219 void RgbInit()
alecselfridge 2:f3ae10decc2f 220 {
alecselfridge 2:f3ae10decc2f 221 terminal.printf("Initializing RGB LED.........");
alecselfridge 2:f3ae10decc2f 222 // 1KHz
alecselfridge 2:f3ae10decc2f 223 R.period(.001);
alecselfridge 2:f3ae10decc2f 224 R = 1.0;
alecselfridge 2:f3ae10decc2f 225 G.period(.001);
alecselfridge 2:f3ae10decc2f 226 G = 1.0;
alecselfridge 2:f3ae10decc2f 227 B.period(.001);
alecselfridge 2:f3ae10decc2f 228 B = 1.0;
alecselfridge 2:f3ae10decc2f 229 terminal.printf("done.");
alecselfridge 2:f3ae10decc2f 230 newline();
alecselfridge 2:f3ae10decc2f 231 }
alecselfridge 2:f3ae10decc2f 232
alecselfridge 0:4555c427d7a8 233 void newline()
alecselfridge 0:4555c427d7a8 234 {
alecselfridge 0:4555c427d7a8 235 // newline = carriage return + line feed
alecselfridge 0:4555c427d7a8 236 terminal.putc('\n');
alecselfridge 0:4555c427d7a8 237 terminal.putc('\r');
alecselfridge 0:4555c427d7a8 238 }
alecselfridge 0:4555c427d7a8 239
alecselfridge 0:4555c427d7a8 240 /*
alecselfridge 0:4555c427d7a8 241 Displays the following header:
alecselfridge 0:4555c427d7a8 242 ************************************************
alecselfridge 0:4555c427d7a8 243 Serial Comm Established with LPC4088
alecselfridge 0:4555c427d7a8 244 ************************************************
alecselfridge 0:4555c427d7a8 245 */
alecselfridge 0:4555c427d7a8 246 void displayBanner()
alecselfridge 0:4555c427d7a8 247 {
alecselfridge 0:4555c427d7a8 248 int i = 0;
alecselfridge 0:4555c427d7a8 249 for(int j = 0; j < 48; j++)
alecselfridge 0:4555c427d7a8 250 terminal.putc('*');
alecselfridge 0:4555c427d7a8 251 newline();
alecselfridge 0:4555c427d7a8 252
alecselfridge 0:4555c427d7a8 253 while(i != 36) {
alecselfridge 0:4555c427d7a8 254 char c = banner[i];
alecselfridge 0:4555c427d7a8 255 terminal.putc(c);
alecselfridge 0:4555c427d7a8 256 i++;
alecselfridge 0:4555c427d7a8 257 }
alecselfridge 0:4555c427d7a8 258 newline();
alecselfridge 0:4555c427d7a8 259
alecselfridge 0:4555c427d7a8 260 for(int j = 0; j < 48; j++)
alecselfridge 0:4555c427d7a8 261 terminal.putc('*');
alecselfridge 0:4555c427d7a8 262 }
alecselfridge 0:4555c427d7a8 263
alecselfridge 0:4555c427d7a8 264 // prints the current positional data from the accelerometer
alecselfridge 0:4555c427d7a8 265 void accFeed()
alecselfridge 0:4555c427d7a8 266 {
alecselfridge 0:4555c427d7a8 267 // returns false if the mode is set to standby or unable to convert
alecselfridge 0:4555c427d7a8 268 if(acc.read(accPos[0], accPos[1], accPos[2])) {
alecselfridge 0:4555c427d7a8 269 terminal.printf("x: %d y: %d z: %d", accPos[0], accPos[1], accPos[2]);
alecselfridge 0:4555c427d7a8 270 newline();
alecselfridge 0:4555c427d7a8 271 }
alecselfridge 0:4555c427d7a8 272 else {
alecselfridge 2:f3ae10decc2f 273 terminal.printf("Unable to access MMA7455.");
alecselfridge 0:4555c427d7a8 274 newline();
alecselfridge 0:4555c427d7a8 275 }
alecselfridge 0:4555c427d7a8 276 }
alecselfridge 0:4555c427d7a8 277
alecselfridge 0:4555c427d7a8 278 /*
alecselfridge 0:4555c427d7a8 279 Samples the accelerometer in 1/4s intervals for the length of "period" (max 5).
alecselfridge 0:4555c427d7a8 280 The results are placed in the 3D array. Additionally, the array passed in
alecselfridge 0:4555c427d7a8 281 will hold the last reading.
alecselfridge 0:4555c427d7a8 282 */
alecselfridge 0:4555c427d7a8 283 void sampleAcc(int period, int32_t data[3])
alecselfridge 0:4555c427d7a8 284 {
alecselfridge 0:4555c427d7a8 285 for(int i = 0; i < period*4; i++) {
alecselfridge 0:4555c427d7a8 286 //load temps
alecselfridge 0:4555c427d7a8 287 acc.read(data[0], data[1], data[2]);
alecselfridge 0:4555c427d7a8 288 accData[i][0][0] = data[0]; // x
alecselfridge 0:4555c427d7a8 289 accData[0][i][0] = data[1]; // y
alecselfridge 0:4555c427d7a8 290 accData[0][0][i] = data[2]; // z
alecselfridge 0:4555c427d7a8 291 wait(.25);
alecselfridge 0:4555c427d7a8 292 }
alecselfridge 0:4555c427d7a8 293 // if we didn't fill the whole array, we'll clear it to avoid confusion later
alecselfridge 0:4555c427d7a8 294 if(period < 5) {
alecselfridge 0:4555c427d7a8 295 for(int i = period*4; i < 20; i++)
alecselfridge 0:4555c427d7a8 296 accData[i][i][i] = 0;
alecselfridge 0:4555c427d7a8 297 }
alecselfridge 1:0a93e9e88ad3 298 }
alecselfridge 1:0a93e9e88ad3 299
alecselfridge 2:f3ae10decc2f 300 // called by the refresh timer every 60ms
alecselfridge 2:f3ae10decc2f 301 void LEDPush(void const *args)
alecselfridge 2:f3ae10decc2f 302 {
alecselfridge 2:f3ae10decc2f 303 ld = 0; // active-low load
alecselfridge 2:f3ae10decc2f 304 leds.write(LEDvals);
alecselfridge 2:f3ae10decc2f 305 ld = 1; // "lock" lights
alecselfridge 2:f3ae10decc2f 306 }
alecselfridge 2:f3ae10decc2f 307
alecselfridge 2:f3ae10decc2f 308 void RGBPush(void)
alecselfridge 2:f3ae10decc2f 309 {
alecselfridge 2:f3ae10decc2f 310 R = r;
alecselfridge 2:f3ae10decc2f 311 G = g;
alecselfridge 2:f3ae10decc2f 312 B = b;
alecselfridge 2:f3ae10decc2f 313 }
alecselfridge 2:f3ae10decc2f 314
alecselfridge 2:f3ae10decc2f 315 /*
alecselfridge 2:f3ae10decc2f 316 -0-90 degrees = 0-65 units (deg/unit ratio)
alecselfridge 2:f3ae10decc2f 317 -90 - conversion = actual angle (a value of 65 means the device is flat)
alecselfridge 2:f3ae10decc2f 318 thus, Theta = 90 - (90/65 * x) (90/65 = 1.38)
alecselfridge 2:f3ae10decc2f 319 -Using ax^2 - by + c allows the data to fit into a custom model of about 1/3 scale
alecselfridge 2:f3ae10decc2f 320 Therefore, the final equation is: Theta = 90 - 3(ax^2 - by + c)
alecselfridge 2:f3ae10decc2f 321 */
alecselfridge 2:f3ae10decc2f 322 double getAngle(void)
alecselfridge 2:f3ae10decc2f 323 {
alecselfridge 2:f3ae10decc2f 324 double deg = 1.38 * accPos[2];
alecselfridge 2:f3ae10decc2f 325 double cal = 1.38 * accCal[2];
alecselfridge 2:f3ae10decc2f 326 return ( 90 - (3.0 * ((X2*deg*deg) - (X*cal) + C)) );
alecselfridge 2:f3ae10decc2f 327 }
alecselfridge 2:f3ae10decc2f 328
alecselfridge 2:f3ae10decc2f 329 // look-up table based on ranges
alecselfridge 2:f3ae10decc2f 330 // this setup gives a nonlinear response from 0-90 degrees
alecselfridge 2:f3ae10decc2f 331 unsigned char mapLEDS(double v)
alecselfridge 1:0a93e9e88ad3 332 {
alecselfridge 2:f3ae10decc2f 333 int angle = int(v);
alecselfridge 2:f3ae10decc2f 334 if(accPos[0] < 0) {
alecselfridge 2:f3ae10decc2f 335 if(angle < 8)
alecselfridge 2:f3ae10decc2f 336 return 0xFF;
alecselfridge 2:f3ae10decc2f 337 else if(angle >= 8 && angle < 15)
alecselfridge 2:f3ae10decc2f 338 return 0x7F;
alecselfridge 2:f3ae10decc2f 339 else if(angle >= 15 && angle < 25)
alecselfridge 2:f3ae10decc2f 340 return 0x3F;
alecselfridge 2:f3ae10decc2f 341 else if(angle >= 25 && angle < 35)
alecselfridge 2:f3ae10decc2f 342 return 0x1F;
alecselfridge 2:f3ae10decc2f 343 else if(angle >= 35 && angle < 40)
alecselfridge 2:f3ae10decc2f 344 return 0x0F;
alecselfridge 2:f3ae10decc2f 345 else if(angle >= 40 && angle < 50)
alecselfridge 2:f3ae10decc2f 346 return 0x07;
alecselfridge 2:f3ae10decc2f 347 else if(angle >= 50 && angle < 60)
alecselfridge 2:f3ae10decc2f 348 return 0x03;
alecselfridge 2:f3ae10decc2f 349 else if(angle >= 60 && angle < 70)
alecselfridge 2:f3ae10decc2f 350 return 0x01;
alecselfridge 2:f3ae10decc2f 351 else if(angle >= 70)
alecselfridge 2:f3ae10decc2f 352 return 0x00;
alecselfridge 2:f3ae10decc2f 353 else
alecselfridge 2:f3ae10decc2f 354 return 0xFF;
alecselfridge 2:f3ae10decc2f 355 }
alecselfridge 2:f3ae10decc2f 356 else {
alecselfridge 2:f3ae10decc2f 357 if(angle < 8)
alecselfridge 2:f3ae10decc2f 358 return 0xFF;
alecselfridge 2:f3ae10decc2f 359 else if(angle >= 8 && angle < 15)
alecselfridge 2:f3ae10decc2f 360 return 0xFE;
alecselfridge 2:f3ae10decc2f 361 else if(angle >= 15 && angle < 25)
alecselfridge 2:f3ae10decc2f 362 return 0xFC;
alecselfridge 2:f3ae10decc2f 363 else if(angle >= 25 && angle < 35)
alecselfridge 2:f3ae10decc2f 364 return 0xF8;
alecselfridge 2:f3ae10decc2f 365 else if(angle >= 35 && angle < 40)
alecselfridge 2:f3ae10decc2f 366 return 0xF0;
alecselfridge 2:f3ae10decc2f 367 else if(angle >= 40 && angle < 50)
alecselfridge 2:f3ae10decc2f 368 return 0xE0;
alecselfridge 2:f3ae10decc2f 369 else if(angle >= 50 && angle < 60)
alecselfridge 2:f3ae10decc2f 370 return 0xC0;
alecselfridge 2:f3ae10decc2f 371 else if(angle >= 60 && angle < 70)
alecselfridge 2:f3ae10decc2f 372 return 0x80;
alecselfridge 2:f3ae10decc2f 373 else if(angle >= 70)
alecselfridge 2:f3ae10decc2f 374 return 0x00;
alecselfridge 2:f3ae10decc2f 375 else
alecselfridge 2:f3ae10decc2f 376 return 0xFF;
alecselfridge 2:f3ae10decc2f 377 }
alecselfridge 1:0a93e9e88ad3 378 }
alecselfridge 1:0a93e9e88ad3 379
alecselfridge 2:f3ae10decc2f 380 /*
alecselfridge 2:f3ae10decc2f 381 joystick: 0 1 2 3 4
alecselfridge 2:f3ae10decc2f 382 center left_hi right_hi left_lo right_lo
alecselfridge 2:f3ae10decc2f 383 */
alecselfridge 2:f3ae10decc2f 384 void RGBpicker(void const *args)
alecselfridge 1:0a93e9e88ad3 385 {
alecselfridge 2:f3ae10decc2f 386 tick++;
alecselfridge 2:f3ae10decc2f 387 if(tick > 1) {
alecselfridge 2:f3ae10decc2f 388 tick = 0;
alecselfridge 2:f3ae10decc2f 389 return;
alecselfridge 2:f3ae10decc2f 390 }
alecselfridge 2:f3ae10decc2f 391 // left : R
alecselfridge 3:25eba54bace2 392 if(!(joystick[3])) {
alecselfridge 2:f3ae10decc2f 393 if(R.read() == 0.0)
alecselfridge 2:f3ae10decc2f 394 r = 1.0;
alecselfridge 2:f3ae10decc2f 395 r = r - .02;
alecselfridge 2:f3ae10decc2f 396 }
alecselfridge 2:f3ae10decc2f 397 // right : B
alecselfridge 3:25eba54bace2 398 if(!(joystick[2])) {
alecselfridge 2:f3ae10decc2f 399 if(B.read() == 0.0)
alecselfridge 2:f3ae10decc2f 400 b = 1.0;
alecselfridge 2:f3ae10decc2f 401 b = b - .02;
alecselfridge 2:f3ae10decc2f 402 }
alecselfridge 2:f3ae10decc2f 403 // up : G
alecselfridge 3:25eba54bace2 404 if(!(joystick[4])) {
alecselfridge 2:f3ae10decc2f 405 if(G.read() == 0.0)
alecselfridge 2:f3ae10decc2f 406 g = 1.0;
alecselfridge 2:f3ae10decc2f 407 g = g - .02;
alecselfridge 2:f3ae10decc2f 408 }
alecselfridge 3:25eba54bace2 409 // down : Reset
alecselfridge 3:25eba54bace2 410 if(!(joystick[1])) {
alecselfridge 3:25eba54bace2 411 r = 1.0; g = 1.0; b = 1.0;
alecselfridge 3:25eba54bace2 412 }
alecselfridge 2:f3ae10decc2f 413 // center : confirm changes
alecselfridge 3:25eba54bace2 414 if(!joystick[0]) {
alecselfridge 2:f3ae10decc2f 415 RGBPush();
alecselfridge 3:25eba54bace2 416 terminal.printf("RGB updated: (%3.2f, %3.2f, %3.2f)", ((1 - R.read())*100), ((1 - G.read())*100), ((1 - B.read())*100));
alecselfridge 2:f3ae10decc2f 417 newline();
alecselfridge 2:f3ae10decc2f 418 }
alecselfridge 1:0a93e9e88ad3 419 }
alecselfridge 1:0a93e9e88ad3 420
alecselfridge 2:f3ae10decc2f 421 // "chase" pattern to verify LED array
alecselfridge 2:f3ae10decc2f 422 void testLED(void)
alecselfridge 2:f3ae10decc2f 423 {
alecselfridge 2:f3ae10decc2f 424 LEDvals = 0x80;
alecselfridge 2:f3ae10decc2f 425 LEDPush(0);
alecselfridge 2:f3ae10decc2f 426 wait_ms(75);
alecselfridge 2:f3ae10decc2f 427 for(int i = 0; i < 8; i++) {
alecselfridge 2:f3ae10decc2f 428 LEDvals = LEDvals >> 1;;
alecselfridge 2:f3ae10decc2f 429 LEDPush(0);
alecselfridge 2:f3ae10decc2f 430 wait_ms(75);
alecselfridge 2:f3ae10decc2f 431 }
alecselfridge 2:f3ae10decc2f 432 LEDvals = 0x01;
alecselfridge 2:f3ae10decc2f 433 LEDPush(0);
alecselfridge 2:f3ae10decc2f 434 wait_ms(75);
alecselfridge 2:f3ae10decc2f 435 for(int i = 0; i < 8; i++) {
alecselfridge 2:f3ae10decc2f 436 LEDvals = LEDvals << 1;;
alecselfridge 2:f3ae10decc2f 437 LEDPush(0);
alecselfridge 2:f3ae10decc2f 438 wait_ms(75);
alecselfridge 2:f3ae10decc2f 439 }
alecselfridge 2:f3ae10decc2f 440 }
alecselfridge 2:f3ae10decc2f 441
alecselfridge 2:f3ae10decc2f 442 // cycles thru various colors to show functionality of an RGB LED
alecselfridge 2:f3ae10decc2f 443 void testRGB(void)
alecselfridge 1:0a93e9e88ad3 444 {
alecselfridge 2:f3ae10decc2f 445 r = 1.0; g = 1.0; b = 1.0;
alecselfridge 2:f3ae10decc2f 446 RGBPush();
alecselfridge 2:f3ae10decc2f 447 wait_ms(150);
alecselfridge 2:f3ae10decc2f 448 r = 0.0; g = 1.0; b = 1.0;
alecselfridge 2:f3ae10decc2f 449 RGBPush();
alecselfridge 2:f3ae10decc2f 450 wait_ms(150);
alecselfridge 2:f3ae10decc2f 451 r = .6; g = 0.0; b = 0.0;
alecselfridge 2:f3ae10decc2f 452 RGBPush();
alecselfridge 2:f3ae10decc2f 453 wait_ms(150);
alecselfridge 2:f3ae10decc2f 454 r = 1.0; g = 1.0; b = 0.0;
alecselfridge 2:f3ae10decc2f 455 RGBPush();
alecselfridge 2:f3ae10decc2f 456 wait_ms(150);
alecselfridge 2:f3ae10decc2f 457 r = 1.0; g = 0.0; b = 0.0;
alecselfridge 2:f3ae10decc2f 458 RGBPush();
alecselfridge 2:f3ae10decc2f 459 wait_ms(150);
alecselfridge 2:f3ae10decc2f 460 r = 1.0; g = 0.0; b = 1.0;
alecselfridge 2:f3ae10decc2f 461 RGBPush();
alecselfridge 2:f3ae10decc2f 462 wait_ms(150);
alecselfridge 2:f3ae10decc2f 463 r = 1.0; g = 1.0; b = 0.0;
alecselfridge 2:f3ae10decc2f 464 RGBPush();
alecselfridge 2:f3ae10decc2f 465 wait_ms(150);
alecselfridge 2:f3ae10decc2f 466 r = 0.0; g = 0.0; b = 1.0;
alecselfridge 2:f3ae10decc2f 467 RGBPush();
alecselfridge 2:f3ae10decc2f 468 wait_ms(150);
alecselfridge 2:f3ae10decc2f 469 r = 0.0; g = 1.0; b = 1.0;
alecselfridge 2:f3ae10decc2f 470 RGBPush();
alecselfridge 2:f3ae10decc2f 471 wait_ms(150);
alecselfridge 2:f3ae10decc2f 472 r = 1.0; g = 1.0; b = 1.0;
alecselfridge 2:f3ae10decc2f 473 RGBPush();
alecselfridge 2:f3ae10decc2f 474 wait_ms(150);
alecselfridge 4:b3c3bca604a6 475 }
alecselfridge 4:b3c3bca604a6 476
alecselfridge 4:b3c3bca604a6 477 // check if we got the proper reply from the slave.
alecselfridge 4:b3c3bca604a6 478 // this tells us if we've got a good comm channel
alecselfridge 4:b3c3bca604a6 479 int testSlave(void)
alecselfridge 4:b3c3bca604a6 480 {
alecselfridge 4:b3c3bca604a6 481 char t = 0;
alecselfridge 4:b3c3bca604a6 482 unsigned char reply = 0;
alecselfridge 4:b3c3bca604a6 483 // 3s timeout
alecselfridge 4:b3c3bca604a6 484 while(t < 300) {
alecselfridge 4:b3c3bca604a6 485 // keep sending init pattern until we get a reply
alecselfridge 4:b3c3bca604a6 486 cs = 0;
alecselfridge 4:b3c3bca604a6 487 reply = leds.write(0xAA);
alecselfridge 4:b3c3bca604a6 488 cs = 1;
alecselfridge 4:b3c3bca604a6 489 if(reply == 0x55)
alecselfridge 4:b3c3bca604a6 490 return 1;
alecselfridge 4:b3c3bca604a6 491 wait_ms(10);
alecselfridge 4:b3c3bca604a6 492 t++;
alecselfridge 4:b3c3bca604a6 493 }
alecselfridge 4:b3c3bca604a6 494 // timeout occured
alecselfridge 4:b3c3bca604a6 495 return 0;
alecselfridge 4:b3c3bca604a6 496 }
alecselfridge 4:b3c3bca604a6 497
alecselfridge 4:b3c3bca604a6 498 void unpackRGB (int RGB){
alecselfridge 4:b3c3bca604a6 499 //Shift R from bits 23:16 to 7:0 and XOR with 0x00 to isolate data.
alecselfridge 4:b3c3bca604a6 500 r = (float((RGB >> 16) ^ 0x00)/255.0);
alecselfridge 4:b3c3bca604a6 501 //Shift G from bits 15:8 to 7:0 and XOR with 0x00 to isolate data.
alecselfridge 4:b3c3bca604a6 502 g = (float((RGB >> 8) ^ 0x00)/255.0);
alecselfridge 4:b3c3bca604a6 503 //XOR B ^ 0x00 to isolate data.
alecselfridge 4:b3c3bca604a6 504 b = (float((RGB ) ^ 0x00)/255.0);
alecselfridge 4:b3c3bca604a6 505 printf("R: %f | G: %f | B: %f", r,g,b);
alecselfridge 4:b3c3bca604a6 506 RGBPush();
alecselfridge 4:b3c3bca604a6 507 }
alecselfridge 4:b3c3bca604a6 508
alecselfridge 4:b3c3bca604a6 509 void packRGB (int R, int G, int B){
alecselfridge 4:b3c3bca604a6 510 RGB = 0;
alecselfridge 4:b3c3bca604a6 511 //Shift R from bits 23:16 to 7:0 and XOR with 0x00 to isolate data.
alecselfridge 4:b3c3bca604a6 512 RGB = RGB|(int(r*255) << 16);
alecselfridge 4:b3c3bca604a6 513 //Shift G from bits 15:8 to 7:0 and XOR with 0x00 to isolate data.
alecselfridge 4:b3c3bca604a6 514 RGB = RGB|(int(g*255) << 8);
alecselfridge 4:b3c3bca604a6 515 //XOR B ^ 0x00 to isolate data.
alecselfridge 4:b3c3bca604a6 516 RGB = RGB|(int(b*255) );
alecselfridge 4:b3c3bca604a6 517 //spi.reply(RGB);
alecselfridge 4:b3c3bca604a6 518 }
alecselfridge 4:b3c3bca604a6 519
alecselfridge 4:b3c3bca604a6 520 void checkSSEL_isr(){
alecselfridge 4:b3c3bca604a6 521 if((~(ld | cs)) == 0){
alecselfridge 4:b3c3bca604a6 522 terminal.printf("ERROR: Both SPI Slave Selects are active!");
alecselfridge 4:b3c3bca604a6 523 newline();
alecselfridge 4:b3c3bca604a6 524 }
alecselfridge 4:b3c3bca604a6 525 }