Experimental UART-based remote control code

robotc_uart.c

#pragma config(Sensor, dgtl1,  quad1,          sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  quad2,          sensorQuadEncoder)
#pragma config(Motor,  port2,           m1,            tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           m2,            tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

signed char readUART(TUARTs port) {
	signed char val;
	while((val = getChar(port)) == -1) {
		sleep(1);
	}
	
	return val;
}

void clearUART(TUARTs port) {
	while(getChar(port) != -1) {
		sleep(1);
	}	
}

// send values in little-endian
void sendShort(TUARTs port, short val) {
	sendChar(port, val & 0xFF);
	sendChar(port, (val >> 8) & 0xFF);
}

void sendStatusMessage() {
	sendChar(UART1, 0xA5); // start-of-message byte
	sendChar(UART1, 0x01); // type byte
	
	/*
	 * <0xA5 start byte>
	 * <1-byte type>
	 * <short quadValue1>
	 * <short quadValue2>
	 * <short batteryLevel>
	 */
	
	sendShort(UART1, SensorValue[quad1]);
	sendShort(UART1, SensorValue[quad2]);
	sendShort(UART1, nAvgBatteryLevel);
}

task main()
{
	configureSerialPort(UART1, uartUserControl);
	setBaudRate(UART1, baudRate9600);
	
	clearUART(UART1);

	while(true) {
		sendStatusMessage();
		
		char startFlag = getChar(UART1);
		
		if(startFlag == 0xA5) {
			char msgType = readUART(UART1);
			
			switch(msgType) {
			case 0x02: // control message
				/*
				 * <...header...>
				 * <1-byte motor value 1>
				 * <1-byte motor value 2>
				 */
				 motor[m1] = readUART(UART1);
				 motor[m2] = readUART(UART2);
				 break;
			default:
					writeDebugStreamLine("Unrecognized message type: %x", msgType);
					break;
			}
		}
		
		sleep(20);
	}
}