To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 8a1314d2 authored by remartin's avatar remartin
Browse files

Added copyright headers, cosmetic changes. Appended Arduino file.

parent 46222d21
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Handles the connection to the Arduino via a serial port.
// Based on previously existing SerialUnix file.
//
// ----------------------------------------------------------------------------------
#pragma once #pragma once
...@@ -18,8 +52,6 @@ ...@@ -18,8 +52,6 @@
//SERIAL COMMUNICATION PARAMETERS //SERIAL COMMUNICATION PARAMETERS
#define MAX_SERIAL_BUFFER_SIZE 180 #define MAX_SERIAL_BUFFER_SIZE 180
//#define SERIAL_BAUD_RATE B921600
#define SERIAL_LIGHT_BAUD_RATE B9600 #define SERIAL_LIGHT_BAUD_RATE B9600
#define SERIAL_BYTE_SIZE 8 #define SERIAL_BYTE_SIZE 8
...@@ -40,19 +72,22 @@ ...@@ -40,19 +72,22 @@
class SerialLight class SerialLight{
{
public: public:
SerialLight(); SerialLight();
// Opens the connection. Needs to detect the
// correct port first.
void init(); void init();
void closeConnection(); void closeConnection();
//bool isConnected(const char* port = "");
bool isConnected(); bool isConnected();
// send something through the port.
bool writeSerialLight(const char* buffer); bool writeSerialLight(const char* buffer);
int manageSerial(); // redundant
std::string readSerialLight(); std::string readSerialLight();
private: private:
......
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// ROS node that listens to UWB position data
// and handles the spotlight calibration procedure.
//
// ----------------------------------------------------------------------------------
#ifndef UWBSPOTLIGHT_MAIN_H #ifndef UWBSPOTLIGHT_MAIN_H
...@@ -13,16 +42,42 @@ ...@@ -13,16 +42,42 @@
#include "std_msgs/UInt32MultiArray.h" #include "std_msgs/UInt32MultiArray.h"
#include "d_fall_pps/CrazyflieData.h" #include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/UWBAnchor.h" #include "d_fall_pps/UWBAnchor.h"
#include "d_fall_pps/UWBAnchorArray.h" #include "d_fall_pps/UWBAnchorArray.h"
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
#include <stdio.h>
#include <string>
#include <vector>
#include <ros/package.h>
#include <rosbag/bag.h>
#include <ros/ros.h>
#include "spotlightManager.h"
namespace d_fall_pps{ namespace d_fall_pps{
spotlightManager slM;
ros::ServiceClient uwbManager;
int spotlightAnchor = 5; // Anchor placed next to the spotlight
bool placedSpotlight = false;
//bool foundTarget = false;
bool finishedCalibration = false;
int frequencyDivision = 150; // 67 Hz by UWB.
int clockState = 1;
// Received data from the target/crazyflie
void updateTargetPositionCallback(const CrazyflieData &data); void updateTargetPositionCallback(const CrazyflieData &data);
void anchorArrangementChangedCallback(const std_msgs::Int32& msg); void anchorArrangementChangedCallback(const std_msgs::Int32& msg);
// reading one necessary anchor value
void placeSpotlight(); void placeSpotlight();
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
} }
#endif #endif
......
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Deals with Coordinate transformations and translation into DMX values.
// Provides various functions used for the calibration procedure
// and testing purposes.
//
// ----------------------------------------------------------------------------------
#pragma once #pragma once
#include<stdio.h>
#include <stdio.h>
#include "SerialLight.h" #include "SerialLight.h"
#include <list> #include <list>
...@@ -10,39 +46,48 @@ ...@@ -10,39 +46,48 @@
#include <cmath> #include <cmath>
#include <sstream> #include <sstream>
#include <iostream>
#include <termios.h> //For serial communication
class spotlightManager{ class spotlightManager{
public: public:
spotlightManager(); spotlightManager();
void someDefaultSettings();
void setPTmanually(bool blockOverhead);
// toggles some pan values. // Turns on the lamp.
void trivialDemo(); void someDefaultSettings();
// changes the dmx values directly.
void lineOnWall(bool ptMode, int msInterval);
// uses actual coordinates
void testPTupdater();
void setTargetPosition(double x, double y, double z); void setTargetPosition(double x, double y, double z);
void setSpotlightPosition(double x, double y, double z); void setSpotlightPosition(double x, double y, double z);
// calculates pan and tilt.
void updatePTvalues();
// private afterwards: // WASD key control
void setPTmanually(bool blockOverlap);
void updateSpherical(); // Procedure to manually point the head at the target.
void updatePTvalues();
void setPositions(double spotlightPosition[3], double targetPosition[3]);
void calibrateOrientation(); void calibrateOrientation();
// Reacts to yaw changes of the target.
void fancyColourFunction(double yaw); void fancyColourFunction(double yaw);
// toggles some pan values.
void trivialDemo();
// changes the dmx values directly.
void lineOnWall(bool ptMode, int msInterval);
// changes actual coordinates.
void testPTupdater();
private: private:
int posPT; int posPT; //Where the four PT values start.
SerialLight sL; SerialLight sL;
const static char portLocation[]; const static char portLocation[];
unsigned char defaultChannel; // value between 0 and 255... unsigned char defaultChannel; // value between 0 and 255...
...@@ -57,6 +102,8 @@ private: ...@@ -57,6 +102,8 @@ private:
double spherical[3]; // saving that is bad practice? double spherical[3]; // saving that is bad practice?
//void calibrateOrientation(); //void calibrateOrientation();
void updateSpherical();
// construct strings that can be decoded by the arduino // construct strings that can be decoded by the arduino
std::string buildMessage(); // 'bad practice to import namespace std in header' std::string buildMessage(); // 'bad practice to import namespace std in header'
std::string buildPT(); std::string buildPT();
......
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Arduino program required to control the spotlight.
// Listens to serial port and translates int the DMX protocol.
//
// ----------------------------------------------------------------------------------
#include <DmxSimple.h>
const byte channelMode = 25; // The spotlight requires all values.
byte currentDmxMessage[channelMode];
byte channelOffset = 0; // corresponding to the device addressed.
byte posPT = 3; // offset of the pan/tilt DMX values.
unsigned long time = 0;
// open the serial connection.
void setup(){
Serial.begin(9600); // 9600 baud
Serial.println("Arduino_DMX has started.");
Serial.println();
Serial.println("Syntax:");
Serial.print(" c*c : DMX channel index is *. ");
Serial.println("Must be below 128-(14/25) due to Arduino limitations.");
Serial.println(" m199,255,40,10,0,33,m : some DMX command. Comma at the end! ");
Serial.println(" p200,200,30,0,p : setting pan, pan fine, tilt, tilt fine only. ");
Serial.println();
Serial.println();
sendFullDmxMessage(); // required to send it out once.
}
void loop() {
// t2 = milis()
//printCurrentDmxMessage();
//sendDmxMessage();
}
void sendFullDmxMessage(){
//Serial.print("Sent out (ch/Setting):");
for(int gg = channelOffset; gg < channelOffset + channelMode; gg++){
//if(gg>128) Serial.println(std7); covered when setting
DmxSimple.write(gg, currentDmxMessage[gg-channelOffset]);
// Serial.print(gg);
// Serial.print("/");
// Serial.print(currentDmxMessage[gg-channelOffset]);
// Serial.print(", ");
}
// Serial.println();
}
void printCurrentDmxMessage(){
Serial.print("Msg atm: ");
for(int a = channelOffset; a < channelOffset + 14; a++){
Serial.print(currentDmxMessage[a-channelOffset]);
Serial.print(", ");
}
Serial.println();
}
int parserTemp = 0;
int i = 0; // Offset in the currentDmxMessage-Array
bool readingMessage = false;
bool readingChannelOffset = false;
bool readingPT = false;
// Reads messages from the serial port: Either new channel numbers (= channelOffset) or DmxMessages.
void serialEvent(){
//Serial.println("Received some stuff here");
//Serial.print("Received nr. of bytes: ");
//Serial.println(Serial.available());
while(Serial.available()){
char ch = Serial.read();
//Serial.write(ch); // Necessary as ACK?
// Detection structure: dm
// Problem: Arduino sends some info by default -> needs to match precise format.
if(!readingMessage && ch == 'm'){
readingMessage = true;
i = 0;
time = micros();
}else if(!readingChannelOffset && ch == 'c'){
readingChannelOffset = true;
}else if(!readingPT && ch == 'p'){
readingPT = true;
i = 3;
time = micros();
}else if(readingMessage){
if(ch == 'm'){//End reached
parserTemp = 0;
i = 0; // reset array offset
readingMessage = false;
time = micros() - time;
Serial.print("Parsing took uS: ");
Serial.println(time, DEC);
}else if(ch == ','){
if(parserTemp>255 || parserTemp<0){
Serial.println("Warning: DMX-Range"); //(std3);
}else{
currentDmxMessage[i] = parserTemp;
DmxSimple.write(channelOffset+i, parserTemp);
}
parserTemp = 0;
i++;
}else if(isDigit(ch)){
parserTemp = 10*parserTemp + (ch-'0');
}else{// abort that attempt and reset everything.
parserTemp = 0;
i = 0;
readingMessage = false;
//Serial.println("Error while parsing a message.");
}
}else if(readingChannelOffset){
if(ch == 'c'){//End of channel setting
if(parserTemp>103){
Serial.println("Warning: ChNr");//(std4);
}else{
channelOffset = parserTemp;
Serial.print("New offset: ");
Serial.print(channelOffset);
Serial.println();
}
parserTemp = 0;
readingChannelOffset = false;
}else if(isDigit(ch)){
parserTemp = 10*parserTemp + (ch-'0');
}else{
parserTemp = 0;
readingChannelOffset = false;
//Serial.println("Error while parsing a channel number.");
}
}else if(readingPT){
if(ch == 'p'){//End reached
parserTemp = 0;
i = 0; // reset array offset
readingPT = false;
time = micros() - time;
Serial.print("Parsing a PT took uS: ");
Serial.println(time, DEC);
//printCurrentDmxMessage();
}else if(ch == ','){
if(parserTemp>255 || parserTemp<0){
Serial.println("Warning: DMX-Range"); //(std3);
}else{
currentDmxMessage[i] = parserTemp;
DmxSimple.write(channelOffset+i, parserTemp);
}
parserTemp = 0;
i++;
}else if(isDigit(ch)){
parserTemp = 10*parserTemp + (ch-'0');
}else{// abort that attempt and reset everything.
parserTemp = 0;
i = 0;
readingPT = false;
//Serial.println("Error while parsing a PT.");
}
}else{// all other stuff
// Arduino sends some info stuff at startup -> need to disable those error messages.
// Serial.println("Received Gibberish. That one:");
// Serial.write(ch);
}
}
}
// Copyright (C) 2018, ETH Zurich, D-ITET
// Martin Reinhard remartin @ee.ethz.ch
//
// This file is part of D-FaLL-System.
//
// D-FaLL-System is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// D-FaLL-System is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
//
//
// ----------------------------------------------------------------------------------
// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
// D D F aaa L L S Y Y S T E MM MM
// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
// D D F a aa L L S Y S T E M M
// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
//
//
// DESCRIPTION:
// Handles the connection to the Arduino via a serial port.
// Based on previously existing SerialUnix file.
//
// ----------------------------------------------------------------------------------
#include "SerialLight.h" #include "SerialLight.h"
...@@ -10,11 +44,11 @@ SerialLight::SerialLight(){ ...@@ -10,11 +44,11 @@ SerialLight::SerialLight(){
void SerialLight::init(){ void SerialLight::init(){
// The Device could be located at ttyACM0 or ttyACM1. // The device could be located at ttyACM0 or ttyACM1.
// Need to determine which. Or just hard-code the path. // Need to determine which.
printf("Searching for the /dev file linked to the Arduino.\n"); printf("Searching for the /dev file linked to the Arduino.\n");
// the command: get device infos, suppress error stream, filter for arduino, count lines. // The command: get device infos, suppress error stream, filter for arduino, count lines.
char command[] = "udevadm info /dev/ttyACM0 2> /dev/null | grep arduino | wc -l > rubbishBin.txt"; char command[] = "udevadm info /dev/ttyACM0 2> /dev/null | grep arduino | wc -l > rubbishBin.txt";
char path[] = "/dev/ttyACM0"; char path[] = "/dev/ttyACM0";
...@@ -40,6 +74,7 @@ void SerialLight::init(){ ...@@ -40,6 +74,7 @@ void SerialLight::init(){
} }
} }
if(!foundDevice){printf("Can't find the Arduino. Is it plugged in? \n");return;} if(!foundDevice){printf("Can't find the Arduino. Is it plugged in? \n");return;}
startConnection(path); startConnection(path);
...@@ -52,7 +87,8 @@ int SerialLight::startConnection(const char* port){ ...@@ -52,7 +87,8 @@ int SerialLight::startConnection(const char* port){
_port = ""; _port = "";
_isConnected = false; _isConnected = false;
tty_fd = open(port, O_RDWR | O_NONBLOCK | O_NOCTTY | O_DSYNC); // both read and write, open nonblockingly tty_fd = open(port, O_RDWR | O_NONBLOCK | O_NOCTTY | O_DSYNC);
// both read and write, open nonblockingly
// some sync on file. Change O_SYNC to O_DSYNC? // some sync on file. Change O_SYNC to O_DSYNC?
if (tty_fd < 0) { if (tty_fd < 0) {
...@@ -86,8 +122,7 @@ int SerialLight::startConnection(const char* port){ ...@@ -86,8 +122,7 @@ int SerialLight::startConnection(const char* port){
cfsetospeed(&tio, SERIAL_LIGHT_BAUD_RATE); cfsetospeed(&tio, SERIAL_LIGHT_BAUD_RATE);
cfsetispeed(&tio, SERIAL_LIGHT_BAUD_RATE);