예제 #1
0
float FishPose::calculateProbabilityOfIdentity(const cv::RotatedRect &second, float &distance, float angleImportance)
{
    distance = static_cast<float>(sqrt(double((_last_known_position.center.x - second.center.x) *
                                               (_last_known_position.center.x - second.center.x) +
                                               (_last_known_position.center.y - second.center.y) *
                                               (_last_known_position.center.y - second.center.y))));

    // we are not sure about the direction of the angle, so just use the closer one
    const float absAngleDifference = std::min(
            std::abs(angleDifference(_angle, static_cast<float>(second.angle * CV_PI / 180.0f))),
            std::abs(angleDifference(static_cast<float>(_angle + CV_PI), static_cast<float>(second.angle * CV_PI / 180.0f))));

    auto normalDistributionPdf = [](double sigma, double distance2)
    {
        return std::exp(-(distance2 * distance2) / (2.0 * sigma * sigma));
    };

    // 0.5cm per millisecond sounds good as a ~66% estimate - that is about 18 km/h
    // the factors are a hand-optimized scaling of the distribution's dropoff
    const float distanceSigma = FishPose::_averageSpeedSigma;
    const double distanceIdentity = normalDistributionPdf(distanceSigma, distance);

    const double angleSigma = 10.0 * CV_PI / 2.0 * 0.05;
//    std::cout << "angleSigma: \t" << angleSigma << std::endl;
    const double angleIdentity = normalDistributionPdf(angleSigma, absAngleDifference);
//    std::cout << "distance: \t" << int(100.0f * distanceIdentity) << "\t\t\t angle: " << int(100.0f * angleIdentity) << std::endl;
//    std::cout << "\t\t^- " << distance << "\t\t\t^-" << (180.0 * absAngleDifference / CV_PI) << "(" << _angle << " - " << second.angle << ")" << std::endl;
    return static_cast<float>((1.0f - angleImportance) * distanceIdentity + angleImportance * angleIdentity);
}
예제 #2
0
rotationResult Rotation::computeRotation(cv::Mat& currentFrame) {
    bool success;
    cv::Point2d centroid, skew;
    double orientation, lambda1, lambda2, eccentricity, compactness;

    // Compute orientation
    boost::tie(success, centroid, skew,	orientation, lambda1, lambda2, eccentricity, compactness) = rotationFromMoments(currentFrame);

    // Track rotation
    if(success) {
        if(m_validPreviousAngle) {
            // Track slip angle between current orientation [0, 180) and previous angle [0, 360)
            double currentAngle1 = orientation;
            double currentAngle2 = 180.0 + orientation;

            double angleDiff;
            double angleDiff1 = angleDifference(m_previousAngle, currentAngle1);
            double angleDiff2 = angleDifference(m_previousAngle, currentAngle2);

            if( fabs(angleDiff1) < fabs(angleDiff2) ) {
                m_currentAngle = currentAngle1;
                angleDiff = angleDiff1;
            } else {
                m_currentAngle = currentAngle2;
                angleDiff = angleDiff2;
            }

            // Discontinuity 360 -> 0
            if (m_currentAngle < m_previousAngle && angleDiff > 0.0) {
                m_n += 1;
            }

            // Discontinuity 0 -> 360
            if (m_currentAngle > m_previousAngle && angleDiff < 0.0) {
                m_n -= 1;
            }

            // Difference to reference angle
            if(m_validReferenceAngle) {
                m_slipAngleReference = m_n*360.0 + m_currentAngle + angleDifference(m_referenceAngle, 0.0);
            }

            // Difference to previous angle
            m_slipAnglePrevious = angleDiff;

        } else { // Computation of last frame's orientation failed, hence no slip
            m_slipAnglePrevious = 0.0;
        }

        m_previousAngle = m_currentAngle;
        m_validPreviousAngle = true;

    } else { // Principal axis method failed
        m_validPreviousAngle = false;
        m_slipAnglePrevious = 0.0;
    }

    return boost::tie(success, m_slipAnglePrevious, m_slipAngleReference, orientation, centroid, skew, lambda1, lambda2, eccentricity, compactness);
}
void faceForward(int forwards){
	if(angleDifference(currentDirection(), forwards) > 0){//too far to the right
		motor[left] = -SPEED; //turn left
	} else {
		motor[right] = -SPEED; //turn right
	}
	while(abs(angleDifference(currentDirection(), forwards)) != 0){
		wait1Msec(1);
	}
}
void turnToAngle(int angle, int undershoot) {
  if( angleDifference(angle, currentDirection()) < 0 ) {
      direction = LEFT;
  } else {
    direction = RIGHT;
  }
  spinInDirection();

	while(abs(angleDifference(currentDirection(), angle)) > undershoot){
		wait1Msec(1);
	}
	motor[left] = 0;
	motor[right] = 0;
}
예제 #5
0
파일: main.c 프로젝트: HideTheMess/Trinidad
int main(void) {
	//adc_init();
	i2c_init();
	
	//initialize timer
	TCCR1A = 0b00000000; // normal counter (0 to TOP = 0xFFFF)
	TCCR1B = 0b00000011; // prescaler to 64
	
	//initialize MiniURT library
	initialize_baud(19200);
	
	while(1) {
		static int16_t oldCompass;
		compass = cmps03_direction();
		uint16_t time = TCNT1;
		if(time >= MIN_VELOCITY_TIME) {
			TCNT1 = 0;
			angularVelocity = angleDifference(compass, oldCompass)/(((double)time)/(F_CPU/64))/10.0;
			oldCompass = compass;
		}
		
		//accx = adc_read(0);
		//accy = adc_read(1);
		//accz = adc_read(2);
		
		if(polled())
			onPoll();
	}
}
bool checkIsLeaf(void) {
	int direction = currentDirection();
	while(!isDark()) {
		if( abs(angleDifference(currentDirection(), direction)) > 170) {
			return true;
		}
	}
	return false;
}
void FlightController::turnTowardsAngle(double target, double hoverTime) {
    if (hoverTime == 0)
        hoverTime = 5;
    double orientation = navData->getRotation();
    double difference = angleDifference(orientation, target);
    int direction = angleDirection(orientation, target);

    ROS_INFO("Orientation: %3.1f\ttarget: %3.1f\tdifference: %3.1f\tdirection: %d\t", orientation,target, difference, direction  );
    int last_ts = (int) (ros::Time::now().toNSec() / 1000000);
    int time_counter = 0;


    //rotateDrone(direction * 1.0);
    int debug_counter = 0;
    while (true) {
        int time = (int) (ros::Time::now().toNSec() / 1000000);
        time_counter += (time - last_ts);
        last_ts = time;
        debug_counter++;
        if(debug_counter > 50) {
            ROS_INFO("LOOP orientation: %3.1f \ttarget: %3.0f \tdifference: %3.1f \tdirection: %d \ttime_counter: %d", orientation, target, difference, direction, time_counter);
            debug_counter = 0;
        }
        if (time_counter > 150) {
            hoverDuration(1);
            time_counter = 0;
            last_ts = (int) (ros::Time::now().toNSec() / 1000000);
        }

        if( difference < (time_counter*time_counter)/3000+4){
            break;
        }
        orientation = navData->getRotation();
        difference = angleDifference(orientation, target);
        direction = angleDirection(orientation, target);

        rotateDrone(direction * 1.0);
    }

    ROS_INFO("VICTORY! orientation: %6.1f\ttarget: %6.1f", orientation, target);
    hoverDuration(hoverTime);
    orientation = navData->getRotation();
    ROS_INFO("VICTORY AFTERMATH! orientation: %6.1f\ttarget: %6.1f", orientation, target);
}
/**
 * Turns around until black is found
 */
