 // For linear actuator
 int incomingByte = 0;   // for incoming serial data
 
-void setup() 
+void setup()
 {
+   Serial.begin(57600); // Open serial connection to report values to host
+   Serial.println("Starting up");
 
-  // Initialize ROS node
-  nh.initNode();
-  nh.advertise(pub_temp);
-  nh.advertise(pub_humid);
+   // Initialize ROS node
+   nh.initNode();
+   nh.advertise(pub_temp);
+   nh.advertise(pub_humid);
 
-  // Setup code for linear actuator
-  // put your setup code here, to run once:
-  #define AIN1 4
-  //#define BIN1 7 
-  #define AIN2 5
-  //#define BIN2 8
-  #define PWMA 6
-  //myservo.attach(6);  // attaches the servo on pin 9 to the servo object
-  //#define PWMB 9 
-  #define STBY 9
-  pinMode(AIN1, OUTPUT);
-  pinMode(AIN2, OUTPUT);
-  
-  // Initialize Serial communications
-  Serial.begin(57600);
-  
-  // Prompt user with instructions
-  Serial.println("Initializing SHT10 Temp/Humidity Sensor...");
-  Serial.println("Type any of the following commands, followed by the ENTER key:");
-  Serial.println("\t'C/c': Display temperature in degrees Celsius");
-  Serial.println("\t'F/f': Display temperature in degrees Fahrenheit");
-  Serial.println("\t'H/h': Display humidity (0-100%)");
-  Serial.println("\t'': Display all available sensor information");
-
-  
-  
+   // Setup code for linear actuator
+   // put your setup code here, to run once:
+   #define AIN1 4
+   //#define BIN1 7 
+   #define AIN2 5
+   //#define BIN2 8
+   #define PWMA 6
+   //myservo.attach(6);  // attaches the servo on pin 9 to the servo object
+   //#define PWMB 9 
+   #define STBY 9
+   pinMode(AIN1, OUTPUT);
+   pinMode(AIN2, OUTPUT);
+   
 }
 
-void loop() 
+void loop()
 {
+  float temp_c;
+  float temp_f;
+  float humidity;
 
-  // Print the temperature info in degrees celsius
-  tempC = mySensor.readTemperatureC();
-  Serial.print("Temperature: "); Serial.print(tempC);
-  Serial.println(" C");
+  // Read values from the sensor
+  temp_c = sht1x.readTemperatureC();
+  //temp_f = sht1x.readTemperatureF();
+  humidity = sht1x.readHumidity();
 
-  humidity = mySensor.readHumidity();
-  Serial.print("Humidity: "); Serial.print(humidity);
+  // Print the values to the serial port
+  Serial.print("Temperature: ");
+  Serial.print(temp_c, DEC);
+  Serial.print("C / ");
+  //Serial.print(temp_f, DEC);
+  //Serial.print("F. Humidity: ");
+  Serial.print("Humidity: ");
+  Serial.print(humidity);
   Serial.println("%");
 
   // ROS stuff
-  temp_msg.data = tempC;
+  temp_msg.data = temp_c;
   humid_msg.data = humidity;
   pub_temp.publish(&temp_msg);
   pub_humid.publish(&humid_msg);
 
-  //nh.spinOnce(); // Placing this here will not make it work
-  
-  if(msgReceived) 
-  {
-    
-    // Display all sensor data
-    if(inString.length() == 0) 
-    {
-      tempC = mySensor.readTemperatureC();
-      tempF = mySensor.readTemperatureF();
-      humidity = mySensor.readHumidity();
+  //nh.spinOnce(); Placing this line here prevents it from working
 
-      Serial.print("Humidity: "); Serial.print(humidity);
-      Serial.println("%");
-      Serial.print("Temperature: "); Serial.print(tempC);
-      Serial.println(" C");
-      Serial.print("Temperature: "); Serial.print(tempF);
-      Serial.println(" F");
-    }
-    // Check for desired reading (degrees C, degrees F, or humidity)
-    if(inString.charAt(0) == 'C' || inString.charAt(0) == 'c') 
-    {
-      tempC = mySensor.readTemperatureC();
-      Serial.print("Temperature: "); Serial.print(tempC);
-      Serial.println(" C");
-    } 
-    else if(inString.charAt(0) == 'F' || inString.charAt(0) == 'f') 
-    {
-      tempF = mySensor.readTemperatureF();
-      Serial.print("Temperature: "); Serial.print(tempF);
-      Serial.println(" F");
-    }
-    else if(inString.charAt(0) == 'H' || inString.charAt(0) == 'h') 
-    {
-      humidity = mySensor.readHumidity();
-      Serial.print("Humidity: "); Serial.print(humidity);
-      Serial.println("%");
-    }
-    
-    // Reset Serial communication variables
-    msgReceived = false;
-    inString = "";
-  }
+  //delay(2000);
 
   // send data only when you receive data:
   if (Serial.available() > 0) 

  //myservo.write(179);   
  // put your main code here, to run repeatedly:

  nh.spinOnce(); // Place this at the end
}

/*
  SerialEvent occurs whenever new data comes in the
  hardware serial RX.  This routine is run between each
  time loop() runs, so using delay inside loop can delay
  response.  Multiple bytes of data may be available.
 */
void serialEvent() 
{
  // Read from Rx until end of data stream, or until '\n' is encountered
  while (Serial.available() && !msgReceived) 
  {
    inChar = (char)Serial.read();          // Get the next byte
    
    // Check for a newline, indicating the end of the message
    if(inChar == '\n') 
    {
      msgReceived = true;    // Set flag to break out of loop and handle message
    } 
    else 
    {
      inString += inChar;    // Append current character to the input string
    }
  }
}

/**
 * ReadSHT1xValues
 *
 * Read temperature and humidity values from an SHT1x-series (SHT10,
 * SHT11, SHT15) sensor.
 *
 * Copyright 2009 Jonathan Oxer <jon@oxer.com.au>
 * www.practicalarduino.com
 */
// ROS Setup Stuff
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>

#include <SHT1x.h>

// Specify data and clock connections and instantiate SHT1x object
#define dataPin  10
#define clockPin 11
SHT1x sht1x(dataPin, clockPin);

ros::NodeHandle nh;

