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

.

parent 1d7bbc56
......@@ -52,6 +52,7 @@
#include <stdio.h>
#include <string>
#include <vector>
#include <chrono>
#include <ros/package.h>
#include <rosbag/bag.h>
......@@ -65,12 +66,14 @@ namespace d_fall_pps{
ros::ServiceClient uwbManager;
int spotlightAnchor = 5; // Anchor placed next to the spotlight
bool placedSpotlight = false;
//bool foundTarget = false;
bool finishedCalibration = false;
int frequencyDivision = 5; // 67 Hz by UWB.
int clockState = 1;
double maxFrequency = 10.0; // 100 Hz by UWBData (rostopic info /8/UWBDataPublisher/UWBData).
std::chrono::high_resolution_clock::time_point lastSent;
long elapsedTime;
// Received data from the target/crazyflie
void updateTargetPositionCallback(const CrazyflieData &data);
......
......@@ -113,26 +113,27 @@ void serialEvent(){
if(!readingMessage && ch == 'm'){
readingMessage = true;
i = 0;
time = micros();
//time = micros();
}else if(!readingChannelOffset && ch == 'c'){
readingChannelOffset = true;
time = micros();
//time = micros();
}else if(!readingPT && ch == 'p'){
readingPT = true;
i = 3;
time = micros();
//time = micros();
}else if(!readingColour && ch == 'f'){
readingColour = true;
i = 9;
time = micros();
//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);
//time = micros() - time;
//Serial.print("Parsing took uS: ");
//Serial.println(time, DEC);
Serial.println("Updated via message.");
}else if(ch == ','){
if(parserTemp>255 || parserTemp<0){
Serial.println("Warning: DMX-Range"); //(std3);
......@@ -159,9 +160,9 @@ void serialEvent(){
Serial.print("New offset: ");
Serial.print(channelOffset);
Serial.println();
time = micros() - time;
Serial.print("Parsing a channel nr. took uS: ");
Serial.println(time, DEC);
//time = micros() - time;
//Serial.print("Parsing a channel nr. took uS: ");
//Serial.println(time, DEC);
}
parserTemp = 0;
readingChannelOffset = false;
......@@ -178,9 +179,10 @@ void serialEvent(){
parserTemp = 0;
i = 0; // reset array offset
readingPT = false;
time = micros() - time;
Serial.print("Parsing a PT took uS: ");
Serial.println(time, DEC);
//time = micros() - time;
//Serial.print("Parsing a PT took uS: ");
//Serial.println(time, DEC);
Serial.println("Updated PT values. ");
//printCurrentDmxMessage();
}else if(ch == ','){
if(parserTemp>255 || parserTemp<0){
......@@ -204,9 +206,10 @@ void serialEvent(){
parserTemp = 0;
i = 0; // reset array offset
readingColour = false;
time = micros() - time;
Serial.print("Parsing a Colour took uS: ");
Serial.println(time, DEC);
//time = micros() - time;
//Serial.print("Parsing a Colour took uS: ");
//Serial.println(time, DEC);
Serial.println("Updated colours. ");
}else if(ch == ','){
if(parserTemp>255 || parserTemp<0){
Serial.println("Warning: DMX-Range"); //(std3);
......
......@@ -66,21 +66,31 @@ int main(int argc, char* argv[]){
// Received data from the target/crazyflie
void d_fall_pps::updateTargetPositionCallback(const CrazyflieData &data){
if(finishedCalibration){
if(clockState >= frequencyDivision){
elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - lastSent).count();
if(elapsedTime > (1000/maxFrequency)){
//ROS_INFO_STREAM("Applying new coordinates: %f,%f,%f. ", data.x, data.y, data.z);
slM.setTargetPosition(data.x, data.y, data.z);
slM.setTargetPosition(data.x, data.y, data.z); // invert z position
slM.updatePTvalues();
slM.fancyColourFunction(data.yaw);
slM.fancyColourFunction(data.yaw);
printf("\n");
//foundTarget = true;
clockState = 1;
}else{
clockState++;
lastSent = std::chrono::high_resolution_clock::now();
}
}else if(placedSpotlight){
slM.setTargetPosition(data.x, data.y, data.z); // invert z position
slM.calibrateOrientation();
lastSent = std::chrono::high_resolution_clock::now();
finishedCalibration = true;
}else{
placeSpotlight();
}
......@@ -99,7 +109,8 @@ void d_fall_pps::placeSpotlight(){
UWBAnchorArray anchors = a.response.anchorArray;
// only need one!
UWBAnchor sl = anchors.data[spotlightAnchor-1];
slM.setSpotlightPosition(sl.x, sl.y, sl.z);
slM.setSpotlightPosition(sl.x/10.0, sl.y/10.0, sl.z/10.0); // bloody decimeters!
placedSpotlight = true;
}else{
ROS_ERROR("[UWBSpotlight_main] Could not grab the positions of all the ankers. ");
......
......@@ -84,14 +84,16 @@ void spotlightManager::someDefaultSettings(){
void spotlightManager::setTargetPosition(double x, double y, double z){
printf("Received new Target position: %f,%f,%f. \n", x,y,z);
//printf("Received new Target position: %f,%f,%f. \n", x,y,z);
//printf("Momemorised spotlight position is: %f,%f,%f. \n", spotlightPos[0], spotlightPos[1], spotlightPos[2]);
printf("Z component currently is: %f \n", z);
targetPos[0] = x;
targetPos[1] = y;
targetPos[2] = z;
}
void spotlightManager::setSpotlightPosition(double x, double y, double z){
printf("Received new Spotlight position: %f,%f,%f. \n", x,y,z);
//printf("Received potlight position: %f,%f,%f. \n", x,y,z);
spotlightPos[0] = x;
spotlightPos[1] = y;
spotlightPos[2] = z;
......@@ -107,15 +109,19 @@ void spotlightManager::updateSpherical(){
//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 phi = atan2(dy, dx)* RAD2DEG; // use atan2 to avoid messing up quadrants
//if(phi<0){phi+=360.0; };
double theta = acos(dz/r) * RAD2DEG;
//printf("The angles: phi is %f°, theta is %f°. \n", phi, theta);
//printf("The saved angles are: phi %f, theta %f.\n", initialSpherical[1], initialSpherical[2]);
printf("The current angles are: phi is %f°, theta is %f°. \n", phi, theta);
printf("Saved theta is %f, current one is %f. \n", initialSpherical[2], theta);
spherical[0] = r;
spherical[1] = phi;
spherical[2] = theta;
}
......@@ -124,11 +130,12 @@ void spotlightManager::updatePTvalues(){
updateSpherical();
double phiDmxPan = 170/(double)360; // save as constant somewhere...
double thetaDmxTilt = 110/(double)90;
double phiDmxPan = 170/360.0; // save as constant somewhere...
double thetaDmxTilt = 110/90.0;
double deviation;
double unicorn;
double hypo;
// 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
......@@ -136,25 +143,32 @@ void spotlightManager::updatePTvalues(){
// 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.
// printf("Pan deviation (DMX): %f \n", deviation);
// overlap situation
hypo = initialPT[0] + deviation;
if(hypo < 0){
deviation += 170;
}else if(hypo > 255){
deviation -= 170;
}
// rounding direction depends on sign of the deviation... it's complicated.
if(deviation>0){
unicorn = floor(deviation);
}else{
unicorn = ceil(deviation);
}
dmxValues[posPT] += unicorn;
dmxValues[posPT] = initialPT[0] + 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);
printf("Tilt deviation (DMX): %f \n", deviation);
double hypo = (double) initialPT[2] + deviation;
hypo = (double) initialPT[2] + deviation;
if(hypo <= 0){ // constraint of the spotlight
dmxValues[posPT+2] = 0;
dmxValues[posPT+3] = 0;
......@@ -169,14 +183,14 @@ void spotlightManager::updatePTvalues(){
}else{
unicorn = ceil(deviation);
}
dmxValues[posPT+2] += unicorn;
dmxValues[posPT+2] = initialPT[2] + unicorn;
unicorn = round((deviation-unicorn)*255); // try to spread the rest
dmxValues[posPT+3] = unicorn;
}
printf("Saved tilt DMX is %i, new one %i.\n", initialPT[2], dmxValues[posPT+2]);
// send already. Buffering?
sL.writeSerialLight((buildPT()).c_str());
printf("PT values updated as follows: %s \n", buildPT().c_str());
}
// WASD key control
......@@ -265,8 +279,8 @@ void spotlightManager::calibrateOrientation(){
int p = (int) dmxValues[posPT]; // candidates.
int t = (int) dmxValues[posPT+2];
bool applyChanges = false;
bool applyChanges = false;
// avoid the Tilt overlap case. // No, would have to introduce overlap mode everywhere.
//if(t>127){
// initialPT[2] = 255-t;
......@@ -303,13 +317,18 @@ void spotlightManager::calibrateOrientation(){
// Reacts to yaw changes of the target.
void spotlightManager::fancyColourFunction(double yaw){
// radians?
dmxValues[9] = (( (int) yaw % 360)/360)*255; // Red
dmxValues[10] = (( (int) (yaw+120) % 360)/360)*255; // Green
dmxValues[11] = (( (int) (yaw+240) % 360)/360)*255; // Blue
// radians? yes...
//printf("Received new Yaw %f. \n", yaw);
int yaw_i = yaw*RAD2DEG;
dmxValues[9] = (int) ( ((yaw_i ) % 360)*255/360.0 ); // Red
dmxValues[10] = (int) ( ((yaw_i+120) % 360)*255/360.0 ); // Green
dmxValues[11] = (int) ( ((yaw_i+240) % 360)*255/360.0 ); // Blue
// White: 12
sL.writeSerialLight((buildColour()).c_str());
// need to adapt Bitrate?
//sendMessage... Or joust colour
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment