Пример #1
0
//Solo Minutos de la latitud/longitud actual
float GPS_5Hz::getMinutes(boolean axis)
{
	float min = 0.0;
	if(axis == LATITUDE) min = latitude - getDegrees(LATITUDE) * 100;
	else if(axis == LONGITUDE) min = longitude - getDegrees(LONGITUDE) * 100;
	return min;
}
Пример #2
0
//Arma una URL para Google Maps con la ubicación actual leída por el GPS (CREDITO POR DESCIFRAR EL URL DE DANIEL ROJAS :D)
void GPS_5Hz::printURL()
{
    Serial.print("https://www.google.com/maps/preview#!q=");
    Serial.print(getDegrees(LATITUDE));
    Serial.print("%C2%B0+");
    Serial.print(getMinutes(LATITUDE), 4);
    Serial.print("'%2C+-");
    Serial.print(getDegrees(LONGITUDE));
    Serial.print("%C2%B0+");
    Serial.print(getMinutes(LONGITUDE), 4);
    Serial.println("'");
}
Пример #3
0
//latitud en el formato especificado por notationType(boolean)
float GPS_5Hz::getLatitude()
{
	float latitud = 0;
	if(notation == MINUTES) latitud = latitude;
	else if(notation == DECIMALS) latitud = float(getDegrees(LATITUDE))*100 + getMinutes(LATITUDE)*100/60;
	return latitud;
}
Пример #4
0
//longitud en el formato especificado por notationType(boolean)
float GPS_5Hz::getLongitude()
{
	float longitud = 0.0;
	if(notation == MINUTES) longitud = longitude;
	else if(notation == DECIMALS) longitud = float(getDegrees(LONGITUDE))*100 + getMinutes(LONGITUDE)*100/60;
	return longitud;
}
bool transformation2D::isBehindRadar(double Xcor, double Ycor)
{
    double considered_angle = rotAngle;
    double tang = tan(considered_angle);

    considered_angle = getDegrees(considered_angle);

    if(considered_angle<0.0) considered_angle *= (-1.0);

    if((considered_angle>(180.0-1.0) && considered_angle<(180.0+1.0)) ||
            (considered_angle>(90.0-1.0) && considered_angle<(90.0+1.0)) ||
            (considered_angle>(0.0-1.0) && considered_angle<(0.0+1.0)) ||
            (considered_angle>(270.0-1.0) && considered_angle<(270.0+1.0)))
    { return false; } // because it is impossible to see nothing according to our scene

    double pos = tang*(Xcor-A1) + A2;


    if(considered_angle>90.0 && considered_angle<270.0)
    {
        if(Ycor>pos) return true;
        else return false;
    }
    if(Ycor<pos) return true;
    return false;
}
Пример #6
0
double ossimUnitConversionTool::getSeconds()const
{
   if(theUnitType == OSSIM_SECONDS)
   {
      return theValue;
   }
   return (getDegrees()*3600.0);
}
Пример #7
0
double ossimUnitConversionTool::getMinutes()const
{
   if(theUnitType == OSSIM_MINUTES)
   {
      return theValue;
   }
   return (getDegrees()*60.0);
}
Пример #8
0
double ossimUnitConversionTool::getRadians()const
{
   if(theUnitType == OSSIM_RADIANS)
   {
      return theValue;
   }
   return getDegrees()*RAD_PER_DEG;
}
Пример #9
0
Angle &Angle::clampDegrees(float mag) {
    _degrees = getDegrees(-180.f);
    if (_degrees >= mag)
        setDegrees(mag);
    if (_degrees <= -mag)
        setDegrees(-mag);

    return *this;
}
// This is the actual task that is run
static portTASK_FUNCTION( vMotorControlTask, pvParameters )
{
    // Get the parameters
    param = (motorControlStruct *) pvParameters;
    // Get the I2C task pointer
    i2cData = param->i2cData;
    // Get the Navigation task pointer
    webData = param->webData;
    // Get the LCD task pointer
    lcdData = param->lcdData;

    prevSpeedVal = MOTOR_FORWARD_SPEED;
    curSpeedVal = MOTOR_FORWARD_SPEED;

    currentOp = NONE;
    lastOp = NONE;
    rightEncoderCount = 0;
    leftEncoderCount = 0;
    currentTime = 0;
    speedDiff = 0;
    forward = 0;
    reverse = 0;
    right = 0;
    left = 0;

    msg[0] = 'f';
    msg[1] = ' ';
    msg[4] = 'b';
    msg[5] = ' ';
    msg[8] = 'r';
    msg[9] = ' ';
    msg[13] = 'l';
    msg[14] = ' ';
    msg[18] = ' ';
    msg[19] = 0;

    // Like all good tasks, this should never exit
    for(;;)
    {
        // Wait for a message from the I2C (Encoder data) or from the Navigation Task (motor command)
        if (xQueueReceive(param->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
            VT_HANDLE_FATAL_ERROR(Q_RECV_ERROR);
        }
		//sendi2cMotorMsg(i2cData, '!', 1, portMAX_DELAY);
       switch(getMsgType(&msgBuffer)){
			case motorTimerMsgType:
			{
               currentTime++;
               if(currentOp != NONE)
               {
                   if(currentTime >= targetVal)
                   {
                       sendi2cMotorMsg(i2cData,MOTOR_STOP_SPEED + RIGHT_MOTOR_OFFSET,MOTOR_STOP_SPEED, portMAX_DELAY);
                       currentOp = NONE;
                       currentTime = 0;
                   }
               }
               if(currentTime%10 == 0){
                   switch(lastOp){
                       case FORWARD:
                       {
                           forward = forward + getCentimeters(rightEncoderCount, leftEncoderCount);
                           //updateMoveForwardMsg(navData,getCentimeters(rightEncoderCount, leftEncoderCount));
                           break;
                       }
                       case REVERSE:
                       {
                           reverse = reverse + getCentimeters(rightEncoderCount, leftEncoderCount);
                           //updateMoveBackwardMsg(navData,getCentimeters(rightEncoderCount, leftEncoderCount));
                           break;
                       }
                       case RIGHT:
                       {
                           right = right + getDegrees(rightEncoderCount, leftEncoderCount);
                           //updateRotateClockwiseMsg(navData,getDegrees(rightEncoderCount, leftEncoderCount));
                           break;
                       }
                       case LEFT:
                       {
                           left = left + getDegrees(rightEncoderCount, leftEncoderCount);  //This is exactly right... Because difference will be negative... Think this out later.
                           //updateRotateCounterclockwiseMsg(navData,getDegrees(rightEncoderCount, leftEncoderCount));
                           break;
                       }
                       default:
                       {
                           break;
                       }
                   }
                   rightEncoderCount = 0;
                   leftEncoderCount = 0;
                   #ifdef SEND_COUNTS_TO_LCD
                   msg[2] = (forward / 10) % 10 + 48;
                   msg[3] = forward % 10 + 48;
                   msg[6] = (reverse / 10) % 10 + 48;
                   msg[7] =  reverse % 10 + 48;
                   msg[10] = (right / 100) % 10 + 48;
                   msg[11] = (right / 10) % 10 + 48;
                   msg[12] = right % 10 + 48;
                   msg[15] = (left / 100) % 10 + 48;
                   msg[16] = (left / 10) % 10 + 48;
                   msg[17] = left % 10 + 48;
                   //SendLCDPrintMsg(lcdData, 20, msg);
                   #endif
               }
				break;
			}
           case setDirForwardMsgType:
           {
               currentOp = FORWARD;
               lastOp = FORWARD;
               currentTime = 0;
               leftEncoderCount = 0;
               rightEncoderCount = 0;
               //targetVal = getTargetVal(&msgBuffer)*TIMER_COUNTS_PER_CENTIMETER;
               sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
               break;
           }
			case setDirReverseMsgType:
           {
               currentOp = REVERSE;
               lastOp = REVERSE;
               currentTime = 0;
               leftEncoderCount = 0;
               rightEncoderCount = 0;
               //targetVal = getTargetVal(&msgBuffer)*TIMER_COUNTS_PER_CENTIMETER;
               sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
               break;
           }
           case setMotorSpeedMsgType:
           {
               speedDiff = curSpeedVal - getNewSpeed(&msgBuffer);
               prevSpeedVal = curSpeedVal;
               curSpeedVal = curSpeedVal + speedDiff;
               break;
           }
           case turnRightMsgType:
           {
               currentOp = RIGHT;
               lastOp = RIGHT;
               currentTime = 0;
               leftEncoderCount = 0;
               rightEncoderCount = 0;
               //targetVal = getTargetVal(&msgBuffer)/DEGREES_PER_TIMER_COUNT;
               sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
               break;
           }
           case turnLeftMsgType:
           {
               currentOp = LEFT;
               lastOp = LEFT;
               currentTime = 0;
               leftEncoderCount = 0;
               rightEncoderCount = 0;
               //targetVal = getTargetVal(&msgBuffer)/DEGREES_PER_TIMER_COUNT;
               sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
               break;
           }
           case motorStopMsgType:
           {
               currentOp = NONE;
               //prevSpeedVal = MOTOR_STOP_SPEED;        // This is undoubtedly incorrect so look at this Matt!!
               //curSpeedVal = MOTOR_STOP_SPEED;         // This is undoubtedly incorrect so look at this Matt!!
               sendi2cMotorMsg(i2cData, MOTOR_STOP_SPEED + RIGHT_MOTOR_OFFSET, MOTOR_STOP_SPEED, portMAX_DELAY);
               currentTime = 0;
               break;
           }
           case encoderDataMsgType:
           {
               rightEncoderCount += getRightCount(&msgBuffer);
               leftEncoderCount += getLeftCount(&msgBuffer);

               // char msg[12];
               // msg[0] = 'L';
               // msg[1] = ':';
               // msg[2] = ' ';
               // msg[3] = (getLeftCount(&msgBuffer) / 10) % 10 + 48;
               // msg[4] = getLeftCount(&msgBuffer) % 10 + 48;
               // msg[5] = ' ';
               // msg[6] = 'R';
               // msg[7] = ':';
               // msg[8] = ' ';
               // msg[9] = (getRightCount(&msgBuffer) / 10) % 10 + 48;
               // msg[10] = getRightCount(&msgBuffer) % 10 + 48;
               // msg[11] = 0;
               // SendLCDPrintMsg(lcdData, 11, msg);
               break;
           }
           default:
           {
               VT_HANDLE_FATAL_ERROR(UNKNOWN_MOTOR_CONTROL_MSG_TYPE);
               break;
           }
       }
    }
}
Пример #11
0
float Angle::getRadians(float low) const {
	float d = getDegrees(low);
	return degreeToRadian(d);
}
Пример #12
0
float Angle::getRadians() const {
	return degreeToRadian(getDegrees());
}
Пример #13
0
Angle &Angle::normalize(float low) {
	_degrees = getDegrees(low);

	return *this;
}
Пример #14
0
float Angle::getRadians(float low) const {
    float d = getDegrees(low);
    return Common::deg2rad(d);
}
Пример #15
0
float Angle::getRadians() const {
    return Common::deg2rad(getDegrees());
}
Пример #16
0
/**
* Constructor. Set the initial heading whenever the program starts.
*/
Compass::Compass(const char CompassPort1, const char CompassPort2)
{
	_initDegrees = getDegrees();
}
Пример #17
0
double ossimUnitConversionTool::getValue(ossimUnitType unitType) const
{
   switch(unitType)
   {
      case OSSIM_METERS:
      {
         return getMeters();
      }
      case OSSIM_RADIANS:
      {
         return getRadians();
      }
      case OSSIM_DEGREES:
      {
         return getDegrees();
      }
      case OSSIM_US_SURVEY_FEET:
      {
         return getUsSurveyFeet();
      }
      case OSSIM_FEET:
      {
         return getFeet();
      }
      case OSSIM_SECONDS:
      {
         return getSeconds();
      }
      case OSSIM_MINUTES:
      {
         return getMinutes();
      }
      case OSSIM_NAUTICAL_MILES:
      {
         return getNauticalMiles();
      }
      case OSSIM_MILES:
      {
         return getMiles();
      }
      case OSSIM_MICRONS:
      {
         return getMicrons();
      }
      case OSSIM_CENTIMETERS:
      {
         return getCentimeters();
      }
      case OSSIM_MILLIMETERS:
      {
         return getMillimeters();
      }
      case OSSIM_YARDS:
      {
         return getYards();
      }
      case OSSIM_INCHES:
      {
         return getInches();
      }
      case OSSIM_KILOMETERS:
      {
         return getKilometers();
      }
      default:
         break;
   }

   return ossim::nan();
}
Пример #18
0
double Angle::getDegreesZeroTo360() const {
    return getDegrees(true);
}
Пример #19
0
double Angle::getDegreesNotBounded() const {
    return getDegrees(false);
}