uartDrive.c

#pragma config(UART_Usage, UART1, uartUserControl, baudRate9600, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartUserControl, baudRate9600, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  enc1,           sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  enc2,           sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           motor2,        tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port10,          motor1,        tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

short waitForChar(short port) {
	while(true) {
		short c = getChar(port);
		if(c != -1) {
			return c;
		}
	}

	return -1;
}

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

	configureSerialPort(UART2, uartUserControl);
	setBaudRate(UART2, baudRate9600);

	int n = 0;
	writeDebugStreamLine("Clearing UART buffer...");
	while(getChar(UART1) != -1) {
		sleep(1);

		if(n % 500 == 0) {
			writeDebugStreamLine("Clearing UART buffer...");
		}

		n += 1;
	}

	while(true) {
		short e1 = -SensorValue[enc1];
		short e2 = SensorValue[enc2];

		sendChar(UART1, 0x55);

		sendChar(UART1, e1 & 0xFF);
		sendChar(UART1, (e1 >> 8) & 0xFF);

		sendChar(UART1, e2 & 0xFF);
		sendChar(UART1, (e2 >> 8) & 0xFF);

		unsigned char f = waitForChar(UART1);
		if(f == 0xAA) {
			short m1 = waitForChar(UART1);
			short m2 = waitForChar(UART1);

			writeDebugStreamLine("Got motor command: %x / %x", m1, m2);

			if(m1 > 0x7F) {
				m1 = (((~m1) + 1) & 0xFF;
				m1 *= -1;
			}

			if(m2 > 0x7F) {
				m2 = ((~m2) + 1) & 0xFF;
				m2 *= -1;
			}

			motor[motor1] = m1;
			motor[motor2] = m2;
		} else if(f != 0 && f != -1) {
			writeDebugStreamLine("%x", f);
		}
	}
}