Commit ef5fa643 authored by ucart's avatar ucart

Able to determine distance to one CrazyFlie

parent 6f3652cb
#include "Bridge.h"
#include <vrpn_Tracker.h>
#include "structures.h"
#include "quadcopterData.h"
......@@ -52,43 +51,36 @@ bool Bridge::getInterpolatedPosition(lateralPosition *pos, int loc) {
return( retVal );
}
/**
* Calculates the distance between the Bridge and each of the active Crazyflie
*
* @param pos Pointer to a lateral position struct
* @param dis Distance between Bridge and Quadcopter1
* @return True if a location is valid, false if no location is able to found
*/
bool Bridge::getDistanceToQuads(lateralPosition *pos, targetDistance *disToQuad1/*,
targetDistance *disToQuad2, targetDistance *disToQuad3, targetDistance *disToQuad4*/) {
bool Bridge::getTargetDistance(lateralPosition *pos, targetDistance *disToQuad1
/*, targetDistance *disToQuad2, targetDistance *disToQuad3, targetDistance *disToQuad4*/) {
bool retVal = false;
if (this->endpoint1Valid){
coordinateSystem coor = crazyflie_info[1].cflieCopter->getCoordinateSystem();
disToQuad1->x = this->endpoint1.x - crazyflie_info[1].cflieCopter->getPositionX( coor );
disToQuad1->y = this->endpoint1.y - crazyflie_info[1].cflieCopter->getPositionY( coor );
disToQuad1->z = this->endpoint1.z - crazyflie_info[1].cflieCopter->getPositionZ( coor );
/*
if(NUM_QUADS>1){
coordinateSystem coor = crazyflie_info[2].cflieCopter->getCoordinateSystem();
disToQuad2->x = this->endpoint1.x - crazyflie_info[2].cflieCopter->getPositionX( coor );
disToQuad2->y = this->endpoint1.y - crazyflie_info[2].cflieCopter->getPositionY( coor );
disToQuad2->z = this->endpoint1.z - crazyflie_info[2].cflieCopter->getPositionZ( coor );
} if(NUM_QUADS>2){
coordinateSystem coor = crazyflie_info[3].cflieCopter->getCoordinateSystem();
disToQuad3->x = this->endpoint1.x - crazyflie_info[3].cflieCopter->getPositionX( coor );
disToQuad3->y = this->endpoint1.y - crazyflie_info[3].cflieCopter->getPositionY( coor );
disToQuad3->z = this->endpoint1.z - crazyflie_info[3].cflieCopter->getPositionZ( coor );
} if(NUM_QUADS>3){
coordinateSystem coor = crazyflie_info[4].cflieCopter->getCoordinateSystem();
disToQuad4->x = this->endpoint1.x - crazyflie_info[4].cflieCopter->getPositionX( coor );
disToQuad4->y = this->endpoint1.y - crazyflie_info[4].cflieCopter->getPositionY( coor );
disToQuad4->z = this->endpoint1.z - crazyflie_info[4].cflieCopter->getPositionZ( coor );
if(this->endpoint1Valid){
if(NUM_QUADS){
coordinateSystem coor = crazyflie_info[0].cflieCopter->getCoordinateSystem();
this->disToQuad1.x = this->endpoint1.x - crazyflie_info[0].cflieCopter->getPositionX( coor );
this->disToQuad1.y = this->endpoint1.y - crazyflie_info[0].cflieCopter->getPositionY( coor );
this->disToQuad1.z = this->endpoint1.z - crazyflie_info[0].cflieCopter->getPositionZ( coor );
}
/* if(NUM_QUADS > 1) {
coordinateSystem coor = crazyflie_info[1].cflieCopter->getCoordinateSystem();
disToQuad2->x = this->endpoint1.x - crazyflie_info[2].cflieCopter->getPositionX( coor );
disToQuad2->y = this->endpoint1.y - crazyflie_info[2].cflieCopter->getPositionY( coor );
disToQuad2->z = this->endpoint1.z - crazyflie_info[2].cflieCopter->getPositionZ( coor );
} if(NUM_QUADS > 2) {
coordinateSystem coor = crazyflie_info[2].cflieCopter->getCoordinateSystem();
disToQuad3->x = this->endpoint1.x - crazyflie_info[3].cflieCopter->getPositionX( coor );
disToQuad3->y = this->endpoint1.y - crazyflie_info[3].cflieCopter->getPositionY( coor );
disToQuad3->z = this->endpoint1.z - crazyflie_info[3].cflieCopter->getPositionZ( coor );
} if(NUM_QUADS > 3) {
coordinateSystem coor = crazyflie_info[3].cflieCopter->getCoordinateSystem();
disToQuad4->x = this->endpoint1.x - crazyflie_info[4].cflieCopter->getPositionX( coor );
disToQuad4->y = this->endpoint1.y - crazyflie_info[4].cflieCopter->getPositionY( coor );
disToQuad4->z = this->endpoint1.z - crazyflie_info[4].cflieCopter->getPositionZ( coor );
}
*/
retVal = true;
retVal = true;
}
return( retVal );
return retVal;
}
/**
......
......@@ -511,7 +511,7 @@ void VRPN_CALLBACK handle_hand(void*, const vrpn_TRACKERCB t) {
cflieCopter->setSendSetpoints(true);
/*
//-
// Safety check the crazyflie, if Crazyflie is vertical kill motors, Crash Imminent
if (cflieCopter->roll() > 90) {
thrustControlOutput = 10000;
......@@ -610,4 +610,4 @@ void VRPN_CALLBACK handle_hand(void*, const vrpn_TRACKERCB t) {
controllerData->vrpnPacketQueueLen = 0;
return;
}
}*/
\ No newline at end of file
}*/
......@@ -42,12 +42,8 @@ void* UIThread(void *threadID) {
// Get a character from the terminal
tempbuf[counter] = getchar();
// If the character is a newline, the command is entered
if(tempbuf[counter] == '`'){
counter = 0;
}
else if (tempbuf[counter] == '\n') {
if (tempbuf[counter] == '\n') {
// Add a null terminator to the buffer
tempbuf[counter] = 0;
......@@ -81,25 +77,6 @@ void* UIThread(void *threadID) {
command.type = tempbuf[1];
command.payload = atof(&tempbuf[2]);
//Make sure commands are reasonable
//Verify command X or Y stays within +/-1
if((command.type == 'x') || (command.type == 'y')){
if(command.payload > 1){
command.payload = 1;
} else if(command.payload < -1){
command.payload = -1;
}
} else if(command.type == 'z'){
if(command.payload > 0){
command.payload = -command.payload;
}
if(command.payload > -0.3){
command.payload = -0.3;
} else if(command.payload < -1.9){
command.payload = -1.9;
}
}
// Pass the command to the Crazyflie
if (quadcopterNumber == 0) {
// This command is for every quadcopter
......
......@@ -31,10 +31,12 @@ void* displayData(void* id) {
cout << "Target 1 X: " << bridge->endpoint1.x << "\tY: " << bridge->endpoint1.y << "\tZ: " << bridge->endpoint1.z;
cout << endl;
lateralPosition bridgePos;
targetDistance disToQuad;
if ( bridge->getDistanceToQuads(&bridgePos, &disToQuad) ) { // The function worked
cout << "Dis Q21T1 X: " << bridge->disToQuad1.x << "\tY: " << bridge->disToQuad1.y << "\tZ: " << bridge->disToQuad1.z;
}
targetDistance pos;
if ( bridge->getInterpolatedPosition(&bridgePos, 0) ){
bridge->getTargetDistance(&bridgePos, &pos);
cout << "Dis Q21T1 X: " << bridge->disToQuad1.x << "\tY: " << bridge->disToQuad1.y << "\tZ: " << bridge->disToQuad1.z;
cout << endl;
}
}
if(bridge->endpoint2Valid){
cout << "Target 2 X: " << bridge->endpoint2.x << "\tY: " << bridge->endpoint2.y << "\tZ: " << bridge->endpoint2.z;
......@@ -296,7 +298,7 @@ void* displayData(void* id) {
return(NULL);
}
usleep(1000);
usleep(10000);
}
return(NULL);
......
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