int main()
{
  // SET single LED port
  DDRC |= _BV(LED_PIN);
  PORTC ^= _BV(LED_PIN);
  // 1s pull up of LED Strip pins */
  apa102_DDRREG &= ~_BV(apa102_data);
  apa102_DDRREG &= ~_BV(apa102_clk);
  apa102_PORTREG |= _BV(apa102_data);
  apa102_PORTREG |= _BV(apa102_clk);
  _delay_ms(900);
  // Disable pull ups
  apa102_PORTREG &= ~_BV(apa102_data);
  apa102_PORTREG &= ~_BV(apa102_clk);
  apa102_DDRREG |= _BV(apa102_data);
  apa102_DDRREG |= _BV(apa102_clk);
  _delay_ms(100);
  PORTC ^= _BV(LED_PIN);

  // Clear LEDs in the strip
  clear_all_leds();

  // Init ROS
  nh.initNode();
  nh.subscribe(set_sub);
  nh.subscribe(set_led_sub);

  ack_led();

  // Wait for Server side to start
  while (!nh.connected())
  {
    nh.spinOnce();
    // LUFA functions that need to be called frequently to keep USB alive
    CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
    USB_USBTask();
    _delay_ms(10);
  }

  ack_led();

  // Publish some debug information
  snprintf(log_str, MAX_MSG_SIZE, "V:%s", GIT_VERSION);
  nh.loginfo(log_str);
  snprintf(log_str, MAX_MSG_SIZE, "FM:%d", get_free_ram());
  nh.loginfo(log_str);

  while(1)
  {
    nh.spinOnce();
    // LUFA functions that need to be called frequently to keep USB alive
    CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
    USB_USBTask();
  }

  return 0;
}
void set_cb(const autonomy_leds_msgs::Command& cmd_msg)
{
  switch (cmd_msg.flag)
  {
    case autonomy_leds_msgs::Command::FLAG_SET_ALL:
    {
      ros_buffer_ptr = cmd_msg.colors_vec;
      ros_buffer_size = cmd_msg.colors_vec_length;
      apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size);
      break;
    }
    case autonomy_leds_msgs::Command::FLAG_CLEAR:
    {
      // Clear everything, it should work even w/o a buffer
      clear_all_leds();
      ros_buffer_size = 0;
      break;
    }
    case autonomy_leds_msgs::Command::FLAG_SHIFTLEFT:
    {
      if (ros_buffer_size == 0) break;
      uint16_t buffer = ros_buffer_ptr[0];
      for (led_counter = 0; led_counter < ros_buffer_size - 1; led_counter++)
      {
        ros_buffer_ptr[led_counter] = ros_buffer_ptr[led_counter + 1];
      }
      ros_buffer_ptr[led_counter] = buffer;
      apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size);
      break;
    }
    case autonomy_leds_msgs::Command::FLAG_SHIFTRIGHT:
    {
      if (ros_buffer_size == 0) break;
      uint16_t buffer = ros_buffer_ptr[ros_buffer_size - 1];
      for (led_counter = ros_buffer_size - 1; led_counter > 0; led_counter--)
      {
        ros_buffer_ptr[led_counter] = ros_buffer_ptr[led_counter - 1];
      }
      ros_buffer_ptr[led_counter] = buffer;
      apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size);
      break;
    }
    case autonomy_leds_msgs::Command::FLAG_INVERT:
    {
      if (ros_buffer_size == 0) break;
      for (led_counter = 0; led_counter < ros_buffer_size; led_counter++)
      {
        ros_buffer_ptr[led_counter] ^= 0b0111111111111111;
      }
      apa102_setleds_packed(ros_buffer_ptr, ros_buffer_size);
      break;
    }
  }

#ifdef PUBLISH_FREE_RAM
  snprintf(log_str, MAX_MSG_SIZE, "%d", get_free_ram());
  nh.loginfo(log_str);
#endif

}
Example #3
0
static inline void quaternionTFToMsg(const geometry_msgs::Quaternion& bt, geometry_msgs::Quaternion& msg)  {
	if (fabs(length2(bt) - 1 ) > QUATERNION_TOLERANCE)  {
		nh.loginfo("TF to MSG: Quaternion Not Properly Normalized");
		geometry_msgs::Quaternion bt_temp = bt; 
		bt_temp = normalize(bt_temp);
		msg.x = bt_temp.x; msg.y = bt_temp.y; msg.z = bt_temp.z;  msg.w = bt_temp.w;
	} else {
		msg.x = bt.x; msg.y = bt.y; msg.z = bt.z;  msg.w = bt.w;
	}
};
Example #4
0
void callback(const geometry_msgs::PoseArray& msg) {
  sum_msg.position.x = 0;
  sum_msg.position.y = 0;
  sum_msg.position.z = 0;
  char message[40];
  sprintf(message, "%d %d %d %d",
      msg.poses_length,
      (int) msg.poses[0].position.x,
      (int) msg.poses[0].position.y,
      (int) msg.poses[0].position.z);
  node_handle.loginfo(message);
  for(int i = 0; i < msg.poses_length; i++) {
    sum_msg.position.x += msg.poses[i].position.x;
    sum_msg.position.y += msg.poses[i].position.y;
    sum_msg.position.z += msg.poses[i].position.z;
  }
  publisher.publish(&sum_msg);
}
Example #5
0
int main() {
    nh.initNode();
    nh.advertise(chatter);

    while (1) {
        str_msg.data = hello;
        chatter.publish( &str_msg );

        nh.logdebug(debug);
        nh.loginfo(info);
        nh.logwarn(warn);
        nh.logerror(errors);
        nh.logfatal(fatal);

        nh.spinOnce();
        wait_ms(500);
    }
}