 
   // Setup neopixel strip to be enabled with correct brightness and all pixels turned off
   _lightStrip.begin();
-  _lightStrip.setBrightness(64);
+  _lightStrip.setBrightness(255);
   for(uint16_t i=0; i<_lightStrip.numPixels(); i++)
     _lightStrip.setPixelColor(i, 0);
   _lightStrip.show();
   _lightCycleColours[0] = 0xFF0000;
-  _lightCycleColours[1] = 0x00FF00;
-  _lightCycleColours[2] = 0x0000FF;
-  _lightCycleColours[3] = 0xFF0000;
-
-  // Setup PWM control for motors and turn everything off to start with
-  pwm.begin();
-  pwm.setPWMFreq(40);
-  for (byte i = 0; i < 16; i++)
-    pwm.setPin(i, 0);
+  _lightCycleColours[1] = 0xFFFFFF;
+  _lightCycleColours[2] = 0x00FF00;
+  _lightCycleColours[3] = 0xFFFFFF;
+  _lightCycleColours[4] = 0x0000FF;
+  _lightCycleColours[5] = 0xFFFFFF;
+  _lightCycleColours[6] = 0xFF0000;
+
+  // Setup PWM control for motors and turn everything off to start with and give it a default pattern
+  _pwmDriver.begin();
+  _pwmDriver.setPWMFreq(40);
+  for (byte i = 0; i < 16; i++) {
+    _pwmDriver.setPin(i, 0);
+    if (i >= 9)
+      break;
+    _vibrationSettings[i].NumberValues = 3;
+    _vibrationSettings[i].Values[0].Start = 1500;
+    _vibrationSettings[i].Values[0].End = 2500;
+    _vibrationSettings[i].Values[0].Duration = 750;
+    _vibrationSettings[i].Values[1].Start = 2500;
+    _vibrationSettings[i].Values[1].End = 1500;
+    _vibrationSettings[i].Values[1].Duration = 750;
+    _vibrationSettings[i].Values[2].Start = 1500;
+    _vibrationSettings[i].Values[2].End = 1500;
+    _vibrationSettings[i].Values[2].Duration = 100;
+  }
 
   // Enable bluetooth interface
   _bluetoothSerial.begin(9600); 
 } 
 
 void lightPerformLoop() {
-  // Determine how long since we were last called - if we're in same millisecond then we do nothing  
-  uint64_t lastLightLoopTime = millis();
-  uint16_t timeSinceLast = (uint16_t)(lastLightLoopTime - _lightLoopLastTime);
+  // Determine how long since we were last called - if we're within threshold then we do nothing
+  uint64_t nowTime = millis();
+  uint16_t timeSinceLast = (uint16_t)(nowTime - _lightLoopLastTime);
   if (timeSinceLast < 50) // Without this arbitrary delay the lower the delay is the more likely we get serial data corruption, the joys of hard-coded assembly
     return;
-  _lightLoopLastTime = lastLightLoopTime;
+  _lightLoopLastTime = nowTime;
 
   // If lights are solid or off we can set direclty
   uint16_t pixelCount = _lightStrip.numPixels();

typedef LightMode;


// Bluetooth declarations
SoftwareSerial _bluetoothSerial(2, 3);
const byte _bluetoothBufferSize = 42;
char _bluetoothBuffer[_bluetoothBufferSize];
byte _bluetoothBufferLength = 0;

// Lighting declarations
Adafruit_NeoPixel _lightStrip = Adafruit_NeoPixel(30, 6, NEO_GRB + NEO_KHZ800);
unsigned long _lightLoopLastTime = 0;
LightMode _lightMode = LightModeCycle;
uint32_t _lightSolidColour = 0xFF0000;
uint16_t _lightCycleCurrentOffsetTime = 0; // How many ms are we into the current sequence for when we are colour cycling
uint8_t _lightCycleColourCount = 4;
uint32_t _lightCycleColours[10];
uint32_t _lightCycleDuration = 10000; // In ms
uint32_t _lightCycleBlendAmount = _lightCycleDuration * 1; // How many different colours are visible of total amount at any time (i.e. 30 means each LED is a different colour within the cycle, 60 means the same but with every second colour skipped and 1 means everything is the same colour)

// Vibration declarations
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
unsigned long _lastMotorCycle = 0;
unsigned char _motorSequence = 0;


void setup() {
  // Begin serial for debugging purposes

    _lightCycleColourCount = _bluetoothBuffer[10] & 0xFF;
    for (int offset = 11, i = 0; i < 10; i++, offset += 3)
      _lightCycleColours[i] = (((uint32_t)(_bluetoothBuffer[offset] & 0xFF)) << 16) | (((uint32_t)(_bluetoothBuffer[offset + 1]  & 0xFF)) << 8) | ((uint32_t)(_bluetoothBuffer[offset + 2]  & 0xFF));
      Serial.print(F("Switch to cycle with duration "));
      Serial.print(_lightCycleDuration);
      Serial.print(F("ms and "));
      Serial.print(_lightCycleColourCount);
      Serial.print(F("colours blending with "));
      Serial.println(_lightCycleBlendAmount);
  }
  else if (_bluetoothBuffer[1] == 'O')
    _lightMode = LightModeOff;
