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 <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
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::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);
print_with_padding(current_score);
//lcd.print(add_padding(current_score));
//lcd.print(char_buffer);
/*lcd.setCursor(0,0);
lcd.print(current_pose.position.x);
lcd.setCursor(6,0);
lcd.print(current_pose.position.y);
lcd.setCursor(12,0);
//lcd.print(quaternionToEulerZ(current_pose.orientation));*/
lcd.setCursor(0,1);
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");
}
nh.spinOnce();
if(loopId == 0)
{
loopId = 1024;
drawLCD();
}
loopId--;