Revision 1019d31f

View differences:

scout/painter/CMakeLists.txt
21 21
#uncomment if you have defined services
22 22
#rosbuild_gensrv()
23 23

  
24
rosbuild_add_executable(painter src/painter.cpp)
24
rosbuild_add_executable(painter src/painter.cpp src/paint-i2c.cpp)
25 25

  
26 26
#common commands for building c++ executables and libraries
27 27
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
scout/painter/src/paint-i2c.cpp
1
/**
2
 * Copyright (c) 2013 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
/**
27
 * @file paint_i2c.cpp
28
 * @brief Contains i2c communication for the paintboard.
29
 *
30
 * @author Colony Project, CMU Robotics Club
31
 * @author Maung Aung (mza)
32
 * @author Tom Mullins (tmullins)
33
 *
34
 */
35

  
36
#include <unistd.h>
37
#include <sys/types.h>
38
#include <sys/stat.h>
39
#include <sys/ioctl.h>
40
#include <fcntl.h>
41
#include <linux/i2c-dev.h>
42
#include "paint-i2c.h"
43

  
44
#define TRACKING_ID 0x43
45
#define SERIAL_NUMBER 0x12
46

  
47
// indicies for paintboard internal data
48
#define PAINT_TRACKING_ID      0 // ro
49
#define PAINT_SERIAL_NUMBER    1 // ro
50
#define PAINT_MOTOR_A          2 // rw
51
#define PAINT_MOTOR_B          3 // rw
52
#define PAINT_SERVO_A          4 // rw
53
#define PAINT_SERVO_B          5 // rw
54
#define PAINT_12V_1            6 // rw
55
#define PAINT_12V_2            7 // rw
56
#define PAINT_12V_3            8 // rw
57
#define PAINT_12V_4            9 // rw
58
#define PAINT_INPUT_1         10 // ro
59
#define PAINT_INPUT_2         11 // ro
60
#define PAINT_INPUT_3         12 // ro
61

  
62
#define PAINT_DATA_LEN        13
63

  
64
int fd;
65

  
66
/*
67
 * This function opens communication to I2C.
68
 */
69
void i2c_start(void) {
70
  /* TODO error check */
71
  fd = open("/dev/i2c-3", O_RDWR);
72
  ioctl(fd, I2C_SLAVE, TRACKING_ID);
73
}
74

  
75
/*
76
 * This function ends communication with I2C.
77
 */
78
void i2c_stop(void) {
79
  if (fd) {
80
    close(fd);
81
  }
82
}
83

  
84
/*
85
 * This function writes a value to a given destination using I2C.
86
 */
87
void i2c_write(int dest, int val) {
88
  char buf[2];
89
  buf[0] = (char) dest;
90
  buf[1] = (char) val;
91

  
92
  write(fd, buf, 2);
93
}
94

  
95
/*
96
 * This function reads a value from the given address using I2C
97
 */
98
char i2c_read(int src) {
99
  char buf;
100

  
101
  buf = src;
102
  write(fd, &buf, 1);
103
  read(fd, &buf, 1);
104
  return buf;
105
}
106

  
107
/*
108
 * This function takes in an int representing the motor to be modified and an 
109
 * int representing the value to which to set the motor.
110
 */
111
void set_motor(int motor, int val) {
112
  if (motor == 0) {
113
    i2c_write(PAINT_MOTOR_A, val);
114
  } else {
115
    i2c_write(PAINT_MOTOR_B, val);
116
  }
117
}
118

  
119
/*
120
 * This function takes in an int representing the servo to be modified and an 
121
 * int representing the value to which to set the servo.
122
 */
123
void set_servo(int servo, int val) {
124
  if (servo == 0) {
125
    i2c_write(PAINT_SERVO_A, val);
126
  } else {
127
    i2c_write(PAINT_SERVO_B, val);
128
  }
129
}
130

  
131
/*
132
 * This function takes in an int representing the solenoid to be modified and
133
 * an int representing the value to which to set the solenoid.
134
 */
135
void set_solenoid(int solenoid, int val) {
136
  if (solenoid == 0) {
137
    i2c_write(PAINT_12V_1, val);
138
  } else if (solenoid == 1) {
139
    i2c_write(PAINT_12V_2, val);
140
  } else if (solenoid == 2) {
141
    i2c_write(PAINT_12V_3, val);
142
  } else {
143
    i2c_write(PAINT_12V_4, val);
144
  }
145
}
146

  
147
/*
148
 * This funciton takes an int representing which of the spare inputs to read.
149
 */
150
int get_input(int input) {
151
  switch (input) {
152
    case 0: return i2c_read(PAINT_INPUT_1);
153
    case 1: return i2c_read(PAINT_INPUT_2);
154
    case 2: return i2c_read(PAINT_INPUT_3);
155
    default: return 0;
156
  }
157
}
scout/painter/src/paint-i2c.h
1
/**
2
 * Copyright (c) 2013 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 */
25

  
26
/**
27
 * @file paint_i2c.h
28
 * @brief Contains i2c communication for the paintboard.
29
 *
30
 * @author Colony Project, CMU Robotics Club
31
 * @author Maung Aung (mza)
32
 * @author Tom Mullins (tmullins)
33
 *
34
 */
35

  
36
void i2c_start(void);
37
void i2c_stop(void);
38
void set_motor(int motor, int val);
39
void set_servo(int servo, int val);
40
void set_solenoid(int solenoid, int val);
41
int get_input(int input);
scout/painter/src/painter.cpp
1 1
/**
2
 * Copyright (c) 2011 Colony Project
2
 * Copyright (c) 2013 Colony Project
3 3
 * 
4 4
 * Permission is hereby granted, free of charge, to any person
5 5
 * obtaining a copy of this software and associated documentation
......
39 39
#include <messages/set_paintboard.h>
40 40
#include <messages/query_paintboard.h>
41 41
#include <messages/query_metal_detector.h>
42
#include "paint-i2c.h"
42 43

  
43 44
using namespace std;
44 45

  
46
int painter_value;
47

  
45 48
/**
46 49
 * @brief ROS callback to turn the paint output on or off
47 50
 */
48 51
void paint_set(const ::messages::set_paintboard::ConstPtr& msg)
49 52
{
53
  painter_value = msg->setting;
54
  set_servo(0, painter_value);
50 55
}
51 56

  
52 57
/**
......
55 60
bool paint_query(::messages::query_paintboard::Request &req,
56 61
                 ::messages::query_paintboard::Response &res)
57 62
{
63
  res.setting = painter_value;
58 64
  return true;
59 65
}
60 66

  
......
64 70
bool metal_query(::messages::query_metal_detector::Request &req,
65 71
                 ::messages::query_metal_detector::Response &res)
66 72
{
73
  res.metal = get_input(0);
67 74
  return true;
68 75
}
69 76

  
......
78 85
    ros::init(argc, argv, "painter");
79 86
    ros::NodeHandle node;
80 87

  
88
    i2c_start();
89
    set_motor(0, 0);
90
    set_servo(0, 0);
91

  
81 92
    ros::Subscriber painter_sub = node.subscribe("/set_paintboard", QUEUE_SIZE,
82 93
        paint_set);
83 94
    ros::ServiceServer painter_srv = node.advertiseService("/query_paintboard",
......
88 99
    ROS_INFO("Painter node ready.");
89 100
    ros::spin();
90 101

  
102
    i2c_stop();
103

  
91 104
    return 0;
92 105
}

Also available in: Unified diff