 
 // Odometry Setup
 void SetupOdometry(void);
-volatile unsigned long rightCount = 0;
-ISR(INT1_vect){rightCount++;}
-
-volatile unsigned long leftCount = 0;
-ISR(INT0_vect){leftCount++;}
+volatile long rightCount = 0;
+ISR(INT1_vect){
+	if (rightSpeed < 93){
+		rightCount++;
+	} else {
+		rightCount--;
+	}
+}
+
+volatile long leftCount = 0;
+ISR(INT0_vect){
+	if (leftSpeed > 93){
+		leftCount++;
+	} else {
+		leftCount--;
+	}
+}
 
 // I2C Response Buffer
 char i2c_buff[32];


	} else if(buff[0] == 'O'){
		// Write back odometry counts
		sprintf(i2c_buff, "%u %u\n", leftCount, rightCount);

		#ifdef DEBUG_ODOMETRY
			char buff[32];
			sprintf(buff, "Left: %u Right: %u\n", leftCount, rightCount);
			Serial.print(buff);
		#endif

	} else if(buff[0] == 'S'){
		// Write back message

		PORTB ^= (1<<PB5);

		char buff[32];
		sprintf(buff, "Left: %ld Right: %ld\n", leftCount, rightCount);
		Serial.print(buff);

		//Required Delay for Sonar Loop, Do Not Remove or Reduce
