void send_query(byte query)
{
myPort.write(MSG_START);
myPort.write(query);
myPort.write(query); //the checksum on queries is just the query again
myPort.write(MSG_END);
}
void send_command(byte command, byte highByte, byte lowByte)
{
if(command!=STOP)
{
byte checksum=(byte)(((int)command+(int)highByte+(int)lowByte)%255);
myPort.write(MSG_START);
delay(del);
myPort.write(command);
delay(del);
myPort.write(highByte);
delay(del);
myPort.write(lowByte);
delay(del);
myPort.write(checksum);
delay(del);
myPort.write(MSG_END);
}
else
{
myPort.write(MSG_START);
delay(del);
myPort.write(command);
delay(del);
myPort.write(command); //checksum
delay(del);
myPort.write(MSG_END);
}
}
void pen_up()
{
myPort.write(MSG_START);
myPort.write(PEN_UP);
myPort.write(PEN_UP); //the checksum on queries is just the query again
myPort.write(MSG_END);
}
void pen_down()
{
myPort.write(MSG_START);
delay(del);
myPort.write(PEN_DOWN);
delay(del);
myPort.write(PEN_DOWN); //the checksum on queries is just the query again
delay(del);
myPort.write(MSG_END);
}
void setup()
{
size(1300, 1000);
img = (loadImage("pic4.jpg"));
/*//Use this to create your own pic of a specific colour to test individual RGB vals
img = createImage(cols,rows,RGB); //
for(int i = 0; i < img.pixels.length; i++) {
img.pixels[i] = color(0, 0, 255);
}
*/
loadPixels();
System.out.println(img.pixels.length);
String portName = "COM4";
myPort = new Serial(this, portName, 9600);
background(0);
dataIn[0][0] = 0;
//send ack every 2 seconds, wait for ack back (see serial event
//this begins negotiation between machines
//reception of the ack is handled by serial interrupt method below
while (!ack) {
myPort.write(88);
delay(2000);
}
ack = true;
}//end setup
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;
}
void send_empty_packet()
{
memset(packet, 0, PACKET_MAX_SIZE);
packet[0] = PACKET_SYNC;
packet[1] = PACKET_VER;
serial_port.write(packet, 16);
}
/*The main loop*/
void draw()
{
//write X pixel data to serial
//for (int y=0;y<48;y++) { ***Don't actually need to increment y right now b/c it is incremented
// the correct amount below in the fullR loop -- which is convenient!
if (ack) {
myPort.write(101);
for (int x=0;x<cols;x++) {
//get pixel, extract rgb vals from pixel using bit shifting (faster)
color rgb = img.pixels[y*cols + x];
int r = rgb >> 16 & 0xFF; //Equivalent to, but faster than red(rgb)
int g = rgb >> 8 & 0xFF;
int b = rgb & 0xFF;
myPort.write(r);
myPort.write(g);
myPort.write(b);
println("Sent " + x);
//delay(50);
}
// for (int x=0;x<10;x++)
// myPort.write(255);
//set this to wait for another request
ack = false;
}
//if a full packet has been received, print to screen
if (fullR)
{
for (int i=1; i<dataIn.length; i++)
{
System.out.println("R " + dataIn[i][0] + " G " + dataIn[i][1] + " B" + dataIn[i][2]);
}
fullR = false;
for (int x=1; x<dataIn.length; x++)
{
fill(dataIn[x][0], dataIn[x][1], dataIn[x][2]);
//stroke(255);
rect((x-1)*5, y*5, 5, 5);
}
y+=1;
System.out.println(y);
}
}//end loop
/*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;
}
int main(int argc, char **argv){
cout<<"Connecting to the k3 robot..."<<endl;
try {
k3_ser.open();
}
catch(std::exception e) {
std::stringstream output;
output<<"Failed to open serial port "<< k3_ser.getPort() << "error: " << e.what() <<endl;
}
if(k3_ser.isOpen()){
cout<<"k3_ser is Connected on Port: "<<k3_ser.getPort()<<" at Baudrate: "<<k3_ser.getBaudrate()<<endl;
}
else{
ROS_ERROR("serial port not openened, verify the k3 robot selector position");
}
//test serial command
k3_ser.write("D,0,0\n");
ros::init(argc, argv, "k3");
cout<<"hello ros";
ros::NodeHandle n;
cmdVelSubscriber = n.subscribe("/robot_motors", 10, handlerVelocity);
ros::Rate loop_rate(10);
while(ros::ok()){
k3Motors();
//k3Cmd();
ros::spinOnce(); //Allow ROS to check for new ROS Messages
loop_rate.sleep(); //Sleep for some amount of time determined by loop_rate
}
return 0;
}
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);
}
}
SCSAPI_VOID telemetry_frame_end(const scs_event_t /*event*/, const void *const /*event_info*/, const scs_context_t /*context*/)
{
if (!serial_port.is_valid())
return;
const unsigned long now = GetTickCount();
const unsigned long diff = now - last_update;
if (diff < 50)
return;
last_update = now;
const float speed_mph = telemetry.speed * METERS_PER_SEC_TO_MILES_PER_HOUR;
const float speed_kph = telemetry.speed * METERS_PER_SEC_TO_KM_PER_HOUR;
const float fuel_ratio = telemetry.fuel / telemetry.fuel_capacity;
unsigned idx = 0;
memset(packet, 0, PACKET_MAX_SIZE);
#define PUT_BYTE(X) packet[idx++] = (unsigned char)(X)
#define PUT_BYTE_FLOAT(X) packet[idx++] = (unsigned char)(float_to_byte(X))
#define GETFLTOPT(X) (option_file.get_option_float(X, 1.0f))
// Packet header
PUT_BYTE(PACKET_SYNC);
PUT_BYTE(PACKET_VER);
// Convert data for output by servos
// Adjust the constant values to map to servo range
PUT_BYTE_FLOAT(fabs(telemetry.speed) * GETFLTOPT("factor_speed")); // Absolute value for when reversing
PUT_BYTE_FLOAT(telemetry.engine_rpm * GETFLTOPT("factor_engine_rpm"));
PUT_BYTE_FLOAT(telemetry.brake_air_pressure * GETFLTOPT("factor_brake_air_pressure"));
PUT_BYTE_FLOAT(telemetry.brake_temperature * GETFLTOPT("factor_brake_temperature"));
PUT_BYTE_FLOAT(fuel_ratio * GETFLTOPT("factor_fuel_ratio")); // Fuel level
PUT_BYTE_FLOAT(telemetry.oil_pressure * GETFLTOPT("factor_oil_pressure"));
PUT_BYTE_FLOAT(telemetry.oil_temperature * GETFLTOPT("factor_oil_temperature"));
PUT_BYTE_FLOAT(telemetry.water_temperature * GETFLTOPT("factor_water_temperature"));
PUT_BYTE_FLOAT(telemetry.battery_voltage * GETFLTOPT("factor_battery_voltage"));
// Pack data for LEDs into bytes
// Truck lights
PUT_BYTE(PACKBOOL(
0, telemetry.light_parking,
telemetry.light_lblinker, telemetry.light_rblinker,
telemetry.light_low_beam, telemetry.light_high_beam,
telemetry.light_brake, telemetry.light_reverse));
// Warning lights
PUT_BYTE(PACKBOOL(
telemetry.parking_brake, telemetry.motor_brake,
telemetry.brake_air_pressure_warning, telemetry.brake_air_pressure_emergency,
telemetry.fuel_warning, telemetry.battery_voltage_warning,
telemetry.oil_pressure_warning, telemetry.water_temperature_warning));
// Enabled flags
PUT_BYTE(PACKBOOL(
0,0,0,0,0, 0,
telemetry.electric_enabled, telemetry.engine_enabled));
// Build string for LCD display
std::stringstream ss;
ss.width(3);
ss << abs(int(speed_mph));
ss << " MPH";
ss << " G ";
if (telemetry.engine_gear > 0)
{
ss << "D";
ss.width(2);
ss << telemetry.engine_gear;
}
else if (telemetry.engine_gear == 0)
{
ss << "N ";
}
else
{
ss << "R";
ss.width(2);
ss << abs(telemetry.engine_gear);
}
ss << "\n";
ss.width(3);
ss << abs(int(speed_kph));
ss << " KPH";
ss << " F ";
ss.width(3);
ss << int(fuel_ratio * 100.0f) << "%";
ss.width(3);
ss << abs(int(speed_kph));
std::string display_str(ss.str());
// Write length of string
PUT_BYTE(display_str.size());
// Followed by string
display_str.copy((char*)&packet[idx], PACKET_MAX_SIZE - idx);
idx += display_str.size();
serial_port.write(packet, idx);
}
//function to write serial commands for motors
void koalaMotors(){
char sercmd[12];
sprintf(sercmd,"D,%d,%d\n",speedLeft,speedRight);
koala_ser.write(sercmd);
}
void koalaCmd(){
koala_ser.write("D,50,50\n");
ros::Duration(3).sleep();
koala_ser.write("D,-50,-50\n");
ros::Duration(3).sleep();
}
CameraTracker::CameraTracker(Serial& serial) : serial(serial)
{
serial.write('0');//ctor
}
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 k3Cmd(){
k3_ser.write("D,l30000,l30000\n");
ros::Duration(2).sleep();
k3_ser.write("D,l-30000,l-30000\n");
ros::Duration(2).sleep();
}
//function to write serial commands for motors
void k3Motors(){
char sercmd[20];
sprintf(sercmd,"D,l%d,l%d\n",speedLeft,speedRight);
k3_ser.write(sercmd);
}
