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@5:8ae5aca33233, 2015-12-12 (annotated)

Committer:
alecselfridge
Date:
Sat Dec 12 02:59:41 2015 +0000
Revision:
5:8ae5aca33233
Parent:
4:b3c3bca604a6
Child:
6:012504bfe3e3
30% transmission success!

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