void turnToLine(void) {
  spinInDirection();

	while(!isDark()) {
		abortTimeslice();
	}
	int startDir = currentDirection();

	while(isDark()) {
	  abortTimeslice();
  }
	motor[left] = 0;
	motor[right] = 0;
	int centre = startDir + (angleDifference(currentDirection(), startDir)/2);
	nxtDisplayCenteredTextLine(6, "cen: %i", centre);
	turnToAngle(centre, 0);
}
//===============================================================================
//Call this function to check the prerequisites of the skill.  This will return 
//a bool indicating whether or not the skill is ciable in the present situation.
bool CreatorPassSkill::isValid() 
{ 
  
 
  //Get the aggressor location
  RobotIndex aggressorID = strategy->getCurrentRoboCupFrame()->getRobotByPosition(AGGRESSOR); 
  Pair aggressorPos(getLocation(aggressorID,*currentVisionData,*sp));
  Pair creator(getLocation(robotID,*currentVisionData,*sp));
  
  bool isPass=false;

  
  //In no pass, check out where the creator currently is
  if(!isPass ){
    isPass = calcYShot(aggressorID,creator.getY(),FACTOR*sp->strategy2002.PASS_LANE_THRESH,
      creator.getX()-CLOSE_BOUND,creator.getX()+CLOSE_BOUND,robotID,*currentVisionData,*sp,&creatorPos);
  }
  //In no pass, check out where the creator currently is
  if(!isPass ){
    isPass = calcShot(aggressorID,creator.getX(),FACTOR*sp->strategy2002.PASS_LANE_THRESH,
      creator.getY()-CLOSE_BOUND,creator.getY()+CLOSE_BOUND,robotID,*currentVisionData,*sp,&creatorPos);
  }

  isPass=isPass && 
         ABS( angleDifference(getRotation(aggressorID,*currentVisionData,*sp),angleBetween(aggressorPos,creatorPos))) < PI/MAXIMUM_ANGLE &&
         creatorPos.distanceTo(creator) < PASS_DIST  &&
         creatorPos.distanceTo(aggressorPos) > MIN_DIST  &&
         ABS(angleBetween(aggressorPos,creator)) < ANGLE_LIMIT;
  /// Set the pass destination
  if(isPass)
  {
    if(!initialized){
      strategy->getCurrentRoboCupFrame()->setPassDest(robotID,creatorPos);
      strategy->getCurrentRoboCupFrame()->setPassValue(robotID,true);
    }
  }


  ///If rotation of aggressor is correct and if pass can be received skill and creator is close enough, is valid
  return(isPass );

 
}
int gatherForwardsData(void) {
  wait10Msec(100);
  PlaySound(soundBlip);
	time100[T2] = 0;
	int timeToWait = minDistance*10/(2*sweepSpeed) - 10;

	int count = 0;
	int total = 0;
	int forwards = 0;

	while(time100[T2] < timeToWait) {
		//forwards = forwards + (angleDifference(currentDirection(), forwards))/++count;
	  count+= 2;
	  total += count;
	  //This formula give more weight to recent samples
	  forwards = forwards + (count * (angleDifference(currentDirection(), forwards))/total);

	  nxtDisplayCenteredTextLine(5, "%i", forwards);
		wait10Msec(5);
	}
	PlaySound(soundBlip);
	PlaySound(soundBlip);
	return forwards;
}
void FlightController::run() {
    double yUpperLimit = 9300;
    double lowerLimit = 1500;
    double xUpperLimit = 8100;
    Route myRoute;
    myRoute.initRoute(true);

    ros::Rate loop_rate(LOOP_RATE);
    setStraightFlight(true);

    bool firstIteration = true;

    takeOff();

    bool turning = true;
    double amountTurned = 0;
    double turnStepSize = 30;

    hoverDuration(3);
    navData->resetRawRotation();
    hoverDuration(2);

    cmd.linear.z = 0.5;
    pub_control.publish(cmd);
    while (navData->getPosition().z < 1200)
        ros::Rate(LOOP_RATE).sleep();



    hoverDuration(1);

    Vector3 pos;
    double starting_orientation = navData->getRawRotation();
    //ROS_INFO("Start while");

    while (!dronePossision.positionLocked) {
        //ROS_INFO("in while");
        if (turning) {
            turnDegrees(turnStepSize);
            double orientation = navData->getRawRotation();
            amountTurned += angleDifference(starting_orientation,orientation);
            //ROS_INFO("AMOUNTTURNED: %f",amountTurned);
            starting_orientation = orientation;
            if (amountTurned >= 360)
                turning = false;
        } else {
            navData->resetRaw();
            flyForward(0.4);
            turning = true;
            amountTurned = 0;
        }

    }

    lookingForQR = false;

    cmd.linear.z = -0.5;
    pub_control.publish(cmd);
    while (navData->getPosition().z > 900)
        ros::Rate(LOOP_RATE).sleep();

    //ROS_INFO("end while");
    navData->resetToPosition(dronePossision.x * 10, dronePossision.y * 10, dronePossision.heading);

    int startWall = dronePossision.wallNumber;

    turnDegrees(-dronePossision.angle);

    if(startWall == 0) {
        while (navData->getPosition().y - 700 > lowerLimit) {
            flyBackward(0.7);
            navData->addToY(-700);

            hoverDuration(3);
        }

        strafe(1, 0.8);
        navData->addToX(1000);
        hoverDuration(2);

        while (navData->getPosition().y + 700 < yUpperLimit) {
            flyForward(0.5);
            navData->addToX(700);

            hoverDuration(3);
        }
    } else if(startWall == 2){
        while (navData->getPosition().y + 700 < yUpperLimit) {
            flyForward(0.5);
            navData->addToX(700);

            hoverDuration(3);
        }

        strafe(1, 0.8);
        navData->addToX(-1000);
        hoverDuration(2);

        while (navData->getPosition().y - 700 > lowerLimit) {
            flyBackward(0.7);
            navData->addToY(-700);

            hoverDuration(3);
        }
    } else if(startWall == 3) {
        while (navData->getPosition().x + 700 < xUpperLimit){
            flyBackward(0.7);
            navData->addToX(700);
            hoverDuration(3);
        }

        strafe(1, 0.8);
        navData->addToY(1000);
        hoverDuration(2);

        while (navData->getPosition().x - 700 > xUpperLimit){
            flyBackward(0.7);
            navData->addToX(-700);
            hoverDuration(3);
        }
    } else if(startWall == 1){
        while (navData->getPosition().x + 700 < xUpperLimit){
            flyBackward(0.7);
            navData->addToX(700);
            hoverDuration(3);
        }

        strafe(1, 0.8);
        navData->addToY(-1000);
        hoverDuration(2);

        while (navData->getPosition().x - 700 > xUpperLimit){
            flyBackward(0.7);
            navData->addToX(-700);
            hoverDuration(3);
        }
    }
   /* else if(dronePossision.wallNumber == 1){
        while (navData->getPosition().y - 1000 > 1500) {
            flyForward(0.7);
            navData->addToY(-1000);

            hoverDuration(3);
            cvHandler->swapCam(false);
            cvHandler->checkCubes();
            cvHandler->swapCam(true);
        }
    }*/




    /*double rotation = navData->getRotation();
    double target;
    int wallNumber = dronePossision.wallNumber;
    ROS_INFO("WALL NUMBER RECEIVED START: %d", wallNumber);
    switch(dronePossision.wallNumber) {
        case 0:
            target = 180;
            break;
        case 1:
            target = 270;
            break;
        case 2:
            target = 0;
            break;
        case 3:
            target = 90;
            break;
    }

    ROS_INFO("Target = %f", target);*/



    /*int i = 0;
    while(i < 4) {
        double difference = angleDifference(rotation, target);
        int direction = angleDirection(rotation, target);
        ROS_INFO("Difference = %f", difference);
        ROS_INFO("Direction = %f", direction);
        //turnDegrees(difference*direction);

        cmd.angular.z = 1;
        pub_control.publish(cmd);
        while(navData->getRotation() > 190 || navData->getRotation() < 170 )
            ros::Rate(50).sleep();

    //ROS_INFO("Difference = %f", difference);
    //ROS_INFO("Direction = %f", direction);
    //turnTowardsAngle(target, 0);

/*    lookingForQR = true;
    flyForward(0.7);
    cvHandler->swapCam(false);
    hoverDuration(3);
    cvHandler->checkCubes();
    cvHandler->swapCam(true);
    flyForward(0.7);
    cvHandler->swapCam(false);
    hoverDuration(3);
    cvHandler->checkCubes();
    cvHandler->swapCam(true);
    flyForward(0.7);
    cvHandler->swapCam(false);
    hoverDuration(3);
    cvHandler->checkCubes();
    cvHandler->swapCam(true);
    flyForward(0.7);
    cvHandler->swapCam(false);
    hoverDuration(3);
    cvHandler->checkCubes();
    cvHandler->swapCam(true);

    lookingForQR = true;
    strafe(0.7, -1);
    cvHandler->swapCam(false);
    hoverDuration(3);
    cvHandler->checkCubes();
    cvHandler->swapCam(true);
    strafe(0.7, -1);
    cvHandler->swapCam(false);
    hoverDuration(3);
    cvHandler->checkCubes();
    cvHandler->swapCam(true);
    strafe(0.7, -1);
    cvHandler->swapCam(false);
    hoverDuration(3);
    cvHandler->checkCubes();
    cvHandler->swapCam(true);
    strafe(0.7, -1);
    cvHandler->swapCam(false);
    hoverDuration(3);
    cvHandler->checkCubes();
    cvHandler->swapCam(true);

    if(target == 0)
        while(navData->getPosition().y +1000 < 9300) {
            flyForward(0.7);
            navData->addToY(1000);

                hoverDuration(4);
                cvHandler->swapCam(false);
                cvHandler->checkCubes();
                cvHandler->swapCam(true);
                if (navData->getRotation() != target) {
                    difference = angleDifference(rotation, target);
                    direction = angleDirection(rotation, target);
                    ROS_INFO("Difference = %f", difference);
                    ROS_INFO("Direction = %f", direction);
                    turnDegrees(difference * direction);
                }
            }
        else if (target == 180)
            while (navData->getPosition().y - 1000 > 1500) {
                flyForward(0.7);
                navData->addToY(-1000);

                hoverDuration(4);
                cvHandler->swapCam(false);
                cvHandler->checkCubes();
                cvHandler->swapCam(true);
                if (navData->getRotation() != target) {
                    difference = angleDifference(rotation, target);
                    direction = angleDirection(rotation, target);
                    ROS_INFO("Difference = %f", difference);
                    ROS_INFO("Direction = %f", direction);
                    turnDegrees(difference * direction);
                }
            }

        }
*/

    land();
    return;
}
예제 #12
0
//=====================================================
///Execute the skill.  This is the main part of the skill, where you tell the
///robot how to perform the skill.
void CutGoalSkill::execute()
{		 
	///If not active, dont do anything!
	if(!initialized)
	{
		return;  
	}
	else
	{
		//grab the ball location
		
    if(!presetBall){
		  ball = strategy->getCurrentRoboCupFrame()->getDefensiveBallLocation();
    }
		

    ///Calculate the angle bisector of the area we want to cover
    float theta;
	  float theta1=angleBetween(sp->field.OUR_GOAL_LINE,point1,ball.getX(),ball.getY());
    float theta2=angleBetween(sp->field.OUR_GOAL_LINE,point2,ball.getX(),ball.getY());
    theta=(theta1+theta2)/2.0f;
    theta=normalizeAngle(theta+PI);
    float halfAngle=ABS(angleDifference(theta1,theta2)/2.0f);

    //calculate midpoint to extend from
		Pair midpoint;
		midpoint.setY((sp->field.OUR_GOAL_LINE-ball.getX()) * TAN(theta) + ball.getY());
	  midpoint.setX(sp->field.OUR_GOAL_LINE);

    /*debugging helpful stuff
    GUI_Record.debuggingInfo.setDebugPoint(robotID,6,midpoint);   
    GUI_Record.debuggingInfo.setDebugPoint(robotID,7,sp->field.OUR_GOAL_LINE,point1);   
    GUI_Record.debuggingInfo.setDebugPoint(robotID,8,sp->field.OUR_GOAL_LINE,point2);   
    Pair ang1(ball.getX()+.2f,ball.getY());
    Pair ang2(ball.getX()+.2f,ball.getY());
    rotateAboutPoint(ang1,ball,theta1,ang1);
    rotateAboutPoint(ang2,ball,theta2,ang2);
    GUI_Record.debuggingInfo.setDebugPoint(robotID,3,ang1);   
    GUI_Record.debuggingInfo.setDebugPoint(robotID,4,ang2);  
    
    Pair t(ball.getX()+.2f,ball.getY());
    rotateAboutPoint(t,ball,theta,t);
    GUI_Record.debuggingInfo.setDebugPoint(robotID,5,t);  
    */

	  // The ideal point we want the robot to be in this circumstances
	  Pair dest;
	  float distance = sp->general.PLAYER_RADIUS / SIN(ABS(halfAngle)) ;
	  //char msg[80]; sprintf(msg,"dist: %5.2f",distance);GUI_Record.debuggingInfo.addDebugMessage(msg);


	  extendPoint(midpoint,ball,-distance,dest);

	  // We have to check if the destination point is between the Upper and lower limit
	  //	  float slope =  (midpoint.getY() - ball.getY()) / (midpoint.getX() - ball.getY()) ;
	  // If it is above the limit
	  if(dest.getX() > UPPER_X){
	    extrapolateForY(midpoint,ball,UPPER_X,dest);
	  }
	  // If it is below the limit
	  if(dest.getX() < LOWER_X){
	    extrapolateForY(midpoint,ball,LOWER_X,dest);
	  }
	  command->setControl(OMNI_NORMAL);
	  command->setPos(dest);

	  command->setRotation(angleBetween(getLocation(robotID, *currentVisionData, *sp),
                                      ball));
      
    strategy->getCurrentFrame()->setMessage(robotID,"Covering Goal");

	}
}
예제 #13
0
/**
 * Module task
 */
