Ask Your Question
0

How to get my LED blinking? (while loop breaks rosserial python node)

asked 2019-11-19 15:15:07 -0600

vexity gravatar image

I'm using ROS Kinetic with Ubuntu Xenial 16.04

So i've set up my RGB Leds and have it working for turning on or off depending on what is being published. Now I would like to have the Led flashing at a certain frequency, but when I add a while loop to do so, my rosserial python node goes out of sync. How can I do this then?

I have my arduino code, and a custom library for the LEDS (leds.cpp and leds.h) files. There are other operations running such as the motors, i don't know if that affects anything or not. (If not, please ignore them)

Leds.h file:

#ifndef Leds_h
#define Leds_h
#include <Arduino.h>

void initializeLeds();
void setColor(int r, int g, int b);
void flashColor(String name);

#endif

Leds.cpp file:

#include <Leds.h>
#include <Arduino.h>
#include <std_msgs/String.h>
#include "ros.h"


#define R_PIN 44
#define G_PIN 46
#define B_PIN 45

bool initialized = false;

void initializeLeds(){
    pinMode(R_PIN, OUTPUT);
    pinMode(G_PIN, OUTPUT);
    pinMode(B_PIN, OUTPUT);
    initialized = true;
}

void flashColor(String name) {
    while (name=="PreQualify") {
    setColor(0,0,1);
    }

    if (name=="GateAndSurface") {
    setColor(0,1,0);
    }

    if (name=="None") {
    setColor(0,0,0);
    }
}
void setColor(int r, int g, int b){

    if(initialized){
    switch(r) {
    case 0: digitalWrite(R_PIN, LOW);
    break;
    case 1: digitalWrite(R_PIN, HIGH);
    break;
    default:
    break;
    }
    switch(g) {
    case 0: digitalWrite(G_PIN, LOW);
    break;
    case 1: digitalWrite(G_PIN, HIGH);
    break;
    default:
    break;
    }
    switch(b) {
    case 0: digitalWrite(B_PIN, LOW);
    break;
    case 1: digitalWrite(B_PIN, HIGH);
    break;
    default:
    break;
    }

  }
}

Arduino code:

#include <Arduino.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <controller_pkg/HorizontalMotors.h>
#include <controller_pkg/VerticalMotors.h>
#include <Motors.h>
#include <Wire.h>
#include <blue_robotics.h>
#include <Servo.h>
#include <Leds.h>
#include <string.h>

//std_msgs::String color;
std_msgs::Float32 depth;
MS5837 pressureSensor;
String name;

// Callback function for left motors
void setHorizontalMotors(const controller_pkg::HorizontalMotors&  motorIntensity)
{
   setHorizontalMotorSpeed(motorIntensity.rightMotor, motorIntensity.leftMotor);
}

// Callback function for simultaneous usage of motors
void setVerticalMotors(const controller_pkg::VerticalMotors& motorIntensity)
{
    setVerticalMotorSpeed(motorIntensity.frontRight, motorIntensity.frontLeft, motorIntensity.backRight, motorIntensity.backLeft);
}

// Callback function for LED lights.
void LEDCallback(const std_msgs::String& msg) {
    name = msg.data;
    flashColor(name);
}

ros::NodeHandle nh;

// Creates all the ROS subscriber objects
ros::Subscriber<controller_pkg::VerticalMotors> subVerticalMotors("vertical_motors", setVerticalMotors);
ros::Subscriber<controller_pkg::HorizontalMotors> subHorizontalMotors("horizontal_motors", setHorizontalMotors);
ros::Subscriber<std_msgs::String> subCurrentMission("CurrentMission", &LEDCallback);

// Create the pressure sensor publisher
ros::Publisher pubToPressureSensor("depth_current", &depth);


void setup() {
  // Initializes the node and prepares for publishing and subscribing.
  nh.initNode();
  nh.advertise(pubToPressureSensor);
  nh.subscribe(subVerticalMotors);
  nh.subscribe(subCurrentMission);
  nh.subscribe(subHorizontalMotors);


  // Initializes all the motors
  initializeVerticalMotors();
  initializeHorizontalMotors("old");

  //initializes leds
  initializeLeds();

  //Initializes wire (protocol to run)
  Wire.begin();

  // If pressure sensor does not initialize, throw error

  while(!pressureSensor.init())
  {

      nh.logerror("Pressure sensor cannot initialize.");

  }

}

void loop()
{

  pressureSensor.read();

  depth.data = pressureSensor.depth()*(3.3);

  pubToPressureSensor.publish(&depth);

  nh.spinOnce();
}

I'd like to add the loop inside my Leds.cpp file, specifically on ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2019-11-20 12:15:03 -0600

duck-development gravatar image

The problem is an the arduino you only have one thread. So if you like to time the blinking you may use a timer, with an isr. Blocking wait is not a solution.

edit flag offensive delete link more

Comments

If possible, could you elaborate more on how I can add the timer and the isr? I’m not very well versed in using Arduino and such

vexity gravatar imagevexity ( 2019-11-20 16:20:03 -0600 )edit
duck-development gravatar imageduck-development ( 2019-11-21 00:49:39 -0600 )edit

Thank you! I'll check it out!

vexity gravatar imagevexity ( 2019-11-22 07:42:58 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2019-11-19 15:15:07 -0600

Seen: 19 times

Last updated: Nov 20