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"); }