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

Added more fancyColourFunction.

parent 7540814f
......@@ -38,6 +38,8 @@ public:
void setPositions(double spotlightPosition[3], double targetPosition[3]);
void calibrateOrientation();
void fancyColourFunction(double yaw);
private:
int posPT;
......
......@@ -130,12 +130,23 @@ std::string SerialLight::readSerialLight(){
return ret;
}
bool SerialLight::writeSerialLight(const char* buffer)
{
bool SerialLight::writeSerialLight(const char* buffer){
if(!_isConnected){
printf("Can't write because we're not connected.\n");
return 0;
}
// Just drop everything being received to avoid an input buffer overflow.
// Start in another terminal if logging is required:
// echo 'A new log: ' > eavesDroppingLog.txt
// cat -v < /dev/ttyACM0 |& tee -a eavesDroppingLog.txt
tcflush(tty_fd, TCIFLUSH);
//int tcflush(int fd, int queue_selector);
//tcflush(ttyDevice, TCIOFLUSH); //TCIFLUSH, TCOFLUSH -> launch eavesdropper otherwise.
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++){
......
......@@ -37,7 +37,10 @@ using namespace d_fall_pps;
spotlightManager slM;
int spotlightAnchor = 4;
bool placedSpotlight = false;
bool foundTarget = false;
//bool foundTarget = false;
bool finishedCalibration = false;
int frequencyDivision = 150; // 67 Hz by UWB.
int clockState = 1;
ros::ServiceClient uwbManager;
......@@ -66,21 +69,17 @@ int main(int argc, char* argv[]){
ROS_INFO_STREAM("[UWBSpotlight_main] started. ");
// Just for testing!
slM.calibrateOrientation();
ros::spin(); // spinning 'till it goes out -> need initialisation procedure on first call?
ros::spin();
ROS_WARN("[UWBSpotlight_main] stopped. ");
return 0;
}
void d_fall_pps::updateTargetPositionCallback(const CrazyflieData &data){
/*
CrazyflieData thingy:
string crazyflieName
string crazyflieNameget
float64 x
float64 y
float64 z
......@@ -92,9 +91,24 @@ void d_fall_pps::updateTargetPositionCallback(const CrazyflieData &data){
*/
// convert from float64?
slM.setTargetPosition(data.x, data.y, data.z);
slM.updatePTvalues();
foundTarget = true;
if(finishedCalibration){
if(clockState >= frequencyDivision){
//ROS_INFO_STREAM("Applying new coordinates: %f,%f,%f. ", data.x, data.y, data.z);
slM.setTargetPosition(data.x, data.y, data.z);
slM.updatePTvalues();
slM.fancyColourFunction(data.yaw);
//foundTarget = true;
clockState = 1;
}else{
clockState++;
}
}else if(placedSpotlight){
slM.calibrateOrientation();
finishedCalibration = true;
}else{
placeSpotlight();
}
}
......@@ -112,9 +126,6 @@ void d_fall_pps::placeSpotlight(){
UWBAnchor sl = anchors.data[spotlightAnchor-1];
slM.setSpotlightPosition(sl.x, sl.y, sl.z);
placedSpotlight = true;
// Just for testing!
slM.calibrateOrientation();
}else{
ROS_ERROR("[UWBSpotlight_main] Could not grab the positions of all the ankers. ");
}
......
......@@ -226,12 +226,14 @@ void spotlightManager::updateSpherical(){
}
void spotlightManager::setTargetPosition(double x, double y, double z){
printf("Received new Target position: %f,%f,%f. \n", x,y,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);
spotlightPos[0] = x;
spotlightPos[1] = y;
spotlightPos[2] = z;
......@@ -291,6 +293,8 @@ void spotlightManager::updatePTvalues(){
dmxValues[posPT+3] = unicorn;
}
// send already. Buffering?
sL.writeSerialLight((buildPT()).c_str());
printf("PT values updated as follows: %s \n", buildPT().c_str());
}
......@@ -423,4 +427,16 @@ string spotlightManager::buildChannel(){
stringstream ss;
ss << 'c' << (int) defaultChannel << 'c';
return ss.str();
}
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
// White: 11
sL.writeSerialLight((buildMessage()).c_str());
// need to adapt Bitrate?
//sendMessage... Or joust colour
}
\ No newline at end of file
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