void draw()
{
background(0);
while(myPort.available()>0)
print((char)myPort.read());
}
bool waa::PC2Arduino(char *command){
int i = 0;
char buffer = '\0';
bool status = false;
//tstring commPortName(Comport);
//Serial serial(commPortName);
serial.flush();
do {
i++;
} while (strcmp(command, PCCommand[i]) != 0 || i >= DEF_ARDUINO_COMMAND_NUMBER);
if (i<DEF_ARDUINO_COMMAND_NUMBER && i >= 0)
{
//serial.write(ArduinoCommand+i);
serial.write(ArduinoCommand[i]);
Sleep(5);
serial.read(&buffer, 1);
Sleep(5);
if (buffer == '0') status = true;
}
else {
printf("command error,please insert again\n");
}
return status;
}
// =================================
// Main
// =================================
int main(int argc, char* argv[]) {
info("Attempting to connect to Arduino");
Serial port;
port.connect("COM4");
if (port._connected) {
info("Connected to Arduino");
}
char data[BUFFER_SIZE]; // Pre-allocate a buffer
flushBuffer(data, BUFFER_SIZE);
int dataLength = BUFFER_SIZE; // How many bytes to read
int readResult = 0; // How many bytes we read from the serial port
while (port._connected) {
readResult = port.read(data, dataLength);
if (readResult > -1) {
printf("RECV: \'%s\' (%i)\n", data, readResult);
flushBuffer(data, BUFFER_SIZE);
}
}
return 0;
}
/*Method is triggered when there is byte on serial port */
void serialEvent(Serial thisPort) {
if (thisPort == myPort)
{
// variable to hold the data on bus
int inByte = thisPort.read();
if (dataIn[0][0] == 100)//if receive header has been received
{
/*Put inByte into array
RGBcount - keeps track of which value (r,g, or b) is to have the value
- loops after 3 (0,1,2)
Once 8 pixels have been received (24 rgb vals), fullR=true. Get ready to receive back
*/
dataIn[count][rgbCount] = inByte;
System.out.println(inByte + "in Count: " + count + " RGB: " + rgbCount);
rgbCount = (rgbCount+1)%3;
if (rgbCount == 0)
count++;
if (count >= dataIn.length)
{
fullR = true;
System.out.println("ONE ROW");
dataIn[0][0] = 0;
}
}
else if (inByte == 100)//header character to indicate transmission of a pixel
{
System.out.println("Header received");
dataIn[0][0] = inByte;
count=1;
}
else if (inByte == 87 && dataIn[0][0] == 0)//ack received from arduino && waiting for ack;
{
//we will only receive ack to start transmission if we are done receiving
//when finished receiving, dataIn is set to 0;
//set ack = true to start transmission
//technically, checking dataIn is irrelavent b/c it is checked before inbyte is checked for 87,
// but we are preventing future challenges if we rearrange the order of this cascading if statement
ack = true;
System.out.println("Ack 87 received");
}
else if (inByte == 89 && dataIn[0][0] == 0)//ack received from arduino && waiting for ack;
{
//we will only receive ack to start transmission if we are done receiving
//when finished receiving, dataIn is set to 0;
//set ack = true to start transmission
//technically, checking dataIn is irrelavent b/c it is checked before inbyte is checked for 87,
// but we are preventing future challenges if we rearrange the order of this cascading if statement
myPort.write(90);
ack = true;
System.out.println("Ack 90 received");
}
}
}//end serialEvent
int main(int argc, char* argv[])
{
printf("[i] Start... \n");
Serial serial;
#if defined(WIN32)
serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE, true);
#elif defined(LINUX)
serial.open(SERIAL_PORT_NAME, SERIAL_PORT_RATE);
#endif
if(!serial.connected())
{
printf("[!] Cant open port!\n");
return -1;
}
char c = 'y';
int res = 12;
char buf[BUF_SIZE1];
memset(buf, 0, BUF_SIZE1);
while(true)
{
serial.write( &c, 1 );
#if 0
if(res = serial.available()){
if( res = serial.Read(buf, res) ){
printf("%d %s\n", res, buf);
}
}
#else
if(serial.waitInput(1000)==0)
printf("[i] timeout!\n");
else
{
if(res = serial.available())
{
res = serial.read(buf, res);
printf("%d %s\n", res, buf);
}
}
#endif
}
serial.close();
printf("[i] End.\n");
return 0;
}
void check_data()
{
/* while(myPort.available()>0)
{
int a=myPort.read();
print(a+" ");
if((a==8)||(a==10))
println();
}*/
while(myPort.available()>0)
{
char a=(char)myPort.read();
print(a);
}
}
TEST(Serial, WriteCOM) {
Serial serial;
serial.open("COM7", 9600);
if(!serial.connected()){
FAIL()<<"cant open serial port!";
}
char buf[1024];
int res = 0;
while(1){
char wbuf[] = {0xff, 0xff, 0x00, 0x00, 0x00, 0x00 ,0xff};
serial.write(wbuf, sizeof(wbuf));
if( serial.waitInput(1000) > 0){
if( (res = serial.available()) > 0 ){
if( res = serial.read(buf, res) ){
printf("[i] read data(%d): \n", res);
for(int i=0; i<res; i++){
printf("%02X ", (unsigned char)buf[i]);
if(i>0 && (i+1)%16 == 0) {
printf("\t");
for(int j=i-15; j<=i; j++){
printf("%c", buf[j]);
}
printf("\n");
}
}
printf("\n");
res = 0;
}
}
}
Sleep(1000);
}
}
int main()
{
/* declarations */
bool special_mode;
indications command, last_command;
indication_mode pattern;
unsigned char buffer[32];
Serial serial;
PID accel;
/* initialize hardware */
leds_init();
buttons_init();
timer1_init();
/* setup control loop for brake light */
pid_zeroize(&accel);
accel.proportional_gain = 1.0f;
accel.integral_gain = 0.0f;
accel.derivative_gain = 0.0f;
/* initialize and read the lsm303 */
lsm303_begin();
lsm303_read();
/* setup usb serial port */
serial.begin();
/* initialize pattern settings */
last_command = ind_off;
command = ind_off;
pattern = off;
/* set light/indication mode to PWM driven */
brake_lights(pwm);
//brake_lights(off);
/* reset leds and animation driver */
turn_signal(command, pattern);
leds_reset();
/* check to see if switch is depressed on startup */
command = get_signal_switch_status();
if(command != ind_off)
{
special_mode = true;
}
else
{
special_mode = false;
}
while(true)
{
/* control brake light using mag/accel data */
brake_light_control(&accel);
/* check to see if the user input has changed state */
command = get_signal_switch_status();
/* if "special mode" is enabled, play a fancy animation on idle */
if(special_mode && (command == ind_off))
{
/* animation selection */
command = ind_hazard;
pattern = loop;
}
else
{
/* default pattern, no command override */
pattern = scroll;
}
/* if the user input has changed, update the pattern driver state */
if (command != last_command)
{
leds_reset();
turn_signal(command, pattern);
}
if(serial.available()) {
int size = serial.read(buffer);
if (size!=0) {
serial.write((const uint8_t*)buffer, size);
serial.write('\n');
}
}
serial.poll();
/* debounce user input */
last_command = command;
_delay_ms(3);
}
return 0;
}
void process_serial(Serial &serial, TBuff<uint8_t> &buff, orcp2::packet &pkt)
{
int res = 0;
if(!buff.data || !pkt.message.data) {
ROS_ERROR("[!] Error: no memory!\n");
return ;
}
ros::Time current_time;
ros_4wd_driver::drive_telemetry_4wd drive;
ros_4wd_driver::sensors_telemetry_4wd sensors;
ros_4wd_driver::imu_raw_data imu;
if( res = serial.waitInput(500) ) {
//printf("[i] waitInput: %d\n", res);
if( (res = serial.available()) > 0 ) {
//printf("[i] available: %d\n", res);
//res = res > (buff.real_size-buff.size) ? (buff.real_size-buff.size) : res ;
if( (res = serial.read(buff.data+buff.size, buff.real_size-buff.size)) > 0 ) {
buff.size += res;
// get packet from data
while ( (res = orcp2::get_packet(buff.data, buff.size, &pkt)) > 0) {
// ROS_INFO("[i] res: %02d message: %04X size: %04d CRC: %02X buff.size: %02d\n",
// res, pkt.message_type, pkt.message_size, pkt.checksum, buff.size);
uint8_t crc = pkt.calc_checksum();
if(crc == pkt.checksum) {
// ROS_INFO("[i] Good message CRC: %02X\n", pkt.checksum);
current_time = ros::Time::now();
switch(pkt.message_type) {
case ORCP2_SEND_STRING:
pkt.message.data[pkt.message.size]=0;
ROS_INFO("[i] String: %s\n", pkt.message.data);
//printf("[i] counter: %d\n", counter);
#if 0
if(++counter > 50) {
//printf("[i] digitalWrite: %d\n", val);
val = !val;
orcp.digitalWrite(13, val);
counter = 0;
}
#endif
break;
case ORCP2_MESSAGE_IMU_RAW_DATA:
deserialize_imu3_data(pkt.message.data, pkt.message.size, &raw_imu_data);
ROS_INFO( "[i] IMU: acc: [%d %d %d] mag: [%d %d %d] gyro: [%d %d %d]\n",
raw_imu_data.Accelerometer[0], raw_imu_data.Accelerometer[1], raw_imu_data.Accelerometer[2],
raw_imu_data.Magnetometer[0], raw_imu_data.Magnetometer[1], raw_imu_data.Magnetometer[2],
raw_imu_data.Gyro[0], raw_imu_data.Gyro[1], raw_imu_data.Gyro[2] );
imu.header.stamp = current_time;
imu.accelerometer1 = raw_imu_data.Accelerometer[0];
imu.accelerometer2 = raw_imu_data.Accelerometer[1];
imu.accelerometer3 = raw_imu_data.Accelerometer[2];
imu.magnetometer1 = raw_imu_data.Magnetometer[0];
imu.magnetometer2 = raw_imu_data.Magnetometer[1];
imu.magnetometer3 = raw_imu_data.Magnetometer[2];
imu.gyro1 = raw_imu_data.Gyro[0];
imu.gyro2 = raw_imu_data.Gyro[1];
imu.gyro3 = raw_imu_data.Gyro[2];
imu_raw_data_pub.publish(imu);
break;
case ORCP2_MESSAGE_ROBOT_4WD_DRIVE_TELEMETRY:
deserialize_robot_4wd_drive_part(pkt.message.data, pkt.message.size, &robot_data);
ROS_INFO( "[i] Drive telemetry: bmp: %d enc: [%d %d %d %d] PWM: [%d %d %d %d]\n",
robot_data.Bamper,
robot_data.Encoder[0], robot_data.Encoder[1], robot_data.Encoder[2], robot_data.Encoder[3],
robot_data.PWM[0], robot_data.PWM[1], robot_data.PWM[2], robot_data.PWM[3] );
drive.header.stamp = current_time;
drive.bamper = robot_data.Bamper;
drive.encoder1 = robot_data.Encoder[0];
drive.encoder2 = robot_data.Encoder[1];
drive.encoder3 = robot_data.Encoder[2];
drive.encoder4 = robot_data.Encoder[3];
drive.pwm1 = robot_data.PWM[0];
drive.pwm2 = robot_data.PWM[1];
drive.pwm3 = robot_data.PWM[2];
drive.pwm4 = robot_data.PWM[3];
drive_telemetry_pub.publish(drive);
calc_odometry(drive);
break;
case ORCP2_MESSAGE_ROBOT_4WD_SENSORS_TELEMETRY:
deserialize_robot_4wd_sensors_part(pkt.message.data, pkt.message.size, &robot_data);
ROS_INFO( "[i] Sensors telemetry: US: %d IR: [%d %d %d %d] V: %d\n",
robot_data.US[0],
robot_data.IR[0], robot_data.IR[1], robot_data.IR[2], robot_data.IR[3],
robot_data.Voltage );
sensors.header.stamp = current_time;
sensors.us = robot_data.US[0];
sensors.ir1 = robot_data.IR[0];
sensors.ir2 = robot_data.IR[1];
sensors.ir3 = robot_data.IR[2];
sensors.ir4 = robot_data.IR[3];
sensors.voltage = robot_data.Voltage;
sensors_telemetry_pub.publish(sensors);
break;
case ORCP2_MESSAGE_ROBOT_4WD_TELEMETRY:
deserialize_robot_4wd(pkt.message.data, pkt.message.size, &robot_data);
ROS_INFO( "[i] Drive Telemetry: bmp: %d enc: [%d %d %d %d] PWM: [%d %d %d %d]\n",
robot_data.Bamper,
robot_data.Encoder[0], robot_data.Encoder[1], robot_data.Encoder[2], robot_data.Encoder[3],
robot_data.PWM[0], robot_data.PWM[1], robot_data.PWM[2], robot_data.PWM[3] );
drive.header.stamp = current_time;
drive.bamper = robot_data.Bamper;
drive.encoder1 = robot_data.Encoder[0];
drive.encoder2 = robot_data.Encoder[1];
drive.encoder3 = robot_data.Encoder[2];
drive.encoder4 = robot_data.Encoder[3];
drive.pwm1 = robot_data.PWM[0];
drive.pwm2 = robot_data.PWM[1];
drive.pwm3 = robot_data.PWM[2];
drive.pwm4 = robot_data.PWM[3];
drive_telemetry_pub.publish(drive);
calc_odometry(drive);
ROS_INFO( "[i] Sensors Telemetry: US: %d IR: [%d %d %d %d] V: %d\n",
robot_data.US[0],
robot_data.IR[0], robot_data.IR[1], robot_data.IR[2], robot_data.IR[3],
robot_data.Voltage );
sensors.header.stamp = current_time;
sensors.us = robot_data.US[0];
sensors.ir1 = robot_data.IR[0];
sensors.ir2 = robot_data.IR[1];
sensors.ir3 = robot_data.IR[2];
sensors.ir4 = robot_data.IR[3];
sensors.voltage = robot_data.Voltage;
sensors_telemetry_pub.publish(sensors);
break;
default:
ROS_INFO("[i] Unknown message type: %04X!\n", pkt.message_type);
break;
}
}
else {
ROS_WARN("[!] Bad message CRC: %02X vs: %02X\n", crc, pkt.checksum);
}
}
}
else {
ROS_WARN("[!] too much data: %d (%d)\n", res, buff.size);
buff.size = 0;
}
}
else {
ROS_INFO("[!] no available: %d\n", res);
}
}
else {
ROS_INFO("[!] waitInput timeout: %d\n", res);
}
}
int main(int argc, char **argv) {
std::string port("/dev/ttyUSB0");
if ( argc > 1 ) {
std::string arg(argv[1]);
if ( arg == "--help" ) {
print_usage();
return 0;
} else {
port = argv[1];
}
}
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " Serial Timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* For demo'ing low latency read timeouts on posix systems." << std::endl;
std::cout << "* Timeouts < 100ms use a custom loop, > 100ms use termios." << std::endl;
std::cout << "* Hook this up to a serial cable to test (actual" << std::endl;
std::cout << "* connection not important)." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
Serial serial;
try {
serial.open(port,BaudRate_115200,DataBits_8,StopBits_1,NoParity);
} catch (StandardException &e ) {
std::cout << "[ERROR] : error opening " << port << std::endl;
std::cout << std::endl;
print_usage();
return 0;
}
TimeStamp time, pre_read_time;
char buffer[256];
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " 100 ms timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* This will use termios to scan." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
serial.block(100);
for ( unsigned int i = 0; i < 10; ++i ) {
pre_read_time.stamp();
long result = serial.read(buffer,256);
time.stamp();
if ( result > 0 ) {
std::cout << "[INFO] : read " << result << " bytes." << std::endl;
} else if ( result == 0 ) {
std::cout << "[INFO] : timed out [" << (time - pre_read_time) << "]." << std::endl;
} else {
std::cout << "[INFO] : error " << result << "." << std::endl;
}
}
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " 50 ms timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* This will internally scan with 5ms loops." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
serial.block(50);
// uncomment this to test reading without a timeout on a loopback connection.
// buffer[0] = 'a';
// buffer[1] = 'b';
// serial.write(buffer,2);
for ( unsigned int i = 0; i < 10; ++i ) {
pre_read_time.stamp();
long result = serial.read(buffer,256);
time.stamp();
if ( result > 0 ) {
std::cout << "[INFO] : read " << result << " bytes." << std::endl;
} else if ( result == 0 ) {
std::cout << "[INFO] : timed out [" << (time - pre_read_time) << "]." << std::endl;
} else {
std::cout << "[INFO] : error " << result << "." << std::endl;
}
}
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " 20 ms timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* This will internally scan with 2ms loops." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
serial.block(20);
for ( unsigned int i = 0; i < 10; ++i ) {
pre_read_time.stamp();
long result = serial.read(buffer,256);
time.stamp();
if ( result > 0 ) {
std::cout << "[INFO] : read " << result << " bytes." << std::endl;
} else if ( result == 0 ) {
std::cout << "[INFO] : timed out [" << (time - pre_read_time) << "]." << std::endl;
} else {
std::cout << "[INFO] : error " << result << "." << std::endl;
}
}
std::cout << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << " 5 ms timeouts" << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << "* This will internally scan with 1ms loops." << std::endl;
std::cout << "***********************************************************" << std::endl;
std::cout << std::endl;
serial.block(5);
for ( unsigned int i = 0; i < 10; ++i ) {
pre_read_time.stamp();
long result = serial.read(buffer,256);
time.stamp();
if ( result > 0 ) {
std::cout << "[INFO] : read " << result << " bytes." << std::endl;
} else if ( result == 0 ) {
std::cout << "[INFO] : timed out [" << (time - pre_read_time) << "]." << std::endl;
} else {
std::cout << "[INFO] : error " << result << "." << std::endl;
}
}
return 0;
}
int getValue() {
int value = -1;
while (port.available() >= 3) {
if (port.read() == 0xff) {
value = (port.read() << 8) | (port.read());
}
}
return value;
}
