void initialize_ds_pins(void) {
const uint8_t NUM_PINS = 11;
uint8_t pin_map[NUM_PINS] = {
DS.A, DS.B, DS.X, DS.Y, DS.L, DS.R,
DS.LEFT, DS.RIGHT, DS.UP, DS.DOWN, DS.START
};
// Set up all button pins, and give them initial values
for (int i = 0; i < NUM_PINS; i++) {
pinMode(pin_map[i], INPUT);
button_control(pin_map[i], 0);
}
// Set up c-stick pins
pinMode(DS.X_AXIS, OUTPUT);
pinMode(DS.Y_AXIS, OUTPUT);
analogWrite(DS.X_AXIS, 30);
analogWrite(DS.Y_AXIS, 200);
}
void JoyControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
for (std::map<std::string, int>::iterator list_iter = step_axis_map_.begin(); list_iter != step_axis_map_.end(); list_iter++)
{
// ROS_INFO("Step axis name: %s", list_iter->first.c_str());
stepAxis(joy, list_iter->first);
}
for (std::map<std::string, int>::iterator list_iter = axis_map_.begin(); list_iter != axis_map_.end(); list_iter++)
{
// ROS_INFO("Axis name: %s", list_iter->first.c_str());
axis(joy, list_iter->first);
}
for (std::map<std::string, int>::iterator list_iter = button_map_.begin(); list_iter != button_map_.end(); list_iter++)
{
if (button_control(joy, list_iter->second))
{
ButtonCommand function = func_button_map_.find(list_iter->first)->second;
function();
}
}
}
