void sendspeed(int leftSpeed, int rightSpeed)
{
char buffer[20];
static int errorCount;
#define DEBUGROBOTEQ //Print Speed Commands
#ifdef DEBUGROBOTEQ
DEBUG(leftSpeed); DEBUGN(rightSpeed);
#endif
if (roboteqHandle == INVALID_HANDLE_VALUE) {
// Either the port was not opened or there was a failure
// Try and open the robotEq again hope it works
initRoboteq();
}
if(ABS(leftSpeed) > 1000) leftSpeed = SIGN(leftSpeed) * 1000;
if(ABS(rightSpeed) > 1000) rightSpeed = SIGN(rightSpeed) * 1000;
sprintf(buffer, "!G %01d %04d\r!G %01d %04d\r",
leftChannelNum, leftSpeed, rightChannelNum,rightSpeed);
if (roboteqHandle != INVALID_HANDLE_VALUE) {
SerialPuts(roboteqHandle, buffer);
FlushRx(roboteqHandle);
FlushTx(roboteqHandle);
} else {
if (++errorCount % 100 == 0) // Print every 100 times this happens
fprintf(stderr, "Invalid roboteqPort. Did you open the port?\n");
}
}
int main(void)
{
// Prepare for UART communication with external world. The default baud rate
// is 921,600 bps
IOWR_FIFOED_AVALON_UART_DIVISOR(UART_BASE, BAUD_RATE(921600.0f));
// Make sure UART interrupts are disabled
alt_ic_irq_disable(UART_IRQ_INTERRUPT_CONTROLLER_ID, UART_IRQ);
// Clear the input and output buffers
FlushRx(UART_BASE);
FlushTx(UART_BASE);
#define MAX_CMD_LEN 64
char cmd[MAX_CMD_LEN];
s8 cmdIndex = 0;
// Sit in an infinite loop waiting for serial commands
while(1)
{
while (IORD_FIFOED_AVALON_UART_STATUS(UART_BASE) & FIFOED_AVALON_UART_CONTROL_RRDY_MSK)
{
// Read the Uart
char rx = IORD_FIFOED_AVALON_UART_RXDATA(UART_BASE);
// If this is the end of a command, then try to parse it
if (('\r' == rx) || ('\n' == rx))
{
cmd[cmdIndex] = '\0';
ExecuteCmd(cmd, UART_BASE);
FlushRx(UART_BASE);
FlushTx(UART_BASE);
cmdIndex = 0;
}
// If this is a backspace
else if ('\b' == rx)
{
SendStr("\b \b", UART_BASE);
if (cmdIndex > 0)
cmdIndex--;
}
// This is any other character
else
{
// echo the character
SendChar(rx, UART_BASE);
// Add it to the buffer, if possible, making sure to save the
// space for the null terminator (when completing the command)
if (cmdIndex < (MAX_CMD_LEN - 1))
cmd[cmdIndex++] = rx;
// Otherwise, report the error and reset the buffer
else
{
cmdIndex = 0;
SendStr(NO_ANSWER, UART_BASE);
}
}
}
}
}
static void ExecuteCmd(const char const *input, const u32 base)
{
SendStr("\r\n", base);
// Tokenize the command
#define MAX_CMD_WORDS 4
char *token[MAX_CMD_WORDS];
char *cmd = (char *)input;
u8 numTokens = 0;
while (1)
{
// Skip leading whitespace.
while ((*cmd) && isspace(*cmd))
cmd++;
// If we get here and we are at the end of the string, then the last
// token must have had trailing white spaces. Let's ignore them
if (!(*cmd))
break;
// If we have exceeded the maximum number of allowable tokens, then
// return as error
if (numTokens >= MAX_CMD_WORDS)
{
SendStr(NO_ANSWER, base);
return;
}
// Store the token.
token[numTokens] = cmd;
numTokens++;
// Everything that isn't a whitespace is part of the token. Let's make
// sure it is in UPPER CASE
while ((*cmd) && (!isspace(*cmd)))
{
*cmd = toupper(*cmd);
cmd++;
}
// When we get here, we are just past the current token, either because
// it ended on a whitespace or because it is the end of the user input.
// If the former, then let's force a null termination for that token. If
// the latter, then we are done tokenizing.
if (!(*cmd))
break;
*cmd = '\0';
cmd++;
}
if (0 == numTokens)
{
SendStr(NO_ANSWER, base);
return;
}
// Process the command
switch (token[0][0])
{
case 'R':
{
if (2 != numTokens)
SendStr(NO_ANSWER, base);
else
{
u32 regAddr;
u32 regValue;
if (StrToU32(token[1], ®Addr) && RegRead(regAddr, ®Value))
{
SendStr("Y ", base);
char regValStr[9];
U32ToStr(regValue, regValStr);
SendStr(regValStr, base);
SendStr("\r\n", base);
}
else
SendStr(NO_ANSWER, base);
}
break;
}
case 'W':
{
if (3 != numTokens)
SendStr(NO_ANSWER, base);
else
{
u32 regAddr;
u32 regValue;
if (StrToU32(token[1], ®Addr) && StrToU32(token[2], ®Value) && RegWrite(regAddr, regValue))
SendStr(YES_ANSWER, base);
else
SendStr(NO_ANSWER, base);
}
break;
}
case 'V':
{
SendStr("FPGA=0x", base);
char versionStr[9];
FpgaRegisters * FPGARegs = (FpgaRegisters *)(REGISTER_BASE | BYPASS_DCACHE_MASK);
U32ToStr(FPGARegs->fpgaVersion, versionStr);
SendStr(versionStr, base);
SendStr(" NIOS=0x", base);
U32ToStr(NIOS_VERSION, versionStr);
SendStr(versionStr, base);
SendStr("\r\n", base);
break;
}
case 'F':
{
if (4 != numTokens)
SendStr(NO_ANSWER, base);
else
{
u32 startAddr;
u32 length;
u32 checksum;
StrToU32(token[1], &startAddr);
StrToU32(token[2], &length);
StrToU32(token[3], &checksum);
// Transfer two chunks to get a full sector worth
#define FLASH_SECTOR_SIZE (64*1024)
#define TRANSFER_SIZE (4*1024)
u8 buffer[FLASH_SECTOR_SIZE];
// Validate the requested transfer size
if (length != TRANSFER_SIZE)
SendStr(NO_ANSWER, base);
else
{
u32 bufferIndex = startAddr % FLASH_SECTOR_SIZE;
u32 runningSum = 0;
u32 numBytesReceived = 0;
// Clear the input buffer
FlushRx(base);
// Acknowledge that the command is good. This will tell the
// sender to actually send the specified number of bytes
SendStr(YES_ANSWER, base);
// We must receive the correct number of bytes
while (true)
{
while (IORD_FIFOED_AVALON_UART_STATUS(base) & FIFOED_AVALON_UART_CONTROL_RRDY_MSK)
{
// Read the Uart
u8 rx = IORD_FIFOED_AVALON_UART_RXDATA(base);
runningSum += rx;
buffer[bufferIndex++] = rx;
numBytesReceived++;
if (numBytesReceived >= length)
break;
}
if (numBytesReceived >= length)
break;
}
// check the checksum
if (runningSum != checksum)
SendStr(NO_ANSWER, base);
else
{
// If we don't have a full sector worth of data, then ACK and wait for more
if (bufferIndex != FLASH_SECTOR_SIZE)
SendStr(YES_ANSWER, base);
else
{
u32 totalWriteBufferChecksum = 0;
int i;
for (i=0; i<sizeof(buffer); i++)
{
totalWriteBufferChecksum += buffer[i];
}
alt_flash_fd* fd = alt_flash_open_dev(SERIAL_FLASH_NAME);
if (NULL == fd)
SendStr(NO_ANSWER, base);
else
{
u32 sectorStartAddr = (startAddr / FLASH_SECTOR_SIZE) * FLASH_SECTOR_SIZE;
if (0 == alt_write_flash(fd, sectorStartAddr, buffer, length))
{
memset(buffer, 0x99, sizeof(buffer));
if (0 == alt_read_flash(fd, sectorStartAddr, buffer, sizeof(buffer)))
{
u32 totalReadBufferChecksum = 0;
for (i=0; i<sizeof(buffer); i++)
{
totalReadBufferChecksum += buffer[i];
}
if (totalReadBufferChecksum == totalWriteBufferChecksum)
SendStr(YES_ANSWER, base);
else
SendStr(NO_ANSWER, base);
}
else
SendStr(NO_ANSWER, base);
}
else
SendStr(NO_ANSWER, base);
alt_flash_close_dev(fd);
}
}
}
}
}
break;
}
default:
SendStr(NO_ANSWER, base);
break;
}
return;
}
