diff --git a/lcd_i2c.ino b/lcd_i2c.ino index 3529387b18854e374dfa142020d9aebfdb5544ef..3e5711f862b561fc881592ab528ae2d7732b7dcc 100644 --- a/lcd_i2c.ino +++ b/lcd_i2c.ino @@ -7,8 +7,6 @@ #include #include #include -//#include -//#include #include 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 score_sub("score", &scoreCb); ros::Subscriber time_sub("remainingTime", &timeCb); -//ros::Subscriber 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(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()