diff --git a/panther_body/CMakeLists.txt b/panther_body/CMakeLists.txt index dcd71ce..9ace032 100644 --- a/panther_body/CMakeLists.txt +++ b/panther_body/CMakeLists.txt @@ -14,7 +14,12 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs ) -catkin_package() +catkin_package( + CATKIN_DEPENDS + geometry_msgs + std_msgs + sensor_msgs +) ########### ## Build ## @@ -27,9 +32,10 @@ rosserial_generate_ros_lib( rosserial_configure_client( DIRECTORY firmware - TOOLCHAIN_FILE ${ROSSERIAL_ARDUINO_TOOLCHAIN} + TOOLCHAIN_FILE ${ROS_TEENSY_TOOLCHAIN} ) -rosserial_add_client_target(firmware body ALL) -rosserial_add_client_target(firmware body-upload) +# TODO: Removed waiting NUCLEO to change +# rosserial_add_client_target(firmware body_Firmware ALL) +# rosserial_add_client_target(firmware body-upload) diff --git a/panther_body/firmware/body.cpp b/panther_body/firmware/body.cpp deleted file mode 100644 index 22b4d3f..0000000 --- a/panther_body/firmware/body.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/** - * Copyright (C) 2020, Raffaello Bonghi - * All rights reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND - * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, - * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE - * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -// Use the following line if you have a Leonardo or MKR1000 -// #define USE_USBCON - -// ROS -#include -#include -#include -#include -// Neopixel -#include -#ifdef __AVR__ - #include -#endif -// I2C - Sonar SFR10 -#include -// Local imports -#include "SFR10.hpp" - -//ALREADY DEFINED -//#define LED_BUILTIN 13 -#define LEDS_LEFT 3 -#define LEDS_RIGHT 4 -// NeoPixels attached -#define NUMPIXELS 43 -// Number Ultrasounds SFR10 -#define SIZE_SFR10 2 - -// Topic definition -#define TWIST_SUBSCRIBER_TOPIC "/cmd_vel" -#define ENABLE_SUBSCRIBER_TOPIC "~status" - -// Initialization ROS -ros::NodeHandle nh; -// Initialization sensors -//SFR10 sensors[SIZE_SFR10] = {SFR10(112, "sfr10_left"), SFR10(113, "sfr10_right"), SFR10(113, "sfr10_rear")}; -//SFR10 sensors[SIZE_SFR10] = {SFR10(112, "sfr10_left"), SFR10(113, "sfr10_right")}; - -void TwistMessageCb( const geometry_msgs::Twist& msg) { - char tmp_buffer[50]; - // LOG linear and angular - sprintf(tmp_buffer, "lin:%.2f ang:%.2f", msg.linear.x, msg.angular.z); - // nh.logdebug(tmp_buffer); - nh.loginfo(tmp_buffer); -} - -void EnableMessageCb( const std_msgs::Bool& msg) { - // Update status controller - //controller_run = msg.data; - // Send log message information - //nh.logdebug("Bool status"); - nh.loginfo("Bool status"); -} - -// Reference Range message -// http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Range.html -sensor_msgs::Range range_msg; -ros::Publisher pub_range_left("sfr10_left", &range_msg); -ros::Publisher pub_range_right("sfr10_right", &range_msg); -// Define the twist subscriber -ros::Subscriber twist(TWIST_SUBSCRIBER_TOPIC, &TwistMessageCb); -ros::Subscriber enable_status(ENABLE_SUBSCRIBER_TOPIC, &EnableMessageCb); - -char frame_left[] = "frame_left"; -char frame_right[] = "frame_right"; -SFR10_t sensors[SIZE_SFR10]; - -void setup() -{ - // Initializtion ROS node - nh.initNode(); - // Initialization LED builtin - pinMode(LED_BUILTIN, OUTPUT); - // Initialization NeoPixels - pinMode(LEDS_LEFT, OUTPUT); - pinMode(LEDS_RIGHT, OUTPUT); - // Initialize SFR10 sensors - SFR10_connect(&range_msg); - SFR10_init(&sensors[0], &pub_range_left, &range_msg, 112, frame_left); - SFR10_init(&sensors[1], &pub_range_right, &range_msg, 113, frame_right); - - // Publish Sonar range - nh.advertise(pub_range_left); - nh.advertise(pub_range_right); - - // Initialization twist and enable subscriber - nh.subscribe(twist); - nh.subscribe(enable_status); -} - -bool status = true; -unsigned long range_timer; - - - -void loop() -{ - // publish the range value every 50 milliseconds - // since it takes that long for the sensor to stabilize - if ( (millis() - range_timer) > 50){ - // Update led status - digitalWrite(LED_BUILTIN, status); - //range_msg.range = sensors[0].getRange(); - int left = SFR10_publish(&nh, &sensors[0]); - //int right = SFR10_publish(&nh, &sensors[1]); - // Update range timer - range_timer = millis(); - status = !status; - } - nh.spinOnce(); -} -// EOF - diff --git a/panther_body/firmware/SFR10.cpp b/panther_body/firmware/body/SFR10.ino similarity index 69% rename from panther_body/firmware/SFR10.cpp rename to panther_body/firmware/body/SFR10.ino index be1cff8..52868e1 100644 --- a/panther_body/firmware/SFR10.cpp +++ b/panther_body/firmware/body/SFR10.ino @@ -28,58 +28,57 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - -#include - -#include "SFR10.hpp" - +/** + * @brief Connect I2C + * SFR10: https://www.robot-electronics.co.uk/htm/srf10tech.htm + */ void SFR10_connect(sensor_msgs::Range* range_msg) { // Waits to make sure everything is powered up before sending or receiving data Wire.begin(); - // Initialze range message - // SFR10 https://www.robot-electronics.co.uk/htm/srf10tech.htm + // Initialize range sensor range_msg->radiation_type = sensor_msgs::Range::ULTRASOUND; - // range_msg.header.frame_id = frameid; - range_msg->field_of_view = 0.01; - range_msg->min_range = 0.03; - range_msg->max_range = 0.4; + range_msg->field_of_view = 1; + range_msg->min_range = 0.0; + range_msg->max_range = 5.0; } - -void SFR10_init(SFR10_t* SFR10, ros::Publisher* pub, sensor_msgs::Range* range_msg, int address, char* name) +/** + * @brief Initialize ROS + */ +void SFR10_init(ros::NodeHandle* nh, SFR10_t &SFR10) { - SFR10->pub = pub; - SFR10->msg = range_msg; - SFR10->address = address; - SFR10->name = name; + // Initialize topic + nh->advertise(*SFR10.pub); } - -int SFR10_publish(ros::NodeHandle* nh, SFR10_t* SFR10) +/** + * @brief Publish range sensor + */ +void SFR10_publish(ros::NodeHandle* nh, SFR10_t &SFR10) { - // read range sensor - int range = SFR10_getRange(SFR10->address); + float range = ((float) SFR10.distance) / 100.0; // Publish message - SFR10->msg->header.stamp = nh->now(); - SFR10->msg->range = range; - SFR10->pub->publish(SFR10->msg); - // return range - return range; + SFR10.msg->header.stamp = nh->now(); + SFR10.msg->header.frame_id = SFR10.frame_id; + SFR10.msg->range = range; + SFR10.pub->publish(SFR10.msg); +} +/** + * @brief Update SFR10 status + */ +void SFR10_update(SFR10_t &SFR10) +{ + // read range sensor + SFR10.distance = SFR10_getRange(SFR10); } - /** * @brief SFR10 sonar * reference: https://www.robot-electronics.co.uk/htm/srf10tech.htm */ -int SFR10_getRange(int srfAddress) +int SFR10_getRange(SFR10_t &SFR10) { - int range = 0; + int distance = 0; // step 1: instruct sensor to read echoes - Wire.beginTransmission(srfAddress); // transmit to device #112 (0x70) + Wire.beginTransmission(SFR10.address); // transmit to device #112 (0x70) // the address specified in the datasheet is 224 (0xE0) // but i2c adressing uses the high 7 bits so it's 112 Wire.write(byte(0x00)); // sets register pointer to the command register (0x00) @@ -91,16 +90,16 @@ int SFR10_getRange(int srfAddress) // step 2: wait for readings to happen delay(70); // datasheet suggests at least 65 milliseconds // step 3: instruct sensor to return a particular echo reading - Wire.beginTransmission(srfAddress); // transmit to device #112 + Wire.beginTransmission(SFR10.address); // transmit to device #112 Wire.write(byte(0x02)); // sets register pointer to echo #1 register (0x02) Wire.endTransmission(); // stop transmitting // step 4: request reading from sensor - Wire.requestFrom(srfAddress, 2); // request 2 bytes from slave device #112 + Wire.requestFrom(SFR10.address, 2); // request 2 bytes from slave device #112 // step 5: receive reading from sensor if (2 <= Wire.available()) { // if two bytes were received - range = Wire.read(); // receive high byte (overwrites previous reading) - range = range << 8; // shift high byte to be high 8 bits - range |= Wire.read(); // receive low byte as lower 8 bits + distance = Wire.read(); // receive high byte (overwrites previous reading) + distance = distance << 8; // shift high byte to be high 8 bits + distance |= Wire.read(); // receive low byte as lower 8 bits } - return range; -} \ No newline at end of file + return distance; +} diff --git a/panther_body/firmware/body/body.ino b/panther_body/firmware/body/body.ino new file mode 100644 index 0000000..77bae7d --- /dev/null +++ b/panther_body/firmware/body/body.ino @@ -0,0 +1,261 @@ +/** + * Copyright (C) 2020, Raffaello Bonghi + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, + * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +//#define LED_BUILTIN 13 //ALREADY DEFINED +#define STRIP_LEFT 3 +#define STRIP_RIGHT 4 +// NeoPixels attached +#define STRIP_NUM 43 +#define STRIP_DEF_BRIGHTNESS 50 +// Topic definition +#define SUBSCRIBER_TWIST "cmd_vel" +#define SUBSCRIBER_STOP "e_stop" +#define SUBSCRIBER_ENABLE "enable" + +// ROS libraries +#include +// http://wiki.ros.org/std_msgs +#include +#include +// http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Range.html +#include +// I2C library +#include +// Load Arduino_NeoPixel ligray +// https://github.com/adafruit/Adafruit_NeoPixel +#include +#ifdef __AVR__ + #include +#endif + +char loginfo_buffer[100]; +// Functions definitions +void TwistMessageCb( const geometry_msgs::Twist& msg); +void EnableMessageCb(const std_msgs::Bool& msg); +void StopMessageCb(const std_msgs::Bool& msg); + + +// Soft timers +typedef struct _soft_timer { + bool start; + unsigned long currentMillis; + unsigned long previousMillis; + unsigned long interval; +} soft_timer_t; +// Publish timer +soft_timer_t publish, sfr10_update; +// Initialize ROS +ros::NodeHandle nh; +// Neopixel +typedef struct _neo_pixel { + // Parameter 1 = number of pixels in strip + // Parameter 2 = Arduino pin number (most are valid) + // Parameter 3 = pixel type flags, add together as needed: + // NEO_KHZ800 800 KHz bitstream (most NeoPixel products w/WS2812 LEDs) + // NEO_KHZ400 400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers) + // NEO_GRB Pixels are wired for GRB bitstream (most NeoPixel products) + // NEO_RGB Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2) + // NEO_RGBW Pixels are wired for RGBW bitstream (NeoPixel RGBW products) + Adafruit_NeoPixel NEOPixel; // NEOPIXEL line definition + unsigned int brightness; + struct { + bool enable; + unsigned int start; + unsigned int end; + uint32_t color; + } range; + struct { + bool enable; + uint32_t color; + bool status; + unsigned int step; + int counter; + int newbrightness; + } bright; + struct { + bool enable; + unsigned int counter; + } rainbow; +} neo_pixel_t; +neo_pixel_t strip_left = {Adafruit_NeoPixel(STRIP_NUM, STRIP_LEFT, NEO_GRB + NEO_KHZ800), + STRIP_DEF_BRIGHTNESS}; +neo_pixel_t strip_right = {Adafruit_NeoPixel(STRIP_NUM, STRIP_RIGHT, NEO_GRB + NEO_KHZ800), + STRIP_DEF_BRIGHTNESS}; +// SFR10 definition +typedef struct _SFR10 { + int address; + char frame_id[15]; + ros::Publisher* pub; + sensor_msgs::Range* msg; + int distance; +} SFR10_t; +// Message +sensor_msgs::Range range_msg; +// Initialization SFR10 sensors +#define SFR10_SIZE 3 +#define SFR10_LEFT 0 +#define SFR10_RIGHT 1 +#define SFR10_REAR 2 +ros::Publisher pub_range_left("sfr10_left", &range_msg); +ros::Publisher pub_range_right("sfr10_right", &range_msg); +ros::Publisher pub_range_rear("sfr10_rear", &range_msg); +SFR10_t sensors[SFR10_SIZE] = { + {112, "frame_left", &pub_range_left, &range_msg, 0}, + {113, "frame_right", &pub_range_right, &range_msg, 0}, + {114, "frame_rear", &pub_range_rear, &range_msg, 0}}; +// Subscribers update +ros::Subscriber sub_twist(SUBSCRIBER_TWIST, &TwistMessageCb); +ros::Subscriber sub_stop(SUBSCRIBER_STOP, &StopMessageCb); +ros::Subscriber sub_enable(SUBSCRIBER_ENABLE, &EnableMessageCb); + +geometry_msgs::Twist twist; +bool enable_status = false; +bool stop_status = false; + +/** + @brief TwistMessageCb Twist message callback and conversion linear and angular velocity in led effect +*/ +void TwistMessageCb( const geometry_msgs::Twist& msg) { + // Save linear velocity + twist = msg; + // LOG linear and angular + sprintf(loginfo_buffer, "Twist: lin:%.2f ang:%.2f", twist.linear.x, twist.angular.z); + nh.loginfo(loginfo_buffer); +} +/** + * @brief Enable message + */ + +void EnableMessageCb(const std_msgs::Bool& msg) { + // Update status controller + enable_status = msg.data; + // Send log message information + sprintf(loginfo_buffer, "Enable: %d", enable_status); + nh.loginfo(loginfo_buffer); +} +/** + * @brief Stop message + */ +void StopMessageCb(const std_msgs::Bool& msg) { + // Update status controller + stop_status = msg.data; + // Send log message information + sprintf(loginfo_buffer, "Stop: %d", stop_status); + nh.loginfo(loginfo_buffer); +} + + +void setup() +{ + // Initialization LED builtin + pinMode(LED_BUILTIN, OUTPUT); + // Initialization NeoPixels + NEOpixel_init(strip_left); + NEOpixel_init(strip_right); + // Initialize ROS and topics + nh.initNode(); + nh.subscribe(sub_twist); + nh.subscribe(sub_enable); + nh.subscribe(sub_stop); + // Connect SFR10 + SFR10_connect(&range_msg); + // Initialize all sensors + for(int i = 0; i < SFR10_SIZE; ++i) + { + SFR10_init(&nh, sensors[i]); + } + // Initialize soft timer + soft_timer_init(sfr10_update, 0.125); + soft_timer_init(publish, 0.500); +} + + +bool led_status = true; + + +void loop() +{ + NEOpixel_clear(strip_left); + NEOpixel_clear(strip_right); + if(enable_status) + { + NEOpixel_rainbow(strip_left); + NEOpixel_rainbow(strip_right); + } else { + NEOpixel_rainbow_stop(strip_left); + NEOpixel_rainbow_stop(strip_right); + } + // Fast loop update SFR10 sensor + if (soft_timer_run(sfr10_update)) + { + // Update SFR10 status + for(int i = 0; i < SFR10_SIZE; ++i) + { + SFR10_update(sensors[i]); + } + // Run only if is not in stop + if(enable_status) + { + NEOpixel_range(strip_left, strip_left.NEOPixel.Color(255, 0, 0), sensors[SFR10_LEFT].distance / 2, true); + NEOpixel_range(strip_right, strip_right.NEOPixel.Color(255, 0, 0), sensors[SFR10_RIGHT].distance / 2, true); + NEOpixel_range(strip_left, strip_left.NEOPixel.Color(255, 0, 0), sensors[SFR10_REAR].distance / 2, false); + NEOpixel_range(strip_right, strip_right.NEOPixel.Color(255, 0, 0), sensors[SFR10_REAR].distance / 2, false); + } else { + NEOpixel_range_stop(strip_left); + NEOpixel_range_stop(strip_right); + } + } + // Slow loop publish SFR10 status + if (soft_timer_run(publish)) + { + // Update led status + digitalWrite(LED_BUILTIN, led_status); + led_status = !led_status; + // Update SFR10 status + for(int i = 0; i < SFR10_SIZE; ++i) + { + SFR10_publish(&nh, sensors[i]); + } + } + // Run only if is not in stop + if(stop_status) + { + NEOpixel_bright(strip_left, strip_left.NEOPixel.Color(255, 0, 0), 250); + NEOpixel_bright(strip_right, strip_right.NEOPixel.Color(255, 0, 0), 250); + } else { + NEOpixel_bright_stop(strip_left); + NEOpixel_bright_stop(strip_right); + } + // Update neopixels + NEOpixel_update(strip_left); + NEOpixel_update(strip_right); + // ROS spin update + nh.spinOnce(); +} diff --git a/panther_body/firmware/body/strip.ino b/panther_body/firmware/body/strip.ino new file mode 100644 index 0000000..0ccc7f0 --- /dev/null +++ b/panther_body/firmware/body/strip.ino @@ -0,0 +1,194 @@ +/** + * Copyright (C) 2020, Raffaello Bonghi + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, + * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * @brief Initialize Neopixel + */ +void NEOpixel_init(neo_pixel_t &strip) +{ + pinMode(strip.NEOPixel.getPin(), OUTPUT); + // Initialize STRIPs + strip.NEOPixel.begin(); + strip.NEOPixel.setBrightness(strip.brightness); + strip.NEOPixel.show(); // Initialize all pixels to 'off' + // Initialize effects + strip.range.enable = false; + strip.bright.enable = false; + strip.bright.step = 10; +} +void NEOpixel_clear(neo_pixel_t &strip) +{ + strip.NEOPixel.clear(); +} +void NEOpixel_update(neo_pixel_t &strip) +{ + // Update rainbow + NEOpixel_rainbow_update(strip); + // Update brightness effect + NEOpixel_bright_update(strip); + // Show + strip.NEOPixel.show(); +} +/** + * @brief Set a range color + */ +void NEOpixel_range(neo_pixel_t &strip, uint32_t color, int range, bool reverse) +{ + // Set color + strip.range.color = color; + // set position + strip.range.start = 0; + strip.range.end = range; + if(range > strip.NEOPixel.numPixels()) + { + strip.range.end = strip.NEOPixel.numPixels(); + } + if(reverse) + { + strip.range.start = strip.NEOPixel.numPixels() - range; + strip.range.end = strip.NEOPixel.numPixels(); + } + // Update colors + for(unsigned int i = strip.range.start; i < strip.range.end; ++i) + { + strip.NEOPixel.setPixelColor(i, strip.range.color); + } + // Enable effect + //strip.range.enable = true; +} +/** + * @brief Stop range effect + */ +void NEOpixel_range_stop(neo_pixel_t &strip) +{ + strip.range.enable = false; + strip.NEOPixel.clear(); +} +/** + * @brief Bright strip + */ +void NEOpixel_bright(neo_pixel_t &strip, uint32_t color, int brightness) +{ + if(strip.bright.enable) + return; + strip.bright.color = color; + strip.bright.counter = strip.brightness; + strip.bright.newbrightness = brightness; + strip.bright.enable = true; + strip.bright.status = true; +} +/** + * @brief bright stop + */ +void NEOpixel_bright_stop(neo_pixel_t &strip) +{ + strip.NEOPixel.setBrightness(strip.brightness); + strip.bright.enable = false; +} +/** + * @brief brightness update + */ +void NEOpixel_bright_update(neo_pixel_t &strip) +{ + if(strip.bright.enable) + { + for(unsigned int i = 0; i < strip.NEOPixel.numPixels(); ++i) + { + strip.NEOPixel.setPixelColor(i, strip.bright.color); + } + if(strip.bright.status) + { + // Update counter effect + strip.bright.counter += strip.bright.step; + if(strip.bright.counter > strip.bright.newbrightness) + { + strip.bright.counter = strip.bright.newbrightness; + strip.bright.status = false; + } + } + else + { + strip.bright.counter -= strip.bright.step; + if(strip.bright.counter < 0) + { + strip.bright.counter = 0; + strip.bright.status = true; + } + } + // Update brighness + strip.NEOPixel.setBrightness(strip.bright.counter); + } +} +/** + * @brief Rainbow effect + */ +void NEOpixel_rainbow(neo_pixel_t &strip) +{ + if(strip.rainbow.enable) + return; + strip.rainbow.counter = 0; + strip.rainbow.enable = true; +} +/** + * @brief disable rainbow + */ +void NEOpixel_rainbow_stop(neo_pixel_t &strip) +{ + strip.rainbow.enable = false; +} +void NEOpixel_rainbow_update(neo_pixel_t &strip) +{ + if(strip.rainbow.enable) + { + for(unsigned int i = 0; i < strip.NEOPixel.numPixels(); i++) { + strip.NEOPixel.setPixelColor(i, Wheel(&strip.NEOPixel, (i + strip.rainbow.counter) & 255)); + } + //Increase counter + strip.rainbow.counter += 1; + if(strip.rainbow.counter > 255) + { + strip.rainbow.counter = 0; + } + } +} +// Input a value 0 to 255 to get a color value. +// The colours are a transition r - g - b - back to r. +uint32_t Wheel(Adafruit_NeoPixel* strip, byte WheelPos) { + WheelPos = 255 - WheelPos; + if(WheelPos < 85) { + return strip->Color(255 - WheelPos * 3, 0, WheelPos * 3); + } + if(WheelPos < 170) { + WheelPos -= 85; + return strip->Color(0, WheelPos * 3, 255 - WheelPos * 3); + } + WheelPos -= 170; + return strip->Color(WheelPos * 3, 255 - WheelPos * 3, 0); +} diff --git a/panther_body/firmware/SFR10.hpp b/panther_body/firmware/body/system.ino similarity index 70% rename from panther_body/firmware/SFR10.hpp rename to panther_body/firmware/body/system.ino index 80616ab..3eebf4a 100644 --- a/panther_body/firmware/SFR10.hpp +++ b/panther_body/firmware/body/system.ino @@ -28,28 +28,38 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include - #include -typedef struct _SFR10 { - sensor_msgs::Range* msg; - ros::Publisher* pub; - int address; - char* name; -} SFR10_t; +void soft_timer_init(soft_timer_t &timer, float inter) +{ + timer.start = true; + timer.currentMillis = 0; + timer.previousMillis = 0; + timer.interval = inter * 1000; +} -/** - * - */ -void SFR10_connect(sensor_msgs::Range* range_msg); +void soft_timer_start(soft_timer_t &timer) +{ + timer.start = true; + timer.previousMillis = millis(); +} +void soft_timer_stop(soft_timer_t &timer) +{ + timer.start = false; +} -void SFR10_init(SFR10_t* SFR10, ros::Publisher* pub, sensor_msgs::Range* range_msg, int address, char* name); -/** - * - */ -int SFR10_publish(ros::NodeHandle* nh, SFR10_t* SFR10); -/** - * Read the status of the sensor - */ -int SFR10_getRange(int srfAddress); \ No newline at end of file +bool soft_timer_run(soft_timer_t &timer) +{ + if(timer.start) { + // software timer + timer.currentMillis = millis(); + // Evaluate interval + if (timer.currentMillis - timer.previousMillis >= timer.interval) { + // save the last time + timer.previousMillis = timer.currentMillis; + + return true; + } + } + return false; +} diff --git a/panther_body/launch/body.launch b/panther_body/launch/body.launch index 2fdbc15..9145c40 100644 --- a/panther_body/launch/body.launch +++ b/panther_body/launch/body.launch @@ -26,8 +26,8 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. --> - - + + + diff --git a/panther_joystick/config/joystick.yaml b/panther_joystick/config/joystick.yaml index 2c99a17..dd3085d 100644 --- a/panther_joystick/config/joystick.yaml +++ b/panther_joystick/config/joystick.yaml @@ -8,7 +8,7 @@ buttons: # Enable/Disable button button: 7 # Topic to send enable/disable message - topic: 'led_controller/status' + topic: 'enable' # Define emergency button for Roboteq controller emergency: # Enable/Disable button