//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;
}
//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("'");
}
//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;
}
//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;
}
double ossimUnitConversionTool::getSeconds()const
{
if(theUnitType == OSSIM_SECONDS)
{
return theValue;
}
return (getDegrees()*3600.0);
}
double ossimUnitConversionTool::getMinutes()const
{
if(theUnitType == OSSIM_MINUTES)
{
return theValue;
}
return (getDegrees()*60.0);
}
double ossimUnitConversionTool::getRadians()const
{
if(theUnitType == OSSIM_RADIANS)
{
return theValue;
}
return getDegrees()*RAD_PER_DEG;
}
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;
}
}
}
}
float Angle::getRadians(float low) const {
float d = getDegrees(low);
return degreeToRadian(d);
}
float Angle::getRadians() const {
return degreeToRadian(getDegrees());
}
Angle &Angle::normalize(float low) {
_degrees = getDegrees(low);
return *this;
}
float Angle::getRadians(float low) const {
float d = getDegrees(low);
return Common::deg2rad(d);
}
float Angle::getRadians() const {
return Common::deg2rad(getDegrees());
}
/**
* Constructor. Set the initial heading whenever the program starts.
*/
Compass::Compass(const char CompassPort1, const char CompassPort2)
{
_initDegrees = getDegrees();
}
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();
}
double Angle::getDegreesZeroTo360() const {
return getDegrees(true);
}
double Angle::getDegreesNotBounded() const {
return getDegrees(false);
}