static void stabilizationTask(void* parameters)
{
	StabilizationSettingsData stabSettings;
	ActuatorDesiredData actuatorDesired;
	AttitudeDesiredData attitudeDesired;
	AttitudeActualData attitudeActual;
	ManualControlCommandData manualControl;
	SystemSettingsData systemSettings;
	portTickType lastSysTime;
	float pitchErrorGlobal, pitchErrorLastGlobal;
	float yawErrorGlobal, yawErrorLastGlobal;
	float pitchError, pitchErrorLast;
	float yawError, yawErrorLast;
	float rollError, rollErrorLast;
	float pitchDerivative;
	float yawDerivative;
	float rollDerivative;
	float pitchIntegral, pitchIntegralLimit;
	float yawIntegral, yawIntegralLimit;
	float rollIntegral, rollIntegralLimit;

	// Initialize
	pitchIntegral = 0.0;
	yawIntegral = 0.0;
	rollIntegral = 0.0;
	pitchErrorLastGlobal = 0.0;
	yawErrorLastGlobal = 0.0;
	rollErrorLast = 0.0;

	// Main task loop
	lastSysTime = xTaskGetTickCount();
	while (1)
	{
		// Read settings and other objects
		StabilizationSettingsGet(&stabSettings);
		SystemSettingsGet(&systemSettings);
		ManualControlCommandGet(&manualControl);
		AttitudeDesiredGet(&attitudeDesired);
		AttitudeActualGet(&attitudeActual);

		// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.

		// global pitch error
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
		{
			pitchErrorGlobal = angleDifference(
					bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
				);
		}
		else
		{
			// in AUTO mode, Stabilization is used to steer the plane freely,
			// while Navigation dictates the flight path, including maneuvers outside stable limits.
			pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
		}

		// global yaw error
		if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL || manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
		{
			// VTOLS consider yaw. AUTO mode considers YAW, too.
			yawErrorGlobal = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
		} else {
			// FIXED WING STABILIZATION however does not.
			yawErrorGlobal = 0;
		}

		// local pitch error
		pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
		// local roll error (no translation needed - always local)
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
		{
			rollError = angleDifference(
					bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
				);
		}
		else
		{
			// in AUTO mode, Stabilization is used to steer the plane freely,
			// while Navigation dictates the flight path, including maneuvers outside stable limits.
			rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
		}
		// local yaw error
		yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;

		// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts

		// pitch last
		pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;

		// yaw last
		yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;

		// global last variables are no longer needed
		pitchErrorLastGlobal = pitchErrorGlobal;
		yawErrorLastGlobal = yawErrorGlobal;

		// local Pitch stabilization control loop
		pitchDerivative = pitchError - pitchErrorLast;
		pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
		pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit);
		actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
		actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);

		// local Roll stabilization control loop
		rollDerivative = rollError - rollErrorLast;
		rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
		rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit);
		actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
		actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
		rollErrorLast = rollError;
