char FT232R_send(unsigned char * msg,unsigned char length)
{
if (USB_getMode() != DATAMODE)
if ( !USB_setMode(DATAMODE) ) //switch to data mode
return 0; //dataMode failed
USB_send(msg,length); //send the msg
return 1; //success
}
void CM730_SendStatus(unsigned char id, unsigned char error, unsigned int length, char* data) {
unsigned char checksum = 0;
char buffer[length + 6];
buffer[0] = 0xFF;
buffer[1] = 0xFF;
buffer[2] = id;
buffer[3] = 2 + length;
buffer[4] = error;
for (unsigned int i = 0; i < length; i++) {
buffer[5 + i] = data[i];
checksum += data[i];
}
buffer[length + 5] = (~(id + (2 + length) + error + checksum) & 0xFF);
USB_send(buffer, length + 6);
}
void CM730_Main() {
char usbBuffer[128];
char usbSendBuffer[64];
Servo_SetPowerState(0);
memset(&cm730_currentMessage, 0, sizeof(cm730_currentMessage));
memset(&cm730_lastMessage, 0, sizeof(cm730_lastMessage));
cm730_state = CM730_STATE_IDLE;
cm730_messageProcessed = 1;
uart0_SetBaudrate(AT91B_DBGU_BAUD_RATE);
mCM730OldUart0Callback = uart0_AssignCharReceivedCallback(CM730_ProcessChar);
mCM730OldUart1Callback = uart1_AssignCharReceivedCallback(CM730_ProcessServoChar);
#ifndef DEBUG_X86
mCM730OldUart0TXBufCallback = uart0_AssignTxReadyCallback(CM730_ExtTXBufferEmpty);
#endif
while (1) {
// Send chars in TX buffer via USB
if (CM730_TXBufferReadPos != CM730_TXBufferWritePos) {
unsigned char idx = 0;
do {
usbSendBuffer[idx] = CM730_TXBuffer[CM730_TXBufferReadPos];
idx++;
CM730_TXBufferReadPos = (CM730_TXBufferReadPos + 1) % TX_BUFFER_SIZE;
} while (CM730_TXBufferReadPos != CM730_TXBufferWritePos && idx < sizeof(usbSendBuffer));
USB_send(usbSendBuffer, idx);
}
// Look for USB chars, when available feed to parser
int read = USBRead_nonblocking(&usbBuffer[0], sizeof(usbBuffer));
if (read > 0) {
for (unsigned char i = 0; i < read; ++i) {
CM730_ProcessChar(usbBuffer[i]);
}
}
// Read button state
unsigned char v = 0;
if (UI_GetKeyState_NoDebounce(0)) v |= 0x1;
if (UI_GetKeyState_NoDebounce(1)) v |= 0x2;
cm730_data[P_BUTTON] = v;
// Update IMU
IMU_accel_update();
IMU_gyro_update();
WRITE_WORD(P_ACC_Z, imu_AccelX_raw);
WRITE_WORD(P_ACC_Y, imu_AccelZ_raw);
WRITE_WORD(P_ACC_X, imu_AccelY_raw);
WRITE_WORD(P_GYRO_Z, imu_GyroX_raw);
WRITE_WORD(P_GYRO_Y, imu_GyroY_raw);
WRITE_WORD(P_GYRO_X, imu_GyroZ_raw);
if (cm730_messageProcessed == 0) { // Message waiting
if (cm730_lastMessage.type == CM730_MY_ID || cm730_lastMessage.type == CM730_BROADCAST) {
//if (cm730_lastMessage.type == CM730_BROADCAST) {
// dbgu_printf("[CM730] Received broadcast\r\n");
//}
if (cm730_lastMessage.instruction == CM730_CMD_WRITE) {
if (cm730_lastMessage.length == 2) {
//dbgu_printf("[CM730] Received byte write request: %d @ %d\r\n", cm730_lastMessage.data[0], cm730_lastMessage.data[1]);
if (cm730_lastMessage.data[0] == P_DXL_POWER) {
Servo_SetPowerState(cm730_lastMessage.data[1]);
cm730_data[P_DXL_POWER] = cm730_lastMessage.data[1];
}
} else if (cm730_lastMessage.length == 3) {
//dbgu_printf("[CM730] Received word write request: %d @ %d\r\n", cm730_lastMessage.data[0], cm730_lastMessage.data[1] | (cm730_lastMessage.data[2] << 8));
}
if (cm730_lastMessage.type != CM730_BROADCAST) {
CM730_SendStatus(cm730_lastMessage.type, 0, 0, NULL);
}
} else if (cm730_lastMessage.instruction == CM730_CMD_PING) {
//dbgu_printf("[CM730] Received ping request\r\n");
if (cm730_lastMessage.type != CM730_BROADCAST) {
CM730_SendStatus(cm730_lastMessage.type, 0, 0, NULL);
}
} else if (cm730_lastMessage.instruction == CM730_CMD_SYNC_WRITE) {
//unsigned char start_addr = cm730_lastMessage.data[0];
//unsigned char len = cm730_lastMessage.data[1];
//for(unsigned char i = 2; i < cm730_lastMessage.length; i += (len + 1)) {
// unsigned char id = cm730_lastMessage.data[i];
// if (id == CM730_MY_ID) {
//dbgu_printf("[CM730] Received sync write request: %d bytes from %d\r\n", len, start_addr);
// }
//}
} else if (cm730_lastMessage.instruction == CM730_CMD_BULK_READ) {
struct parser_message bulk_message;
memcpy(&bulk_message, &cm730_lastMessage, sizeof(struct parser_message));
unsigned char num = (bulk_message.length - 1) / 3;
unsigned char last_id = 0xFF;
for(char i = 0; i < num; ++i) {
unsigned char id = bulk_message.data[(3*i) + 2];
unsigned char len = bulk_message.data[(3*i) + 1];
unsigned char addr = bulk_message.data[(3*i) + 3];
if (id == CM730_MY_ID) {
//dbgu_printf("[CM730] Received bulk read request for %d bytes from %d\r\n", len, addr);
if (last_id == 0xFF) { // We are the first in the list
//process
if (addr + (len - 1) >= sizeof(cm730_data)) {
CM730_SendStatus(CM730_MY_ID, CM730_ERROR_RANGE, 0, NULL);
} else {
CM730_SendStatus(CM730_MY_ID, 0, len, &cm730_data[addr]);
}
break;
} else {
// wait
while (1) {
cm730_messageProcessed = 1; // Message processed
//TODO: Add timeout
while (cm730_messageProcessed == 1); // Wait for new message
if (cm730_currentMessage.type == last_id) {
break;
}
}
if (addr + (len - 1) >= sizeof(cm730_data)) {
CM730_SendStatus(CM730_MY_ID, CM730_ERROR_RANGE, 0, NULL);
} else {
CM730_SendStatus(CM730_MY_ID, 0, len, &cm730_data[addr]);
}
break;
}
}
last_id = id;
}
} else {
dbgu_printf("[CM730] Unknown instruction: %d (data length: %d)\r\n", cm730_lastMessage.instruction, cm730_lastMessage.length);
}
//} else {
// dbgu_printf("[CM730] Received instruction for ID %d\r\n", cm730_lastMessage.type);
}
cm730_messageProcessed = 1; // Message processed
}
}
uart0_SetBaudrate(AT91_BAUD_RATE_EXT);
uart0_AssignCharReceivedCallback(mCM730OldUart0Callback);
uart1_AssignCharReceivedCallback(mCM730OldUart1Callback);
#ifndef DEBUG_X86
uart0_AssignTxReadyCallback(mCM730OldUart0TXBufCallback);
#endif
Servo_SetPowerState(1);
}
