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
void do_computer_control(void) {

  while (1) {
    read_switches();
    if (function_changed) {
      // oops i guess they want something else, return!
      clear_all_leds();
      clock_leds();
      return;
    }
    
    //putstring("computer kontrol\n\r");
    
  }
}