Newer
Older
/****************************************************************************
* Author : Victor Dubois
****************************************************************************/
#include <ros.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Duration.h>
#include <math.h>
LiquidCrystal_I2C lcd(0x27,16,2); // set the LCD address to 0x27 for a 16 chars and 2 line display
bool stopped = true;
uint16_t current_score;
uint16_t remaining_time;
int loopId = 0;
void timeCb( const std_msgs::Duration& a_remaining_time){
remaining_time = a_remaining_time.data.toSec();
}
void scoreCb( const std_msgs::UInt16& score){
current_score = score.data;
ros::Subscriber<std_msgs::UInt16> score_sub("score", &scoreCb);
ros::Subscriber<std_msgs::Duration> time_sub("remainingTime", &timeCb);
void drawLCD()
{
// Print a message to the LCD.
lcd.setCursor(7,1);
print_with_padding(current_score);
lcd.setCursor(0,1);
print_with_padding(remaining_time);
lcd.print("s");
}
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 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;
nh.initNode();
nh.subscribe(score_sub);
nh.subscribe(time_sub);
lcd.init(); // initialize the lcd
lcd.backlight();
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));
nh.spinOnce();
if(loopId == 0)
{
loopId = 1024;
drawLCD();
}
loopId--;