working score + time display

lcd_i2c.ino

-//YWROBOT
-//Compatible with the Arduino IDE 1.0
-//Library version:1.1
+/****************************************************************************
+* Author : Victor Dubois
+****************************************************************************/
+
 #include <ros.h>
 #include <Wire.h> 
 #include <LiquidCrystal_I2C.h>
-#include <std_msgs/UInt32.h>
+#include <std_msgs/UInt16.h>
+#include <std_msgs/Duration.h>
+//#include <geometry_msgs/Pose.h>
+//#include <geometry_msgs/Quaternion.h>
+#include <math.h>
 
 LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
+
 ros::NodeHandle nh;
+bool stopped = true;
+uint16_t current_score;
+uint16_t remaining_time;
+//geometry_msgs::Pose current_pose;
+int loopId = 0;
 
+void timeCb( const std_msgs::Duration& a_remaining_time){
+    remaining_time = a_remaining_time.data.toSec();
+}
 
-void scoreCb( const std_msgs::UInt32& score){
-  lcd.setCursor(13,1);
-  uint32_t hundreds = (score.data%1000 - score.data%100)/100;
-  uint32_t units = score.data%10;
-  uint32_t tens = (score.data%1000 - hundreds*100 - units)/10;
-  lcd.print(hundreds);
-  lcd.print(tens);
-  lcd.print(units);
+void scoreCb( const std_msgs::UInt16& score){
+    current_score = score.data;
+    //drawLCD();
 }
 
-ros::Subscriber<std_msgs::UInt32> score_sub("score", &scoreCb);
+/*void poseCb( const geometry_msgs::Pose& a_pose){
+    current_pose = a_pose;
+}*/
 
+ros::Subscriber<std_msgs::UInt16> score_sub("score", &scoreCb);
+ros::Subscriber<std_msgs::Duration> time_sub("remainingTime", &timeCb);
+//ros::Subscriber<geometry_msgs::Pose> pose_sub("current_pose", poseCb);
 
-void setup()
+/*int quaternionToEulerZ(geometry_msgs::Quaternion& q)
 {
-  
-  lcd.init();                      // initialize the lcd 
-  lcd.init();
-  // Print a message to the LCD.
-  lcd.backlight();
-  lcd.setCursor(7,1);
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+    return static_cast<int>(atan2(siny_cosp, cosy_cosp));
+}*/
+
+void drawLCD()
+{
+    char* toto = "tu";
+
+    // Print a message to the LCD.
+    lcd.setCursor(7,1);
     lcd.print("Score:");
-  lcd.setCursor(0,0);
+    print_with_padding(current_score);
+    //lcd.print(add_padding(current_score));
+    //lcd.print(char_buffer);
+    /*lcd.setCursor(0,0);
     lcd.print("X");
-      lcd.setCursor(6,0);
+    lcd.print(current_pose.position.x);
+    lcd.setCursor(6,0);
     lcd.print("Y");
-      lcd.setCursor(12,0);
+    lcd.print(current_pose.position.y);
+    lcd.setCursor(12,0);
     lcd.print("T");
-      lcd.setCursor(0,1);
+    //lcd.print(quaternionToEulerZ(current_pose.orientation));*/
+    lcd.setCursor(0,1);
     lcd.print("T");
- 
-      nh.initNode();
-      nh.subscribe(score_sub);
+    print_with_padding(remaining_time);
+    lcd.print("s");
+    /*add_padding(remaining_time, &char_buffer);
+    lcd.print(char_buffer);*/
+}
 
+void print_with_padding(uint16_t number)
+{
+    uint16_t hundreds = (number%1000 - number%100)/100;
+    uint16_t units = number%10;
+    uint16_t tens = (number%1000 - hundreds*100 - units)/10;
+    if(hundreds == 0)
+    {
+        lcd.print(" ");
+    }
+    else
+    {
+        lcd.print(hundreds);
+    };
+    if(tens == 0 && hundreds == 0)
+    {
+        lcd.print(" ");
+    }
+    else
+    {
+        lcd.print(tens);
+    };
+    lcd.print(units);
 }
 
+void setup()
+{ 
+    current_score = 0;
+    loopId = 255;
+    remaining_time = 100;
+    //current_pose = geometry_msgs::Pose();
+    
+    nh.initNode();
+    nh.subscribe(score_sub);
+    nh.subscribe(time_sub);
+    //nh.subscribe(pose_sub);
+
+    lcd.init();                      // initialize the lcd 
+    lcd.backlight();
+    lcd.setCursor(0,1);
+    lcd.print("T");
+}
 
 void loop()
 {
-  nh.spinOnce();
-  delay(10);
+    nh.spinOnce();
+    if(loopId == 0)
+    {
+        loopId = 1024;
+        drawLCD();
+    }
+    loopId--;
 }
+