sdf;

		// local Yaw stabilization control loop (only enabled on VTOL airframes)
		if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
		{
			yawDerivative = yawError - yawErrorLast;
			yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
			yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit);
			actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
			actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
		}
		else
		{
			actuatorDesired.Yaw = 0.0;
		}


		// Setup throttle
		actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);

		// Write actuator desired (if not in manual mode)
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
		{
			ActuatorDesiredSet(&actuatorDesired);
		}
		else
		{
			pitchIntegral = 0.0;
			yawIntegral = 0.0;
			rollIntegral = 0.0;
			pitchErrorLastGlobal = 0.0;
			yawErrorLastGlobal = 0.0;
			rollErrorLast = 0.0;
		}

		// Clear alarms
		AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);

		// Wait until next update
		vTaskDelayUntil(&lastSysTime, stabSettings.UpdatePeriod / portTICK_RATE_MS );
	}
}
bool detectNode(int forwards) {
	return abs(angleDifference(currentDirection(), forwards)) > ANGLE_THRESHOLD;
}
예제 #15
0
//===================================================================================
float laneHalfAngle (Pair passerLoc, // Position of the passing robot.
                     Pair passDestination, // location of the pass destination
                     const VisionData &field, // where everyone else is now
                     const SystemParameters &rp, // contains necessary game parameters  
                     const Pair * extraObstacle, // Check this (optional) location as an obstacle too 
                     bool checkOurRobots) //to see if we check for our robots or not...
{

  float minLaneAngle = PI/2.0f; // Initialize the lane half angle to PI/2 (a clear lane)

  // Lane parameters
  float laneLength = passerLoc.distanceTo (passDestination);
  laneLength = laneLength + rp.general.PLAYER_RADIUS + rp.general.BALL_RADIUS;
  float laneDirection = angleWithXAxis (passerLoc, passDestination);

  // Obstacle parameters
  Pair obstacleLoc;         // Obstacle centre location
  float obstacleDist;       // Distance of the obstacle centre from the passerLoc
  float obstacleDirection;  // Angle made by the segment from passerLoc to obstacle with the x-axis
  float obstacleHalfAngle;  // Half the angle subtended by the obstacle on the passer loc
  float angleWithLane1;     // Angle between the tangents drawn from passerLoc to obstacle with 
  float angleWithLane2;     // the lane direction


  // Check opponent robots first
  RobotIndex i;
  for (i = ROBOT0; i < NUM_ROBOTS; i++) {

    if (theirRobotFound (i, field, rp)) {

      // Get obstacle direction and distance from the passerLoc
      obstacleLoc = getTheirRobotLocation (i, field, rp);
      obstacleDist = passerLoc.distanceTo (obstacleLoc);
  
      // Get the direction of the segment from passerLoc to obstacle with the x-axis
      obstacleDirection = angleWithXAxis (passerLoc, obstacleLoc);
      obstacleHalfAngle = ATAN2 (rp.general.OPPONENT_RADIUS, obstacleDist);

      // Evaluate angles only if the passer loc is closer to the obtacle than the lane length
      if (obstacleDist <= laneLength) {

        // If the difference between the obstacle direction and lane direction is within the half
        // angle subtended by the obstacle on the passer loc, the lane is being blocked
        if (ABS (laneDirection - obstacleDirection) <= obstacleHalfAngle)
          return 0.0f;

        // Get the directions of the tangent to the obstacle from the passer loc and their angular
        // difference from the lane direction
        angleWithLane1 = ABS (angleDifference (obstacleDirection + obstacleHalfAngle, laneDirection));
        angleWithLane2 = ABS (angleDifference (obstacleDirection - obstacleHalfAngle, laneDirection));
      

        // Compare the angle of the tangent closer to the lane diretion
        float smallerAngle = MIN (angleWithLane1, angleWithLane2);
        if (smallerAngle < minLaneAngle)
          minLaneAngle = smallerAngle;

      }
    }

  }

  //IF WE WANT TO TAKE INTO ACCOUNT OUR OWN ROBOTS...
  if (checkOurRobots)
  {
    for (i = ROBOT0; i < NUM_ROBOTS; i++) {

      //If our robot is found, and he is not the passer or receiver => check to see if it is in the way
          if(robotFound(i, field, rp)&&(dist(i,passerLoc,field,rp)>rp.general.PLAYER_RADIUS)&&(dist(i,passDestination,field,rp)>rp.general.PLAYER_RADIUS)) {      

        // Get obstacle direction and distance from the passerLoc
        obstacleLoc = getLocation (i, field, rp);
        obstacleDist = passerLoc.distanceTo (obstacleLoc);
  
        // Get the direction of the segment from passerLoc to obstacle with the x-axis
        obstacleDirection = angleWithXAxis (passerLoc, obstacleLoc);
        obstacleHalfAngle = ATAN2 (rp.general.PLAYER_RADIUS, obstacleDist);

        // Evaluate angles only if the passer loc is closer to the obtacle than the lane length
        if (obstacleDist <= laneLength) {

          // If the difference between the obstacle direction and lane direction is within the half
          // angle subtended by the obstacle on the passer loc, the lane is being blocked
          if (ABS (laneDirection - obstacleDirection) <= obstacleHalfAngle)
            return 0.0f;

          // Get the directions of the tangent to the obstacle from the passer loc and their angular
          // difference from the lane direction
          angleWithLane1 = ABS (angleDifference (obstacleDirection + obstacleHalfAngle, laneDirection));
          angleWithLane2 = ABS (angleDifference (obstacleDirection - obstacleHalfAngle, laneDirection));
      

          // Compare the angle of the tangent closer to the lane diretion
          float smallerAngle = MIN (angleWithLane1, angleWithLane2);
          if (smallerAngle < minLaneAngle)
            minLaneAngle = smallerAngle;

        }
      }

    }
  }


  // Check the passed extra location as an obstacle having the radius the same as our robots
  if (extraObstacle) {

    // Get obstacle direction and distance from the passerLoc
    obstacleLoc = * extraObstacle;
    obstacleDist = passerLoc.distanceTo (obstacleLoc);

    // Get the direction of the segment from passerLoc to obstacle with the x-axis
    obstacleDirection = angleWithXAxis (passerLoc, obstacleLoc);
    obstacleHalfAngle = ATAN2 (rp.general.PLAYER_RADIUS, obstacleDist);

    // Evaluate angles only if the passer loc is closer to the obtacle than the lane length
    if (obstacleDist <= laneLength) {

      // If the difference between the obstacle direction and lane direction is within the half
      // angle subtended by the obstacle on the passer loc, the lane is being blocked
      if (ABS (laneDirection - obstacleDirection) <= obstacleHalfAngle)
        return 0.0f;

      // Get the directions of the tangent to the obstacle from the passer loc and their angular
      // difference from the lane direction
      angleWithLane1 = ABS (angleDifference (obstacleDirection + obstacleHalfAngle, laneDirection));
      angleWithLane2 = ABS (angleDifference (obstacleDirection - obstacleHalfAngle, laneDirection));
      

      // Compare the angle of the tangent closer to the lane diretion
      float smallerAngle = MIN (angleWithLane1, angleWithLane2);
      if (smallerAngle < minLaneAngle)
        minLaneAngle = smallerAngle;

    }
  }

  return minLaneAngle;
}
예제 #16
0
/**
 * process data string from flight simulator
 */
