Merge pull request #252 from KurtE/MillisTimer_fix_timings

Event Responder - fix _ms when adding items

teensy3/EventResponder.cpp

 	if (_state != TimerOff) end();
 	if (!milliseconds) return;
 	_event = &event;
-	_ms = milliseconds;
+	_ms = (milliseconds > 2)? milliseconds-2 : 0;
 	_reload = 0;
 	addToWaitingList();
 }
 	if (_state != TimerOff) end();
 	if (!milliseconds) return;
 	_event = &event;
-	_ms = milliseconds;
+	_ms = (milliseconds > 2)? milliseconds-2 : 0;
 	_reload = milliseconds;
 	addToWaitingList();
 }
 	bool irq = disableTimerInterrupt();
 	_next = listWaiting;
 	listWaiting = this; // TODO: use STREX to avoid interrupt disable
+	_state = TimerWaiting;
 	enableTimerInterrupt(irq);
 }
 
 		_next = listActive;
 		_prev = nullptr;
 		listActive->_prev = this;
+		// Decrement the next items wait time be our wait time as to properly handle waits for all other items...
+		listActive->_ms -= _ms;	
 		listActive = this;
 	} else {
 		// add this timer somewhere after the first already on the list
 				_prev = timer->_prev;
 				timer->_prev = this;
 				_prev->_next = this;
+				timer->_ms -= _ms;
 				_state = TimerActive;
 				return;
 			}
 	if (s == TimerActive) {
 		if (_next) {
 			_next->_prev = _prev;
+			_next->_ms += _ms;   // add in the rest of our timing to next entry...
 		}
 		if (_prev) {
 			_prev->_next = _next;