+/**
+ * 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>
-#define dataPin 10
+
+// Specify data and clock connections and instantiate SHT1x object
+#define dataPin  10
 #define clockPin 11
-
-// Serial Communication Variables
-bool msgReceived = false;   // Flag to indicate receipt of message
-String inString;            // Store user input as string
-char inChar;
-
-// Sensor variables
-float tempC = 0;    // Temperature in Celsius (-40 - 123.8, -40 on error)
-float tempF = 0;    // Temperature in Fahrenheit (-40 - 254.9, -40 on error)
-float humidity = 0; // Humidity (0-100%, negative on error)
-
-// Initialize sensor object
-SHT1x mySensor(dataPin, clockPin);
-
+SHT1x sht1x(dataPin, clockPin);
 
 ros::NodeHandle nh;
 

// For linear actuator
int incomingByte = 0;   // for incoming serial data

void setup() 
{

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

  
  
}

void loop() 
{

  // Print the temperature info in degrees celsius
  tempC = mySensor.readTemperatureC();
  Serial.print("Temperature: "); Serial.print(tempC);
  Serial.println(" C");

  humidity = mySensor.readHumidity();
  Serial.print("Humidity: "); Serial.print(humidity);
  Serial.println("%");

  // ROS stuff
  temp_msg.data = tempC;
  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();

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

  // send data only when you receive data:
  if (Serial.available() > 0) 

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

  nh.spinOnce();
  
}
