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 bed3f551 authored by remartin's avatar remartin
Browse files

Migrated files into existing structure.

parent f5bd257a
#pragma once
#include <errno.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h> //Timer
#include <fcntl.h> //Device settings
#include <termios.h> //For serial comm
#include <sys/ioctl.h> //For ioctl
#include <string.h> // needed for memset
#include <string>
#include <vector>
#include <ctime>
//SERIAL COMMUNICATION PARAMETERS
#define MAX_SERIAL_BUFFER_SIZE 180
//#define SERIAL_BAUD_RATE B921600
#define SERIAL_LIGHT_BAUD_RATE B9600
#define SERIAL_BYTE_SIZE 8
#define SERIAL_STOP_BITS 0
#define SERIAL_PARITY 0
#define SERIAL_COMMAND_END '\n'
#define SERIAL_READY_FLAG "-7\n"
#define SERIAL_FLAG_TIMEOUT 1000
#define K_WARNING "\x1B[33m"
#define K_ERROR "\x1B[31m"
#define K_SUCCESS "\x1B[32m"
#define K_NORMAL "\x1B[0m"
#define KBLU "\x1B[34m"
#define KMAG "\x1B[35m"
#define KCYN "\x1B[36m"
#define KWHT "\x1B[37m"
class SerialLight
{
public:
SerialLight();
void init();
int startConnection(const char* port);
void closeConnection();
bool isConnected(const char* port = "");
bool writeSerialLight(const char* buffer);
int manageSerial();
std::string readSerialLight();
private:
int tty_fd;
struct termios tio;
std::vector<std::string> _commands;
std::string _cur_command;
std::clock_t timer;
std::string _port;
bool _ready;
bool _isConnected;
};
\ No newline at end of file
#pragma once
#include<stdio.h>
#include "SerialLight.h"
#include <list>
#include <vector>
#include <string>
#include <stdio.h>
#include <cmath>
#include <sstream>
class spotlightManager{
public:
spotlightManager();
void someDefaultSettings();
void setPTmanually(bool blockOverhead);
// toggles some pan values.
void trivialDemo();
// changes the dmx values directly.
void lineOnWall(bool ptMode, int msInterval);
// uses actual coordinates
void testPTupdater();
private:
int posPT;
SerialLight sL;
const static char portLocation[];
unsigned char defaultChannel; // value between 0 and 255...
int messageLength;
unsigned char dmxValues[14];
double initialSpherical[3];
unsigned char initialPT[4];
double spotlightPos[3];
double targetPos[3];
double spherical[3]; // saving that is bad practice?
void updateSpherical();
void updatePTvalues();
void calibrateOrientation();
// construct strings that can be decoded by the arduino
std::string buildMessage(); // 'bad practice to import namespace std in header'
std::string buildPT();
std::string buildChannel();
};
<launch>
<node pkg="d_fall_pps" name="UWBL_main" output="screen" type="UWBL_main">
<param launchingViaRos=true/>
</node>
</launch>
#include "SerialLight.h"
SerialLight::SerialLight(){
_isConnected = false;
_port = "";
_ready = true;
_cur_command = "";
}
void SerialLight::init(){
startConnection("/dev/ttyACM0");
usleep(500000);
}
int SerialLight::startConnection(const char* port){
//printf("Attempting to connect to Serial port %s...\n", port);
_port = "";
_isConnected = false;
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?
if (tty_fd < 0) {
printf("%sError opening port: %s\n", K_ERROR, strerror(errno));
return -1;
}else{
printf("Opening port should've worked.\n");
}
/* Setting other Port Stuff */
tio.c_cflag &= ~PARENB; // no parity bits
tio.c_cflag &= ~CSTOPB; // not two stop bits
tio.c_cflag &= ~CSIZE;
tio.c_cflag &= ~CRTSCTS; // no flow control
tio.c_cflag |= CS8;
tio.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines
tio.c_cc[VMIN] = 1; // read doesn't block
tio.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tio.c_iflag = 0;
tio.c_oflag = 0;
//tio.c_cflag = CS8 | CREAD | CLOCAL | HUPCL; // char size 8, enable read, ignore modem control lines, lower timeout stuff
tio.c_lflag = 0;
//tio.c_cc[VMIN] = 0; //1
//tio.c_cc[VTIME] = 5;
/* Make raw */
cfmakeraw(&tio);
cfsetospeed(&tio, SERIAL_LIGHT_BAUD_RATE);
cfsetispeed(&tio, SERIAL_LIGHT_BAUD_RATE);
if (tcsetattr(tty_fd, TCSANOW, &tio) != 0) // "TCSANOW: change occurs immediately"
{
printf("%sError from tcsetattr: %s\n", K_ERROR, strerror(errno));
return -1;
}
printf("Set baud rate and stuff. \n");
_port = port;
_ready = true;
_isConnected = true;
//printf("%sConnected\n", K_WARNING);
//printf("%s", K_NORMAL);
return 1;
}
void SerialLight::closeConnection(){
if (_port != "" && _isConnected)
close(tty_fd); // Port release
}
bool SerialLight::isConnected(const char* port){
if (port != ""){
return (_isConnected && _port == port);
}
return _isConnected;
}
std::string SerialLight::readSerialLight(){
if (_commands.empty())
return "";
std::string ret = _commands.front();
_commands.erase(_commands.begin());
return ret;
}
bool SerialLight::writeSerialLight(const char* buffer)
{
if(!_isConnected){
printf("Can't write because we're not connected.\n");
return 0;
}
printf("Sending packet to Serial (%s)\n", buffer);
// zero day in code? (Just sending first 14 bytes -> pointer stuff? )
//for (unsigned int i = 0; i < strlen(buffer); i++){
// write(tty_fd, &buffer[i], 1);
//}
//printf("Length of that thing: %i \n", strlen(buffer));
write(tty_fd, buffer, strlen(buffer));
//usleep(1000);
//ioctl(tty_fd, TCFLSH, 1); // flush transmit
// bool timeout = true;
// _ready = false;
// timer = std::clock();
// while (!_ready && timeout) {
// timeout = (std::clock() - timer) / ((double)CLOCKS_PER_SEC / 1000) < SERIAL_FLAG_TIMEOUT;
// manageSerial();
// }
//
// if (!timeout)
// {
// printf("%s---- BASE STATION ACK TIMEOUT ----\n", K_ERROR); //We got a timeout
// return 0;
// }
return 1;
}
// just some ack stuff-> unnecessary?
int SerialLight::manageSerial()
{
if(!_isConnected)
return 0;
int bytes = 0;
ioctl(tty_fd, FIONREAD, &bytes);
//go through all bytes (read them separately)
if (bytes) { //There are bytes on the buffer
char value;
for (unsigned int i = 0; i < bytes; i++)
{
if (read(tty_fd, &value, 1) > 0) {
//Append character to current command
_cur_command.push_back(value);
//End of command -> push
if (value == SERIAL_COMMAND_END || _cur_command.size() >= MAX_SERIAL_BUFFER_SIZE) {
//check if it is the ready flag
if (_cur_command == SERIAL_READY_FLAG)
_ready = true;
else
_commands.push_back(_cur_command);
_cur_command = "";
}
}
else {
printf("%sFailed to read\n", K_ERROR);
return 0;
}
}
}
return 1;
}
\ No newline at end of file
//#include<iostream>
#include<stdio.h>
#include "SerialLight.h"
#include "spotlightManager.h"
using namespace std;
int main(){
for(int i = 0; i<10; i++){
printf("Globifisch\n");
//cout << "GlobiSchnaegg";
}
spotlightManager slM;
// printf("Testing the message builder: ");
// printf((slM.buildMessage()).c_str());
// printf("\n");
//slM.setPTmanually(false);
slM.testPTupdater();
//slM.someDefaultSettings();
//slM.setPTmanually();
// need ros stuff before -> put into an init() method?
//slM.calibrateOrientation();
//slM.trivialDemo();
//slM.lineOnWall(true, 50);
return 0;
}
rm uwb_light
g++ spotlightManager.cpp SerialLight.cpp UWBL_main.cpp -o uwb_light
./uwb_light -launchingViaRos true
//#include<iostream>
#include<stdio.h>
#include "spotlightManager.h"
#include "SerialLight.h"
#include <string>
#include <iostream>
#include <termios.h> //For serial comm
#define RAD2DEG 57.2957795131;
//#define PHI_DMX_PAN = //170/360, DMX steps per degree.
//#define THETA_DMX_TILT //110/90, DMX steps per degree.
using namespace std;
spotlightManager::spotlightManager(){
const char portLocation[] = "/dev/ttyACM0";
defaultChannel = 10; // value between 0 and 255...
messageLength = 14;
posPT = 3; // where the four PT values start.
for(int hans = 0; hans < 14; hans++){
dmxValues[hans] = 0;
}
sL.startConnection(portLocation);
if(sL.isConnected(portLocation)){
printf("Could connect to the serial stuff.\n");
}else{
printf("There's no connection.\n");
}
}
void spotlightManager::trivialDemo(){
string s1 = "m0,200,150,100,200,150,200,0,0,0,200,0,0,0,m";
string s2 = "m0,200,150,120,200,150,200,0,0,0,200,0,0,0,m";
string reset = "m0,0,0,0,0,0,0,0,0,0,0,0,0,0,m";
//int temp = (int) defaultChannel;
//char s[];
//sprintf(s, "c%ic", (int) defaultChannel);
//char chSetter[] = {'c', s.cstr(),'c'};
//printf(s);
//string defCh = "c10c";
//string defCh = "c";
//string temp = to_string((int) defaultChannel);
//defCh += temp;
//defCh += 'c';
// build string:
string defCh = buildChannel();
if(!sL.writeSerialLight(defCh.c_str())){printf("Couldn't send. \n");}
usleep(500000); // 0.5 seconds
usleep(500000); // 0.5 seconds
//while(sL.isConnected(portLocation)){
while(true){
if(!sL.writeSerialLight(s1.c_str())){printf("Couldn't send. \n");}
usleep(500000); // 0.5 seconds
usleep(500000); // 0.5 seconds
if(!sL.writeSerialLight(s2.c_str())){printf("Couldn't send. \n");}
usleep(500000); // 0.5 seconds
usleep(500000); // 0.5 seconds
}
}
void spotlightManager::someDefaultSettings(){
string defCh = "c10c";
if(!sL.writeSerialLight(defCh.c_str())){printf("Couldn't send. \n");}
dmxValues[1] = 100;
dmxValues[2] = 200;
//dmxValues[3] = 170;
//dmxValues[5] = 17;
dmxValues[10] = 255;
// need to apply that once:
sL.writeSerialLight((buildMessage()).c_str());
}
void spotlightManager::lineOnWall(bool ptMode, int msInterval){
int steps = 20;
int trav = 0;
string defCh = "c10c";
someDefaultSettings();
while(true){
if(!sL.writeSerialLight(defCh.c_str())){printf("Couldn't send. \n");}
usleep(500000); // 0.5 seconds
usleep(500000); // 0.5 seconds
for(trav; trav<steps; trav++){
dmxValues[3]++;
if(ptMode){
sL.writeSerialLight((buildPT()).c_str());
}else{
sL.writeSerialLight((buildMessage()).c_str());
}
usleep(msInterval*1000);
}
for(trav; trav>=0; trav--){
dmxValues[3]--;
if(ptMode){
sL.writeSerialLight((buildPT()).c_str());
}else{
sL.writeSerialLight((buildMessage()).c_str());
}
usleep(msInterval*1000);
}
}
}
void spotlightManager::setPTmanually(bool blockOverhead){
someDefaultSettings();
if(blockOverhead && dmxValues[posPT+2]>127){
dmxValues[posPT+2] = 127;
sL.writeSerialLight((buildPT()).c_str());
}
/*
ASCII:
w: 119
a: 97
s: 115
d: 100
k: 107
*/
// Disabling the buffer of the terminal.
// (thanks to Luke Leber : http://www.cplusplus.com/forum/general/29137/)
struct termios t;
tcgetattr(STDIN_FILENO, &t); //get the current terminal I/O structure
t.c_lflag &= ~ICANON; // Manipulate the flag bits to do what you want it to do
tcsetattr(STDIN_FILENO, TCSANOW, &t); // Apply the new settings
char key;
bool coarse = true;
printf("Set Pan/Tilt values with the WASD keys. Konfirm with k. \n");
while(coarse){
//key = getch();
cin >> key;
//printf("Char %c picked. \n", key);
switch (key){
case 97: dmxValues[posPT ]++; sL.writeSerialLight((buildPT()).c_str()); break;
case 100: dmxValues[posPT ]--; sL.writeSerialLight((buildPT()).c_str()); break;
case 119:
if(blockOverhead && (dmxValues[posPT+2]>=127)){
printf("Overhead disabled. Can't go any further in that direction. \n");
}else{
dmxValues[posPT+2]++;
sL.writeSerialLight((buildPT()).c_str());
}
break;
case 115:
if(blockOverhead && (dmxValues[posPT+2]==0)){
// nutthing
}else{
dmxValues[posPT+2]--;
sL.writeSerialLight((buildPT()).c_str());
}
break;
case 107: coarse = false; break;
}
}
bool fine = true;
printf("Set the fine Pan/Tilt values with the WASD keys. Konfirm with k. \n");
while(fine){
cin >> key;
//printf("Char %c picked. \n", key);
switch (key){
case 119: dmxValues[posPT+1]++; sL.writeSerialLight((buildPT()).c_str()); break;
case 97: dmxValues[posPT+3]++; sL.writeSerialLight((buildPT()).c_str()); break;
case 115: dmxValues[posPT+1]--; sL.writeSerialLight((buildPT()).c_str()); break;
case 100: dmxValues[posPT+3]--; sL.writeSerialLight((buildPT()).c_str()); break;
case 107: fine = false; break;
}
}
// Restore the terminal settings:
t.c_lflag |= ICANON;
tcsetattr(STDIN_FILENO, TCSANOW, &t);
}
void spotlightManager::updateSpherical(){
double dx = targetPos[0]-spotlightPos[0];
double dy = targetPos[1]-spotlightPos[1];
double dz = targetPos[2]-spotlightPos[2];
//printf("The differences are: dx is %f, dy is %f, dz is %f. \n", dx, dy, dz);
double r = sqrt(dx*dx + dy*dy + dz*dz);
double phi = atan(dy/dx)* RAD2DEG;
if(phi<0){phi+=360.0; };
double theta = acos(dz/r) * RAD2DEG;
//printf("The angles: phi is %f°, theta is %f°. \n", phi, theta);
// save values (debugging/calibrateOrientation method)
spherical[0] = r;
spherical[1] = phi;
spherical[2] = theta;
}
void spotlightManager::updatePTvalues(){
updateSpherical();
double phiDmxPan = 170/(double)360; // save as constant somewhere...
double thetaDmxTilt = 110/(double)90;
double deviation;
double unicorn;
// possibly add minus sign if headsup is fixed here? Planar is required?
// (not too big a constraint, no IMU to get stuff as on the crazyflie
// ------> would need to proejct the angles somehow. )
// type conversion works properly that way.
//Pan:
dmxValues[posPT] = initialPT[0];
deviation = (spherical[1]-initialSpherical[1])*phiDmxPan; // stays within 0 and 170
//printf("Pan deviation: %f \n", deviation);
// direction depends on sign of the deviation... it's complicated.
if(deviation>0){
unicorn = floor(deviation);
}else{
unicorn = ceil(deviation);
}
dmxValues[posPT] += unicorn;
unicorn = round((deviation-unicorn)*255); // try to spread the rest
dmxValues[posPT+1] = unicorn;
//Tilt:
dmxValues[posPT+2] = initialPT[2];
deviation = (initialSpherical[2]-spherical[2])*thetaDmxTilt;
//printf("Tilt deviation: %f \n", deviation);
double hypo = (double) initialPT[2] + deviation;
if(hypo <= 0){ // constraint of the spotlight
dmxValues[posPT+2] = 0;
dmxValues[posPT+3] = 0;
printf("WARNING: Can't go any lower. \n");
}else if(hypo>127){// "Flyover" case, described in report.