Commit 6f3652cb authored by rbuckley's avatar rbuckley

Changed distance calc to single quad

Added limits to x,y,z setpoints
parent 1b0887b4
......@@ -59,13 +59,32 @@ bool Bridge::getInterpolatedPosition(lateralPosition *pos, int loc) {
* @param dis Distance between Bridge and Quadcopter1
* @return True if a location is valid, false if no location is able to found
*/
bool getDistanceToQuad1(lateralPosition *pos, targetDistance *disToQuad1) {
bool Bridge::getDistanceToQuads(lateralPosition *pos, targetDistance *disToQuad1/*,
targetDistance *disToQuad2, targetDistance *disToQuad3, targetDistance *disToQuad4*/) {
bool retVal = false;
if (bridge->endpoint1Valid){
if (this->endpoint1Valid){
coordinateSystem coor = crazyflie_info[1].cflieCopter->getCoordinateSystem();
disToQuad1->x = bridge->endpoint1.x - crazyflie_info[1].cflieCopter->getPositionX( coor );
disToQuad1->y = bridge->endpoint1.y - crazyflie_info[1].cflieCopter->getPositionY( coor );
disToQuad1->z = bridge->endpoint1.z - crazyflie_info[1].cflieCopter->getPositionZ( coor );
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 );
}
*/
retVal = true;
}
return( retVal );
......
......@@ -42,8 +42,12 @@ 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] == '\n') {
if(tempbuf[counter] == '`'){
counter = 0;
}
else if (tempbuf[counter] == '\n') {
// Add a null terminator to the buffer
tempbuf[counter] = 0;
......@@ -77,6 +81,25 @@ 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
......
......@@ -30,8 +30,12 @@ void* displayData(void* id) {
if(bridge->endpoint1Valid){
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;
}
}
if(bridge->endpoint2Valid){
cout << "Target 2 X: " << bridge->endpoint2.x << "\tY: " << bridge->endpoint2.y << "\tZ: " << bridge->endpoint2.z;
cout << endl;
......
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