void deviceTrajectoryHandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
if (header == COMMAND_GET_ABSOLUTE_POSITION) {
appendAck(outputStream);
updateTrajectoryWithNoThreshold();
append(outputStream, COMMAND_GET_ABSOLUTE_POSITION);
notifyAbsolutePositionWithoutHeader(outputStream);
}
else if (header == COMMAND_DEBUG_GET_ABSOLUTE_POSITION) {
appendAck(outputStream);
updateTrajectoryWithNoThreshold();
append(outputStream, COMMAND_DEBUG_GET_ABSOLUTE_POSITION);
OutputStream* debugOutputStream = getOutputStreamLogger(ALWAYS);
printPosition(debugOutputStream);
}
else if (header == COMMAND_SET_ABSOLUTE_POSITION) {
long newX = readHex4(inputStream);
inputStream->readChar(inputStream);
long newY = readHex4(inputStream);
inputStream->readChar(inputStream);
long newAngle = readHex4(inputStream);
appendAck(outputStream);
OutputStream* debugOutputStream = getDebugOutputStreamLogger();
appendStringAndDec(debugOutputStream, "newX=", newX);
appendStringAndDec(debugOutputStream, ",newY=", newY);
appendStringAndDec(debugOutputStream, ",newAngle=", newAngle);
float fX = (float) newX;
float fY = (float) newY;
float fAngle = PI_DIVIDE_1800 * (float) newAngle;
appendStringAndDecf(debugOutputStream, "fX=", fX);
appendStringAndDecf(debugOutputStream, ",fY=", fY);
appendStringAndDecf(debugOutputStream, ",fAngle=", fAngle);
setPosition(fX, fY, fAngle);
append(outputStream, COMMAND_SET_ABSOLUTE_POSITION);
}
}
void deviceRobotInfraredDetectorHandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
// Command to ask
if (header == COMMAND_INFRARED_DETECTOR_DETECTION) {
appendAck(outputStream);
append(outputStream, COMMAND_INFRARED_DETECTOR_DETECTION);
int type = readHex2(inputStream);
BOOL hasDetected;
if (type == DETECTOR_FORWARD_INDEX) {
hasDetected = getRobotInfraredObstacleForward();
}
else {
hasDetected = getRobotInfraredObstacleBackward();
}
// Send argument
if (hasDetected) {
appendHex2(outputStream, 1);
}
else {
appendHex2(outputStream, 0);
}
}
}
void deviceTestHandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
if (header == COMMAND_TEST) {
int arg1 = readHex2(inputStream);
int arg2 = readHex2(inputStream);
int result = arg1 + arg2;
// data
appendAck(outputStream);
append(outputStream, COMMAND_TEST);
appendHex2(outputStream, result);
}
/* TODO
else if (header == COMMAND_NOTIFY_TEST) {
int argument = readHex2(inputStream);
argument *= 2;
Buffer* buffer = getI2CSlaveOutputBuffer();
OutputStream* i2cOutputStream = getOutputStream(buffer);
// Add the value to I2C
append(i2cOutputStream, COMMAND_NOTIFY_TEST);
appendHex2(i2cOutputStream, argument);
// Response to the call
appendAck(outputStream);
append(outputStream, COMMAND_NOTIFY_TEST);
appendHex2(outputStream, argument);
}
*/
}
void devicePinHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) {
if (header == COMMAND_SET_PIN_VALUE) {
int pinIndex = readHex2(inputStream);
int pinValue = readHex2(inputStream);
appendAck(outputStream);
setPinValue(pinIndex, pinValue);
append(outputStream, COMMAND_SET_PIN_VALUE);
} else if (header == COMMAND_GET_PIN_VALUE) {
int pinIndex = readHex2(inputStream);
appendAck(outputStream);
int pinValue = getPinValue(pinIndex);
// Response
append(outputStream, COMMAND_GET_PIN_VALUE);
appendHex2(outputStream, pinValue);
}
}
void deviceStartMatchDetectorHandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
if (header == COMMAND_STEP_BY_STEP) {
appendAck(outputStream);
robotNextStep();
append(outputStream, COMMAND_STEP_BY_STEP);
}
}
void deviceArm2012HandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
if (header == COMMAND_ARM_2012_UP) {
int armIndex = readHex2(inputStream);
upArm(armIndex);
appendAck(outputStream);
append(outputStream, COMMAND_ARM_2012_UP);
} else if (header == COMMAND_ARM_2012_DOWN) {
int armIndex = readHex2(inputStream);
downArm(armIndex);
appendAck(outputStream);
append(outputStream, COMMAND_ARM_2012_DOWN);
}
}
void deviceConfigHandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
if (header == COMMAND_CONFIG) {
// Send ack first
appendAck(outputStream);
// can take a little time
refreshConfig();
append(outputStream, COMMAND_CONFIG);
appendHex4(outputStream, config);
}
}
void deviceRelayHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
if (commandHeader == COMMAND_SET_RELAY) {
int relayIndex = readHex2(inputStream);
unsigned char value = inputStream->readChar(inputStream);
if (relayIndex == 0) {
LATDbits.LATD8 = value;
}
else if (relayIndex == 1) {
LATDbits.LATD9 = value;
}
appendAck(outputStream);
append(outputStream, COMMAND_SET_RELAY);
}
}
void deviceAirConditionningBoardHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) {
if (header == COMMAND_AIR_CONDITIONNING) {
int powerState = readHex2(inputStream);
if (powerState != 0x00) {
pwmServoAll(SERVO_SPEED, SERVO_VALUE_TOUCH);
delaymSec(1000);
pwmServoAll(SERVO_SPEED, SERVO_VALUE_STAND_BY);
} else {
pwmServoAll(SERVO_SPEED, SERVO_VALUE_TOUCH);
delaymSec(1000);
pwmServoAll(SERVO_SPEED, SERVO_VALUE_STAND_BY);
}
appendAck(outputStream);
append(outputStream, COMMAND_AIR_CONDITIONNING);
}
}
void deviceSystemHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) {
if (header == COMMAND_PING) {
// data
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_PING);
// Read and write in output the pingIndex (to control that it's the right which does the response)
unsigned char pingIndex = readHex2(inputStream);
appendHex2(outputStream, pingIndex);
}
// Last Error
else if (header == COMMAND_GET_LAST_ERROR) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_GET_LAST_ERROR);
unsigned int lastError = getLastError();
appendHex4(outputStream, lastError);
}
else if (header == COMMAND_CLEAR_LAST_ERROR) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_CLEAR_LAST_ERROR);
clearLastError();
}
// Device list
else if (header == COMMAND_DEVICE_LIST) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_DEVICE_LIST);
printDeviceList(getInfoOutputStreamLogger());
// Usage
} else if (header == COMMAND_USAGE) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE);
printDeviceListUsage(getInfoOutputStreamLogger(), false);
} else if (header == COMMAND_USAGE_PROBLEM) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE_PROBLEM);
printDeviceListUsage(getInfoOutputStreamLogger(), true);
}
else if (header == COMMAND_USAGE_SPECIFIC_DEVICE) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE_SPECIFIC_DEVICE);
char deviceHeader = readBinaryChar(inputStream);
int size = getDeviceCount();
int i;
for (i = 0; i < size; i++) {
Device* device = getDevice(i);
if (deviceHeader == device->deviceInterface->deviceHeader) {
println(getInfoOutputStreamLogger());
printDeviceUsage(getInfoOutputStreamLogger(), device, false);
return;
}
}
appendString(getErrorOutputStreamLogger(), "Device Not Found ! ");
}
else if (header == COMMAND_CLS) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_CLS);
#ifdef PC_COMPILER
system("cls");
#else
appendString(outputStream, "Unsupported Operation");
#endif // PC_COMPILER
}
else if (header == COMMAND_RESET) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_RESET);
#ifdef PC_COMPILER
appendString(outputStream, "Unsupported Operation");
#else
// goto 0;
#endif // PC_COMPILER
}
// Notifications
else if (header == COMMAND_NOTIFICATION) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_NOTIFICATION);
printDeviceListNotification(getInfoOutputStreamLogger(), false);
} else if (header == COMMAND_WAIT) {
appendAck(outputStream);
int mSec = readHex4(inputStream);
delaymSec(mSec);
append(outputStream, SYSTEM_DEVICE_HEADER);
append(outputStream, COMMAND_WAIT);
} else if (header == COMMAND_BOARD_NAME) {
appendString(getInfoOutputStreamLogger(), getBoardName());
println(getInfoOutputStreamLogger());
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_BOARD_NAME);
}
}