void IL2Simulator::processUpdate(const QByteArray& inp)
{
    // save old flight data to calculate delta's later
    old=current;
    QString data(inp);
    // Split
    QStringList fields = data.split("/");

    // split up response string
    int t;
    for (t=0; t<fields.length(); t++) {
        QStringList values = fields[t].split("\\");
        // parse values
        if (values.length()>=2) {
            int id = values[0].toInt();
            float value = values[1].toFloat();
            switch (id) {
            case 30:
                current.cas=value * KM_PER_HOUR2METERS_PER_SECOND;
                break;
            case 32:
                current.dZ=value;
                break;
            case 40:
                current.Z=value;
                break;
            case 42:
                current.azimuth=value;
                break;
            case 46:
                current.roll=-value;
                break;
            case 48:
                current.pitch=value;
                break;
            }
        }
    }

    // measure time
    current.dT = ((float)time->restart()) / 1000.0;
    if (current.dT<0.001)  current.dT=0.001;
    current.T = old.T+current.dT;
    current.i = old.i+1;
    if (current.i==1) {
        old.dRoll=0;
        old.dPitch=0;
        old.dAzimuth=0;
        old.ddX=0;
        old.ddX=0;
        old.ddX=0;
    }

    // calculate TAS from alt and CAS
    current.tas = cas2tas(current.cas, current.Z, airParameters, GRAVITY);

    // assume the plane actually flies straight and no wind
    // groundspeed is horizontal vector of TAS
    current.groundspeed = current.tas*cos(current.pitch*DEG2RAD);
    // x and y vector components
    current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD);
    current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD);

    // simple IMS - integration over time the easy way...
    current.X = old.X + (current.dX*current.dT);
    current.Y = old.Y + (current.dY*current.dT);

    // accelerations (filtered)
    if (isnan(old.ddX) || isinf(old.ddX)) old.ddX=0;
    if (isnan(old.ddY) || isinf(old.ddY)) old.ddY=0;
    if (isnan(old.ddZ) || isinf(old.ddZ)) old.ddZ=0;
#define SPEED_FILTER 10
    current.ddX = ((current.dX-old.dX)/current.dT + SPEED_FILTER * (old.ddX)) / (SPEED_FILTER+1);
    current.ddY = ((current.dY-old.dY)/current.dT + SPEED_FILTER * (old.ddY)) / (SPEED_FILTER+1);
    current.ddZ = ((current.dZ-old.dZ)/current.dT + SPEED_FILTER * (old.ddZ)) / (SPEED_FILTER+1);

