How to get my LED blinking? (while loop breaks rosserial python node)
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 my flashColor function, that depending on what the string says, it'll flash a different color, but adding a while loop breaks the rosserial node's sync.
Asked by vexity on 2019-11-19 16:15:07 UTC
Answers
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.
Asked by duck-development on 2019-11-20 13:15:03 UTC
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
Asked by vexity on 2019-11-20 17:20:03 UTC
You may read this https://www.instructables.com/id/Arduino-Timer-Interrupts/ Or this is also quite good https://learn.adafruit.com/multi-tasking-the-arduino-part-2/timers
Asked by duck-development on 2019-11-21 01:49:39 UTC
Thank you! I'll check it out!
Asked by vexity on 2019-11-22 08:42:58 UTC
Comments