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");
}
}
/**
*@brief Printf over Serial Port
*@param fd device file number
*@param *message String array
*@return none
*/
void SerialPrintf (int fd, char const *message, ...)
{
va_list argp ;
char buffer [1024] ;
va_start (argp, message) ;
vsnprintf (buffer, 1023, message, argp) ;
va_end (argp) ;
SerialPuts (fd, buffer) ;
}
int main(int argc, char* argv[])
{
unsigned int ser = SerialOpen(115200); //Open serial port with a 115200 baud rate
int a = 0;
SerialFlush(ser); //Clear TX and RX buffer, can be called periodically
SerialPuts(ser,"Serial communication using the Sensorian Shield.\r\n");
SerialPrintf(ser,"You are using device number %d.\r\n",ser);
SerialFlush(ser);
while(1)
{ if(SerialDataAvail(ser))
{
a = SerialGetchar(ser);
SerialPrintf(ser,"You sent %c.\r\n",(char)(a));
if(a == 'b') break;
}
}
SerialPrintf(ser,"Closing serial port ...");
SerialClose(ser); //Close serial port
return 0;
}
void initRoboteq()
{
roboteqHandle = SerialInit(roboteqPort, 115200, 8, 0, 0, 0, 0, 0);
SerialPuts(roboteqHandle, "!G 1 0000\r!G 2 0000\r");
}