#define TURN_FILTER 10
    // turn speeds (filtered)
    if (isnan(old.dAzimuth) || isinf(old.dAzimuth)) old.dAzimuth=0;
    if (isnan(old.dPitch) || isinf(old.dPitch)) old.dPitch=0;
    if (isnan(old.dRoll) || isinf(old.dRoll)) old.dRoll=0;
    current.dAzimuth = (angleDifference(current.azimuth,old.azimuth)/current.dT + TURN_FILTER * (old.dAzimuth)) / (TURN_FILTER+1);
    current.dPitch   = (angleDifference(current.pitch,old.pitch)/current.dT     + TURN_FILTER * (old.dPitch))   / (TURN_FILTER+1);
    current.dRoll    = (angleDifference(current.roll,old.roll)/current.dT       + TURN_FILTER * (old.dRoll))    / (TURN_FILTER+1);

    ///////
    // Output formatting
    ///////
    Output2Hardware out;
    memset(&out, 0, sizeof(Output2Hardware));

    // Compute rotation matrix, for later calculations
    float Rbe[3][3];
    float rpy[3];
    float quat[4];
    rpy[0]=current.roll;
    rpy[1]=current.pitch;
    rpy[2]=current.azimuth;
    Utils::CoordinateConversions().RPY2Quaternion(rpy,quat);
    Utils::CoordinateConversions().Quaternion2R(quat,Rbe);

    // Update GPS Position objects, convertin from the current NED position
    // to LLA
    double HomeLLA[3];
    double LLA[3];
    double NED[3];
    HomeLLA[0]=settings.latitude.toFloat();
    HomeLLA[1]=settings.longitude.toFloat();
    HomeLLA[2]=0;
    NED[0] = current.Y;
    NED[1] = current.X;
    NED[2] = -current.Z;
    Utils::CoordinateConversions().NED2LLA_HomeLLA(HomeLLA, NED, LLA);
    out.latitude = LLA[0];
    out.longitude = LLA[1];
    out.groundspeed = current.groundspeed;

    out.calibratedAirspeed = current.cas;
    out.trueAirspeed=cas2tas(current.cas, current.Z, airParameters, GRAVITY);

    out.dstN=current.Y;
    out.dstE=current.X;
    out.dstD=-current.Z;

    // Update BaroAltitude object
    out.altitude = current.Z;
    out.agl = current.Z;
    out.temperature = airParameters.groundTemp + (current.Z * airParameters.tempLapseRate) - 273.0;
    out.pressure = airPressureFromAltitude(current.Z, airParameters, GRAVITY) ; // kpa


    // Update attActual object
    out.roll = current.roll;   //roll;
    out.pitch = current.pitch;  // pitch
    out.heading = current.azimuth; // yaw


    // Update VelocityActual.{North,East,Down}
    out.velNorth = current.dY;
    out.velEast = current.dX;
    out.velDown = -current.dZ;

    // rotate turn rates and accelerations into body frame
    // (note: rotation deltas are NOT in NED frame but in RPY - manual conversion!)
    out.rollRate = current.dRoll;
    out.pitchRate = cos(DEG2RAD * current.roll) * current.dPitch + sin(DEG2RAD * current.roll) * current.dAzimuth;
    out.yawRate = cos(DEG2RAD * current.roll) * current.dAzimuth - sin(DEG2RAD * current.roll) * current.dPitch;

    //Update accelerometer sensor data
    out.accX = current.ddX*Rbe[0][0]
            +current.ddY*Rbe[0][1]
            +(current.ddZ+GRAVITY)*Rbe[0][2];
    out.accY = current.ddX*Rbe[1][0]
            +current.ddY*Rbe[1][1]
            +(current.ddZ+GRAVITY)*Rbe[1][2];
    out.accZ = - (current.ddX*Rbe[2][0]
                     +current.ddY*Rbe[2][1]
                     +(current.ddZ+GRAVITY)*Rbe[2][2]);

    updateUAVOs(out);
}
//===============================================================================
//Execute the skill.  This is the main part of the skill, where you tell the
//robot how to perform the skill.
void RotateAroundBallSkill::execute()
{    
  ///If not active, dont do anything!
  if(!initialized) return; 

  command->setDribble(SLOW_DRIBBLE);
  command->setVerticalDribble(SLOW_V_DRIBBLE);

  if(USE_BALL_FOR_PIVOT)
  {
    pivotPoint.set(strategy->getCurrentRoboCupFrame()->getOffensiveBallLocation());     
  }
  else
  {
    pivotPoint.set(frontOfRobot(robotID, *currentVisionData, *sp));
  }

  ultimateAngle = angleBetween(pivotPoint,ultimateDestination);

  //get robots current location and angle
  robotLoc.set(getLocation(robotID,*currentVisionData,*sp));
  robotAngle = angleBetween(robotLoc, pivotPoint);

  //if finished, then keep position and rotation
  if(isFinished())
  {
    command->setPos(robotLoc);
    command->setRotation(angleBetween(robotLoc, ultimateDestination));
    strategy->getCurrentFrame()->setMessage(robotID, "rotate done");
    return;
  }

  strategy->getCurrentFrame()->setMessage(robotID, "rotate rotating");

  //determine next angle robot should be at
  nextAngle = ultimateAngle;

  //determine angle diff 
  angleDiff = angleDifference(ultimateAngle, robotAngle);
  
  if( angleDiff > ROTATION_STEP )
  {
    nextAngle = robotAngle + ROTATION_STEP;
    angleDiff = ROTATION_STEP;
  }
  else if (angleDiff < -ROTATION_STEP)
  {
    nextAngle = robotAngle - ROTATION_STEP;
  }

  if(ABS(angleDiff) < FINISHED_ROTATION_THRESHOLD)
  {
    finished = true;
  }

  Pair currLoc;
  extendPoint(robotLoc, 
              pivotPoint,
              -(sp->general.PLAYER_RADIUS - PUSH_FACTOR),
              currLoc);

  rotateAboutPoint(currLoc, 
                   pivotPoint,
                   nextAngle - robotAngle, 
                   rotatedDestination);

  destRot = angleBetween(rotatedDestination, pivotPoint);

  command->setPos(rotatedDestination);
  command->setRotation(destRot);

  GUI_Record.debuggingInfo.setDebugPoint(robotID,1,ultimateDestination);
  GUI_Record.debuggingInfo.setDebugPoint(robotID,2,robotLoc);
  GUI_Record.debuggingInfo.setDebugPoint(robotID,3,rotatedDestination);

}
예제 #18
0
//==============================================
void FormationTester::executePlay(VisionData* vision, RobocupStrategyData* rsd) 
{
  if(moveOn)
  {
    robotID++;
    mode = 0;
    moveOn = false;
  }

  //for not active robots, set their dest location and rotation
  for(int i=0; i<NUM_ROBOTS; i++)
  {
    rsd->getDestination(i)->setControl(OMNI_NORMAL_ENTERBOX);
    if(i != robotID)
    {   
      rsd->getDestination(i)->setPos(location[i]);
      rsd->getDestination(i)->setRotation(rotation[i]);
      rsd->setMessage((RobotIndex)i, "Camping Out");
    }
  }

  //if robot doesn't exist, move
  while(
         robotID <= NUM_ROBOTS &&
         !found[robotID]       
       )
  {
    robotID++;
  }

  if(robotID >=0 && robotID < NUM_ROBOTS)
  {
    //rotate right
    if(mode == 0)
    {
      rsd->getDestination(robotID)->setPos(location[robotID]);

      float destAngle = normalizeAngle(rotation[robotID] - 5.0f*PI/12.0f);
      rsd->getDestination(robotID)->setRotation(destAngle);
      rsd->setMessage((RobotIndex)robotID, "Rotating Right");

      if(
          ABS(angleDifference(
                               getRotation((RobotIndex)robotID, *vision, rsd->getSystemParams()),
                               destAngle
                             )) < PI/16.0f
        )
      {
        mode++;
      }
    }
    //rotate left
    else if(mode == 1)
    {
      rsd->getDestination(robotID)->setPos(location[robotID]);

      float destAngle = normalizeAngle(rotation[robotID] + 5.0f*PI/12.0f);
      rsd->getDestination(robotID)->setRotation(destAngle);
      rsd->setMessage((RobotIndex)robotID, "Rotating Left");

      if(
          ABS(angleDifference(
                               getRotation((RobotIndex)robotID, *vision, rsd->getSystemParams()),
                               destAngle
                             )) < PI/16.0f
        )
      {
        mode++;
      }
    }
    //go back to normal
    else
    {
      rsd->getDestination(robotID)->setPos(location[robotID]);

      float destAngle = rotation[robotID];     
      rsd->getDestination(robotID)->setRotation(destAngle);
      rsd->setMessage((RobotIndex)robotID, "Rotating Back");
      if(
          ABS(angleDifference(
                               getRotation((RobotIndex)robotID, *vision, rsd->getSystemParams()),
                               destAngle
                             )) < PI/16.0f
        )
      {
        moveOn = true;
      }
    }
  }
}
예제 #19
0
//===============================================================================
//Execute the skill.  This is the main part of the skill, where you tell the
//robot how to perform the skill.
void JamAndShootSkill::execute()
{  
  ///If not active, dont do anything!
	if(!initialized)
	{
		return;  
  }

  //aggresive mode
  if(sp->strategy2002.JAM_AND_SHOOT_MODE == 1)
  {
    skillSet->getSkill(AggressiveJamAndShootSkill::skillNum)->run();
    return;
  }
  //stupid mode
  else if(sp->strategy2002.JAM_AND_SHOOT_MODE == 2)
  {
    skillSet->getSkill(StupidJamAndShootSkill::skillNum)->run();
    return;
  }
  
  
  
  //else normal mode
    //decrease the shoot threshold linearly as the skill runs.
  shoot_threshold = sp->strategy2002.SHOOT_LANE_THRESH;// *
                    //(WAIT_TIME - (float)timer->currentTime()) / WAIT_TIME;
                   
  
  Pair currentPos(getLocation(robotID,*currentVisionData,*sp));
  float currentRot = getRotation(robotID,*currentVisionData,*sp);
  float good_shoot_thresh=shoot_threshold;
  if(kicked) good_shoot_thresh*=1.5f;
  else if(aimed) good_shoot_thresh=.05f;
  //finished waiting.  Shoot!
  //note, we only shoot if we're facing in the direction of the goal.
  kickFrames--;
  if(
      !gaveup  && 
      towardGoal(currentPos,
                 currentRot,sp->field.THEIR_GOAL_LINE,
                 sp->field.LEFT_GOAL_POST,
                 sp->field.RIGHT_GOAL_POST) &&
      (timer->currentTime() > WAIT_TIME || 
       goodImmediateShot(sp->general.TEAM,robotID,sp->strategy2002.KICK_VELOCITY,
                        *currentVisionData,*sp,good_shoot_thresh)
      //goodCurrentShot(currentPos,currentRot,*currentVisionData,*sp,good_shoot_thresh)
      )
    )
  {
    gaveup=true;
    if(driftdir != 0){
      kickFrames = PAUSE_FRAMES + 2*rand()*PAUSE_RANDOM/RAND_MAX - PAUSE_RANDOM;
      char msg[80];
      sprintf(msg,"Jam and Shoot pausing for %d frames",kickFrames);
      GUI_Record.debuggingInfo.addDebugMessage(msg);
    }
    kicked=false;
    timer->resetTimer();
  }


  if (gaveup && kickFrames <= 0){
    //we're out of time or have a good current shot, so just kick.
    SimpleKickSkill * kickSkill=(SimpleKickSkill *) skillSet->getSkill(SimpleKickSkill::skillNum);
    if(!kickSkill->isInitialized()){
      kickSkill->initialize(KICK_SHOT);
    }
    kickSkill->run();
    strategy->getCurrentFrame()->setMessage(robotID,"No Turn and Kick. Just Shooting.");
  }else if(skillSet->getSkill(PullBallOffWallSkill::skillNum)->isValid()){
    //deal with the ball in the corner or on the wall
    if(!skillSet->getSkill(PullBallOffWallSkill::skillNum)->isInitialized()){
      skillSet->getSkill(PullBallOffWallSkill::skillNum)->initialize();
    }
    skillSet->getSkill(PullBallOffWallSkill::skillNum)->run();
    
  }else if(kicked) {
    //We have a shot, let's aim and kick.
    TurnAndKickSkill *kicker=(TurnAndKickSkill *)skillSet->getSkill(TurnAndKickSkill::skillNum);
    kicker->run();
    //strategy->getCurrentFrame()->setMessage(robotID,"Have Shot - taking it");    
  }else{
	  
    bool isShot;
    //find best shot (if we're already drifting, use a larger threshold)
    if(aimed)isShot=calcShot(robotID,sp->field.THEIR_GOAL_LINE,sp->strategy2002.SHOOT_LANE_THRESH+2*sp->general.PLAYER_RADIUS,
      sp->field.RIGHT_GOAL_POST,sp->field.LEFT_GOAL_POST,NO_ROBOT,*currentVisionData,*sp,&target);
    else isShot=calcShot(robotID,sp->field.THEIR_GOAL_LINE,sp->strategy2002.SHOOT_LANE_THRESH,
      sp->field.RIGHT_GOAL_POST,sp->field.LEFT_GOAL_POST,NO_ROBOT,*currentVisionData,*sp,&target);

    if(isShot && driftdir==0)
    {
      //take the shot
      TurnAndKickSkill *kicker=(TurnAndKickSkill *)skillSet->getSkill(TurnAndKickSkill::skillNum);
      kicker->initialize(target,KICK_SHOT,true);
      kicker->run();
      kicked=true;
      strategy->getCurrentFrame()->setMessage(robotID,"Have a shot!");   
      kickFrames=0;
    }else{
      target.set(sp->field.THEIR_GOAL_LINE,sp->field.SPLIT_LINE);
      //if no shot, start to wait, dodge if possible
      GUI_Record.debuggingInfo.setDebugPoint(robotID,4,upperbound,testline);
      GUI_Record.debuggingInfo.setDebugPoint(robotID,5,lowerbound,testline);

      if (driftdir==0){
        //Calculate which direction to drift

        //first, see if we can shoot by shifting to the near post.
        //if not, drift across the field.

        //Default to drift across.
        float goalPost;
        if(currentPos.getY()>sp->field.SPLIT_LINE){
          driftdir = 1;
          goalPost=sp->field.LEFT_GOAL_POST;
        }else{
          driftdir = -1;
          goalPost=sp->field.RIGHT_GOAL_POST;
        }
        upperbound=sp->field.THEIR_GOAL_LINE;
        lowerbound=sp->field.KILL_ZONE_LINE;
        testline=sp->field.SPLIT_LINE + driftdir*(SIDE_DIST);

        Pair loc;

        //check near-side shot.
        if(currentPos.getY() * driftdir >= goalPost*driftdir &&
           calcYShot(Pair(sp->field.THEIR_GOAL_LINE,sp->field.SPLIT_LINE),
                     testline,
                     sp->strategy2002.SHOOT_LANE_THRESH*SIDE_LANE_FACTOR,
                     upperbound,
                     lowerbound,NO_ROBOT,
                     *currentVisionData,
                     *sp, 
                     &loc)){
          driftdir=-driftdir;
        }
      }
      //  printf("%f\n",ABS(angleDifference(angleBetween(currentPos,target),currentRot)));

      if(ABS(angleDifference(angleBetween(currentPos,target),currentRot)) < AIM_ANGLE  || aimed){
        aimed=true;
        //if(target.getX() != sp->field.THEIR_GOAL_LINE) target.setX(sp->field.THEIR_GOAL_LINE);
        //drift to the side to see if we have a shot.
        //calculate the direction to drift
        
        
        Pair dest;
        float changeAngle;
        //switch directions if we've gone too far.
        if(angleBetween(currentPos,target)*driftdir > BOUNCE_ANGLE ){
          driftdir=-driftdir;
        }
        target.setY(target.getY() - AIM_DISTANCE*driftdir);
        changeAngle= DRIFT_ANGLE*driftdir;

        char msg[80];
        sprintf(msg, "No Shot - drifting %s",( driftdir > 0 ? "right" : "left"));
        strategy->getCurrentFrame()->setMessage(robotID,msg);
         //drift around goal 
        //See if we can push forward:
        rotateAboutPoint(frontOfRobot(robotID,*currentVisionData,*sp), target,changeAngle, dest);

   //DUE TO J&S BACKING UP TOO MUCH, I'M GETTING RID OF THIS NEXT PART
        
        if(!isLane(currentPos,dest,sp->general.PLAYER_RADIUS,*currentVisionData,*sp,true)){
          //we can't.  :(  drift sideways.
          rotateAboutPoint(currentPos, target,changeAngle, dest);
        }
        
        
        //see if one of their robots is in the way, and back up if they are.
        if(!isLane(currentPos,dest,sp->general.PLAYER_RADIUS,*currentVisionData,*sp,true)){
          //move destination back
          extendPoint(target,dest,sp->general.PLAYER_RADIUS,dest);
        }
        
        command->setPos(dest);
        //Adjust angle to fast the direction we're drifting a bit.
        command->setRotation(angleBetween(currentPos,target) - driftdir*DRIFT_ADJUST_ANGLE);
        command->setDribble(DRIBBLE_DEFAULT);
        command->setVerticalDribble(V_DRIBBLE_DEFAULT);
        command->setSpeed(BALL_POSSESSION_SPEED);
      }else{
        strategy->getCurrentFrame()->setMessage(robotID,"No Shot - turning to goal");
        //turn to face goal
        SpinAroundBallSkill *spin=(SpinAroundBallSkill *)strategy->getSkillSet(robotID)->getSkill(SpinAroundBallSkill::skillNum);
        spin->initialize(target);
        spin->run();
      }
    }
  }
  RobotIndex goalie;
  if(getLocation(robotID,*currentVisionData,*sp).getX() > sp->field.HALF_LINE){
    if(getTheirGoalie(*currentVisionData,*sp,&goalie)){
      if(getLocation(sp->general.OTHER_TEAM,goalie,*currentVisionData).distanceTo(getLocation(robotID,*currentVisionData,*sp)) >= 
          ENTERBOX_CAUTION_DIST+sp->general.PLAYER_RADIUS + sp->general.OPPONENT_RADIUS){
        command->setControl(OMNI_FAST_ENTERBOX);
      }
    }else{
      //if no goalie, enter the box
      command->setControl(OMNI_FAST_ENTERBOX);
    }
  }


  GUI_Record.debuggingInfo.setDebugPoint(robotID,0,target);
  //command->setControl(OMNI_NORMAL_ENTERBOX);
  command->setRotation(angleBetween(currentPos,target));
}
예제 #20
0
/**
 * Module task
 */
