cleanup + custom chars

lcd_i2c.ino

@@ -7,8 +7,6 @@
 #include <LiquidCrystal_I2C.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
@@ -17,7 +15,6 @@ 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){
@@ -26,50 +23,21 @@ void timeCb( const std_msgs::Duration& a_remaining_time){
 
 void scoreCb( const std_msgs::UInt16& score){
     current_score = score.data;
-    //drawLCD();
 }
 
-/*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);
-
-/*int quaternionToEulerZ(geometry_msgs::Quaternion& q)
-{
-    // 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:");
     print_with_padding(current_score);
-    //lcd.print(add_padding(current_score));
-    //lcd.print(char_buffer);
-    /*lcd.setCursor(0,0);
-    lcd.print("X");
-    lcd.print(current_pose.position.x);
-    lcd.setCursor(6,0);
-    lcd.print("Y");
-    lcd.print(current_pose.position.y);
-    lcd.setCursor(12,0);
-    lcd.print("T");
-    //lcd.print(quaternionToEulerZ(current_pose.orientation));*/
     lcd.setCursor(0,1);
     lcd.print("T");
     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)
@@ -96,22 +64,57 @@ void print_with_padding(uint16_t number)
     lcd.print(units);
 }
 
+void createCrab()
+{
+    byte Crab1[8] = {
+        0b01000,
+        0b11001,
+        0b00110,
+        0b01110,
+        0b01110,
+        0b00110,
+        0b11001,
+        0b01000
+    };
+    byte Crab2[8] = {
+        0b01000,
+        0b11001,
+        0b00110,
+        0b01111,
+        0b01111,
+        0b00110,
+        0b11001,
+        0b01000
+    };
+    lcd.createChar(0, Crab1);
+    lcd.createChar(1, Crab2);
+}
+
 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");
+    createCrab();
+    lcd.setCursor(0,0);
+lcd.write(byte(0));
+lcd.write(byte(0));
+lcd.write(byte(0));
+    
+    lcd.setCursor(4,0);
+    lcd.print("Kraboss");
+    lcd.setCursor(13,0);
+lcd.write(byte(0));
+lcd.write(byte(1));
+lcd.write(byte(0));
+    
 }
 
 void loop()