Reading more sensordata from the datastream

There was enough time in handling of the datastream to add more sensorpackets. So now the datastream includes bump sensors, wheel drops, cliff sensors & light bumpers:

case datastreamStarting:
	if ((millis() - commandTimer) > COMMANDTIMEOUT){
		//setup datastream
		Serial.write(OCDatastream); //start stream
		Serial.write(19); //19 packets
		Serial.write(7);  //1- Bumps & wheel drops U 1
		Serial.write(9);  //2- Clif left U 1
		Serial.write(10); //3- Clif front left U 1
		Serial.write(11); //4- Clif front right U 1
		Serial.write(12); //5- Clif right U 1
		Serial.write(14); //6- Wheel overcurrents U 1
		Serial.write(15); //7- dirt detect U 1
		Serial.write(21); //8- Charging state U 1
		Serial.write(22); //9- voltage U 2
		Serial.write(23); //10- current S 2
		Serial.write(24); //11- Battery temp S 1
		Serial.write(25); //12- Battery charge U 2
		Serial.write(26); //13- Battery capacity U 2
		Serial.write(34); //14- Charging sources available U 1
		Serial.write(35); //15- OI mode U 1
		Serial.write(37); //16- Song playing? U 1
		Serial.write(43); //17- Wheel encoder counts left S 2
		Serial.write(44); //18-Wheel encoder counts right S 2
		Serial.write(45); //19- Light bumper U 1
		//Number of packets 19, with 25 databytes 
		//serial write startDatastream
		dataState = datastreamWaitingForHeader;
		//Set timer
		messagetimeouttimer = millis();
	}
	break;
case datastreamCheckingData:
	if(Serial.available()){
		checksum += Serial.read(); //Addition to checksum should give 256 on rollover of byte to 0
		if (checksum == 0){
			//checksum passed
			bumpAndDropByte = databuffer[1];
			bumpRight = bitRead(bumpAndDropByte, 0); 
			bumpLeft = bitRead(bumpAndDropByte, 1);
			wheelDropRight = bitRead(bumpAndDropByte, 2);
			wheelDropLeft = bitRead(bumpAndDropByte, 3);
			
			cliffLeft = databuffer[3];
			cliffFrontLeft = databuffer[5];
			cliffFrontRight = databuffer[7];
			cliffRight = databuffer[9];
			
			wheelOvercurrentByte = databuffer[11];
			sideBrushOvercurrent = bitRead(wheelOvercurrentByte, 0);
			mainBrushOvercurrent = bitRead(wheelOvercurrentByte, 2);
			rightWheelOvercurrent = bitRead(wheelOvercurrentByte, 3);
			leftWheelOvercurrent = bitRead(wheelOvercurrentByte, 4);
			
			dirtDetect = databuffer[13];
			chargingState = databuffer[15];
			batteryVoltage = (databuffer[17] << 8) | databuffer[18];
			current = (databuffer[20] << 8) | databuffer[21];
			batteryTemp = databuffer[23];
			batteryCharge = (databuffer[25] << 8) | databuffer[26];
			batteryCapacity = (databuffer[28] << 8) | databuffer[29];
			docked = bitRead(databuffer[31],1);
			OImode = databuffer[33];
			songPlaying = databuffer[35];
			encoderLeft = (databuffer[37] << 8) | databuffer[38];
			encoderRight = (databuffer[40] << 8) | databuffer[41];
			
			lightBumpersByte = databuffer[43];
			ltBumperLeft = bitRead(lightBumpersByte, 0);
			ltBumperFrontLeft = bitRead(lightBumpersByte, 1);
			ltBumperCenterLeft = bitRead(lightBumpersByte, 2);
			ltBumperCenterRight = bitRead(lightBumpersByte, 3);
			ltBumperFrontRight = bitRead(lightBumpersByte, 4);
			ltBumperRight = bitRead(lightBumpersByte, 5);
			//debugVal = lightBumpersByte;
			
			p_encodersReady = true;
			errorCounter = 0;
			if(docked){
				dataState = datastreamStopping;
			}
			else{
				dataState = datastreamWaitingForHeader;
			}
		}
		else{
			//checksum failed
			errorCounter++;
			dataState = datastreamWaitingForHeader;
			if (errorCounter == DATASTREAMMAXERROR){
				dataState = datastreamStopping;
		   }
		}
	}
	//timeout
	else if ((millis() - messagetimeouttimer) > CHARGINGMESSAGETIMEOUT){
		//dataState = datastreamWaitingForHeader;
		errorCounter++;
		dataState = datastreamStart;
		if (errorCounter == DATASTREAMMAXERROR){
			dataState = dataReset;
		}
	}
	break;

 The state machine for handling of the charging message is also improved. (I think) It would go to an idle state sometimes when on the charging dock. This would somehow mess up all kind of things.

I think this would occur when a different message than the charging header would come (checking of the FET's for instance)

Hoping it's stable now and moving on to displaying the detected walls on the generated map.