static void stabilizationTask(void* parameters)
{
	UAVObjEvent ev;

	StabilizationSettingsData stabSettings;
	ActuatorDesiredData actuatorDesired;
	AttitudeDesiredData attitudeDesired;
	AttitudeActualData attitudeActual;
	ManualControlCommandData manualControl;
	SystemSettingsData systemSettings;
	portTickType lastSysTime;
	portTickType thisSysTime;
	float pitchErrorGlobal, pitchErrorLastGlobal;
	float yawErrorGlobal, yawErrorLastGlobal;
	float pitchError, pitchErrorLast;
	float yawError, yawErrorLast;
	float rollError, rollErrorLast;
	float pitchDerivative;
	float yawDerivative;
	float rollDerivative;
	float pitchIntegral, pitchIntegralLimit;
	float yawIntegral, yawIntegralLimit;
	float rollIntegral, rollIntegralLimit;
	float yawPrevious;
	float yawChange;

	// Initialize
	pitchIntegral = 0.0;
	yawIntegral = 0.0;
	rollIntegral = 0.0;
	pitchErrorLastGlobal = 0.0;
	yawErrorLastGlobal = 0.0;
	rollErrorLast = 0.0;
	yawPrevious = 0.0;

	// Main task loop
	lastSysTime = xTaskGetTickCount();
	while (1)
	{
		// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
                if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
                {
                        AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
                }

		// Check how long since last update
		thisSysTime = xTaskGetTickCount();
		if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
			dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
		lastSysTime = thisSysTime;

		// Read settings and other objects
		StabilizationSettingsGet(&stabSettings);
		SystemSettingsGet(&systemSettings);
		ManualControlCommandGet(&manualControl);
		AttitudeDesiredGet(&attitudeDesired);
		AttitudeActualGet(&attitudeActual);

		// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.

		// global pitch error
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
		{
			pitchErrorGlobal = angleDifference(
					bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
				);
		} 
		else
		{
			// in AUTO mode, Stabilization is used to steer the plane freely,
			// while Navigation dictates the flight path, including maneuvers outside stable limits.
			pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
		}

		// global yaw error
		if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
		{
			// VTOLS consider yaw. AUTO mode considers YAW, too.
			if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) {  // rate stabilization on yaw
					yawChange = (attitudeActual.Yaw - yawPrevious) / dT;
					yawPrevious = attitudeActual.Yaw;
					yawErrorGlobal = angleDifference(bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange);
				} else { // heading stabilization
					yawError = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
				}
		} else {
			// FIXED WING STABILIZATION however does not.
			yawErrorGlobal = 0;
		}
	
		// local pitch error
		pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
		// local roll error (no translation needed - always local)
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
		{
			rollError = angleDifference(
					bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
				);
		}
		else
		{
			// in AUTO mode, Stabilization is used to steer the plane freely,
			// while Navigation dictates the flight path, including maneuvers outside stable limits.
			rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
		}
		// local yaw error
		yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;
		
		// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts

		// pitch last
		pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;

		// yaw last
		yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;
		
		// global last variables are no longer needed 
		pitchErrorLastGlobal = pitchErrorGlobal;
		yawErrorLastGlobal = yawErrorGlobal;
		
		// local Pitch stabilization control loop
		pitchDerivative = pitchError - pitchErrorLast;
		pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
		pitchIntegral = bound(pitchIntegral+pitchError*dT, -pitchIntegralLimit, pitchIntegralLimit);
		actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
		actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);

		// local Roll stabilization control loop
		rollDerivative = rollError - rollErrorLast;
		rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
		rollIntegral = bound(rollIntegral+rollError*dT, -rollIntegralLimit, rollIntegralLimit);
		actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
		actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
		rollErrorLast = rollError;


		// local Yaw stabilization control loop (only enabled on VTOL airframes)
		if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
		    ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
		{
			yawDerivative = yawError - yawErrorLast;
			yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
			yawIntegral = bound(yawIntegral+yawError*dT, -yawIntegralLimit, yawIntegralLimit);
			actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
			actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
		}
		else
		{
			actuatorDesired.Yaw = 0.0;
		}


		// Setup throttle
		actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);

		// Save dT
		actuatorDesired.UpdateTime = dT * 1000;

		// Write actuator desired (if not in manual mode)
		if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
		{
			ActuatorDesiredSet(&actuatorDesired);
		}
		else
		{
			pitchIntegral = 0.0;
			yawIntegral = 0.0;
			rollIntegral = 0.0;
			pitchErrorLastGlobal = 0.0;
			yawErrorLastGlobal = 0.0;
			rollErrorLast = 0.0;
		}

		// Clear alarms
		AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
	}
}