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);
}
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;
}
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;
}
//=====================================================
///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");
}
}
/**
* 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;
}
//===================================================================================
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;
}
/**
* 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);
}
//==============================================
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;
}
}
}
}
//===============================================================================
//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));
}
/**
* 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);
}
}