lcd_i2c.ino 2.53 KB
Newer Older
Victor Dubois's avatar
Victor Dubois committed
1 2 3 4
/****************************************************************************
* Author : Victor Dubois
****************************************************************************/

Victor Dubois's avatar
Victor Dubois committed
5 6 7
#include <ros.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
Victor Dubois's avatar
Victor Dubois committed
8 9 10
#include <std_msgs/UInt16.h>
#include <std_msgs/Duration.h>
#include <math.h>
Victor Dubois's avatar
Victor Dubois committed
11 12

LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
Victor Dubois's avatar
Victor Dubois committed
13

Victor Dubois's avatar
Victor Dubois committed
14
ros::NodeHandle nh;
Victor Dubois's avatar
Victor Dubois committed
15 16 17 18
bool stopped = true;
uint16_t current_score;
uint16_t remaining_time;
int loopId = 0;
Victor Dubois's avatar
Victor Dubois committed
19

Victor Dubois's avatar
Victor Dubois committed
20 21 22
void timeCb( const std_msgs::Duration& a_remaining_time){
    remaining_time = a_remaining_time.data.toSec();
}
Victor Dubois's avatar
Victor Dubois committed
23

Victor Dubois's avatar
Victor Dubois committed
24 25
void scoreCb( const std_msgs::UInt16& score){
    current_score = score.data;
Victor Dubois's avatar
Victor Dubois committed
26 27
}

Victor Dubois's avatar
Victor Dubois committed
28 29 30 31 32 33 34
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);
Victor Dubois's avatar
Victor Dubois committed
35
    lcd.print("Score:");
Victor Dubois's avatar
Victor Dubois committed
36 37
    print_with_padding(current_score);
    lcd.setCursor(0,1);
Victor Dubois's avatar
Victor Dubois committed
38
    lcd.print("T");
Victor Dubois's avatar
Victor Dubois committed
39 40 41
    print_with_padding(remaining_time);
    lcd.print("s");
}
Victor Dubois's avatar
Victor Dubois committed
42

Victor Dubois's avatar
Victor Dubois committed
43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
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);
Victor Dubois's avatar
Victor Dubois committed
65 66
}

Victor Dubois's avatar
Victor Dubois committed
67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92
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);
}

Victor Dubois's avatar
Victor Dubois committed
93 94 95 96 97 98 99 100 101 102 103 104
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();
Victor Dubois's avatar
Victor Dubois committed
105 106 107 108 109 110 111 112 113 114 115 116 117
    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));
    
Victor Dubois's avatar
Victor Dubois committed
118
}
Victor Dubois's avatar
Victor Dubois committed
119 120 121

void loop()
{
Victor Dubois's avatar
Victor Dubois committed
122 123 124 125 126 127 128
    nh.spinOnce();
    if(loopId == 0)
    {
        loopId = 1024;
        drawLCD();
    }
    loopId--;
Victor Dubois's avatar
Victor Dubois committed
129
}
Victor Dubois's avatar
Victor Dubois committed
130