void GetEulers(float qx, float qy, float qz, float qw, float *angle1,float *angle2, float *angle3)
{
float &heading = *angle1;
float &attitude = *angle2;
float &bank = *angle3;
double test = qx*qy + qz*qw;
if (test > 0.499) // singularity at north pole
{
heading = (float) 2.0f * atan2(qx,qw);
attitude = (float) PI/2.0f;
bank = 0;
}
else if (test < -0.499) // singularity at south pole
{
heading = (float) -2.0f * atan2(qx,qw);
attitude = (float) - PI/2.0f;
bank = 0;
}
else
{
double sqx = qx*qx;
double sqy = qy*qy;
double sqz = qz*qz;
heading = (float) atan2((double)2.0*qy*qw-2.0*qx*qz , (double)1 - 2.0*sqy - 2.0*sqz);
attitude = (float)asin(2.0*test);
bank = (float) atan2((double)2.0*qx*qw-2.0*qy*qz , (double)1.0 - 2.0*sqx - 2.0*sqz);
}
heading = RadiansToDegrees(heading);
attitude = RadiansToDegrees(attitude);
bank = RadiansToDegrees(bank);
}
void Camera::lookAt(glm::vec3 position) {
assert(position != _position);
glm::vec3 direction = glm::normalize(position - _position);
_verticalAngle = RadiansToDegrees(asinf(-direction.y));
_horizontalAngle = -RadiansToDegrees(atan2f(-direction.x, -direction.z));
normalizeAngles();
}
// calculates the midpoint between two given coordinates
static void GetMidpoint(double *latitudeMidpoint, double *longitudeMidpoint, double latiudeA, double longitudeA, double latiudeB, double longitudeB)
{
double dLongitude = DegreesToRadians(longitudeB - longitudeA);
double bX = cos(DegreesToRadians(latiudeB)) * cos(dLongitude);
double bY = cos(DegreesToRadians(latiudeB)) * sin(dLongitude);
*latitudeMidpoint = RadiansToDegrees(atan2(sin(DegreesToRadians(latiudeA)) + sin(DegreesToRadians(latiudeB)), sqrt((cos(DegreesToRadians(latiudeA)) + bX) * (cos(DegreesToRadians(latiudeA)) + bX) + bY * bY)));
*longitudeMidpoint = longitudeA + RadiansToDegrees(atan2(bY, cos(DegreesToRadians(latiudeA)) + bX));
}
void * BankMinMaxLimit_Query( int column, int row, LWItemID id, LWTime time ) {
LWDVector min_vec, max_vec;
item_info->limits( id, LWIP_ROTATION, min_vec, max_vec );
if( column == 2 )
value_float = RadiansToDegrees( min_vec[ 2 ] );
else
value_float = RadiansToDegrees( max_vec[ 2 ] );
return &value_float;
}
//update body
void MyPhysics::UpdateBody(_RigidBody *body, float dtime){
Vector Ae; //accelerate
float Aa;
Vector k1;
float k1a;
float dt = dtime;
// linear velocity
Ae = body->vForces / body->fMass;
k1 = Ae*dt;
Aa = body->vMoment.z / body->fInertia;
k1a = Aa* dt;
// add the k1 terms to the respective initial velocities
// vt = vo+at
body->vVelocity += k1;
body->vAngularVelocity.z += k1a;
// update position
body->vPosition += body->vVelocity*dt;
/*body->vPosition +=test;*/
// calculate the new orientation
body->fOrientation += RadiansToDegrees(body->vAngularVelocity.z*dt);
}
void RigidBody2D::UpdateBodyEuler(double dt)
{
Vector a;
Vector dv;
Vector ds;
float aa;
float dav;
float dr;
CalcLoads();
a = vForces / fMass;
dv = a * dt;
vVelocity += dv;
ds = vVelocity * dt;
vPosition += ds;
aa = vMoment.z / fInertia;
dav = aa * dt;
vAngularVelocity.z += dav;
dr = RadiansToDegrees(vAngularVelocity.z * dt);
fOrientation += dr;
fSpeed = vVelocity.Magnitude();
vVelocityBody = VRotate2D(-fOrientation, vVelocity);
}
//-*****************************************************************************
double XformSample::getZRotation() const
{
Abc::V3d rot;
Imath::extractEulerXYZ( this->getMatrix(), rot );
return RadiansToDegrees( rot[2] );
}
void Map::addEntity (int clientMask, const IEntity& entity) const
{
const EntityAngle angle = static_cast<EntityAngle>(RadiansToDegrees(entity.getAngle()));
const AddEntityMessage msg(entity.getID(), entity.getType(), Animation::NONE,
entity.getSpriteID(), entity.getCol(), entity.getRow(), 1.0f, 1.0f, angle, ENTITY_ALIGN_UPPER_LEFT);
_serviceProvider->getNetwork().sendToClients(clientMask, msg);
}
/*
* Spotlight Cone Angle
*/
void * ShadowMapAngle_Query( int column, int row, LWItemID id, LWTime time ) {
if( item_info->type( id ) != LWI_LIGHT ) {
value_float = 0.0;
return &value_float;
}
value_float = RadiansToDegrees( light_info->shadMapAngle( id, time ) );
return &value_float;
}
double KauaibotsTarget::GetRobotVerticalAngle()
{
double yPixelPos = ((m_yPos*m_nWidth)+m_nWidth)/2;
double vertAngleRadians =
atan((((yPixelPos)-m_nHeight/2)*m_nVertPixelPitchMM)/cCameraFocalLengthMM);
return RadiansToDegrees( vertAngleRadians ) - GetVertServoAngle();
}
double KauaibotsTarget::GetRobotHorizontalAngle()
{
double xPixelPos = ((m_xPos*m_nHeight)+m_nWidth)/2;
double horizAngleRadians =
atan((((xPixelPos)-m_nWidth/2)*m_nHorizPixelPitchMM)/cCameraFocalLengthMM);
return RadiansToDegrees( horizAngleRadians ) - GetHorizServoAngle();
}
float SmartGyro::GetAngle(){
float angle = Gyro::GetAngle();
float radian = angle * PI;
switch(gyroMode){
case radiansOverPi:return angle;
case radians: return radian;
case degrees: return RadiansToDegrees(radian);
default: return angle;
}
}
void * BankMinMaxLimit_Evaluate( int column, int row, LWItemID id, LWTime time, void *value, int apply ) {
LWDVector min_vec, max_vec;
item_info->limits( id, LWIP_ROTATION, min_vec, max_vec );
value_float = *(double *)value;
if( apply ) {
char buffer[ 100 ];
sprintf( buffer, "SelectItem %x", id );
command( buffer );
if( column == 2 )
sprintf( buffer, "BLimits %g %g", value_float, RadiansToDegrees( max_vec[ 2 ] ) );
else
sprintf( buffer, "BLimits %g %g", RadiansToDegrees( min_vec[ 2 ] ), value_float );
command( buffer );
}
return &value_float;
}
//--------------------------------------------------------------------------
//
//--------------------------------------------------------------------------
void tDatFileExport::PositionToStrings( const tMCoord& position, QString& lat, QString& northSouth,
QString& lon, QString& eastWest )
{
tRCoord rCoord( position );
// Format as "DDMM.MMMM"
double degrees = RadiansToDegrees( rCoord.RLat() );
northSouth = (degrees > 0) ? QString( "N" ): QString( "S" );
degrees = qAbs( degrees );
int degreesInt = static_cast<int>( degrees );
double minutes = ( degrees - degreesInt ) * 60;
lat = QString( "%1%2" ).arg( degreesInt, 2, 10, QChar('0') ).arg( minutes, 7, 'f', 4, '0' );
// Format as DDDMM.MMMM
degrees = RadiansToDegrees( rCoord.RLon() );
eastWest = (degrees > 0) ? QString( "E" ): QString( "W" );
degrees = qAbs( degrees );
degreesInt = static_cast<int>( degrees );
minutes = ( degrees - degreesInt ) * 60;
lon = QString( "%1%2" ).arg( degreesInt, 3, 10, QChar('0') ).arg( minutes, 7, 'f', 4, '0' );
}
void tSonar3D::mouseReleaseEvent(QMouseEvent* pEvent)
{
if (m_pCameraInteractor != 0)
{
m_pCameraInteractor->HandleMouseRelease(pEvent);
}
m_CameraRotating = false;
float lastHeadingCartesianDegrees = HeadingToCartesianAngle(m_LastHeading);
double azimuthDegrees = RadiansToDegrees(m_SceneSettings.camera.Azimuth(false));
m_CameraRelativeAngleDegrees = tMathUtil::Normalize(azimuthDegrees - lastHeadingCartesianDegrees, -180.0, 180.0);
UpdateScene();
}
// ****************************************************************************
//
// Function Name: YFontInfo::operator*( )
//
// Description: Apply the transform to alter the FontInfo
// This involves scaling the height by the transform.XScale
// and changing the rotation by the transform XRotation
//
// Returns: Nothing
//
// Exceptions: Nothing
//
// ****************************************************************************
//
YFontInfo YFontInfo::operator*( const R2dTransform& transform ) const
{
YFontInfo newFont = *this;
YAngle rotation;
YRealDimension xScale;
YRealDimension yScale;
transform.Decompose( rotation, xScale, yScale );
if ( xScale < yScale )
yScale = xScale;
else
xScale = yScale;
newFont.height = ::Abs( ::Round( (YRealDimension)height * yScale ) );
newFont.width = ::Abs( ::Round( (YRealDimension)width * xScale ) );
newFont.angle += (YIntDegrees)Round( RadiansToDegrees( rotation ) );
return newFont;
}
//-*****************************************************************************
double XformOp::getZRotation() const
{
ABCA_ASSERT( m_type == kRotateOperation || m_type == kRotateZOperation,
"Meaningless to get rotation angle from non-rotation op." );
if ( m_type == kRotateZOperation )
{
return m_channels[0];
}
else
{
Abc::M44d m;
Abc::V3d rot;
m.makeIdentity();
m.setAxisAngle( this->getVector(), DegreesToRadians( m_channels[3] ) );
Imath::extractEulerXYZ( m, rot );
return RadiansToDegrees( rot[2] );
}
}
int BitmapFont::printMax (const std::string& text, const Color& color, int x, int y, int maxLength, bool rotate) const
{
if (_fontDefPtr->getHeight() < 8)
return 0;
if (text.empty())
return 0;
if (color[3] <= 0.0001f)
return 0;
const int beginX = x;
int yShift = 0;
_frontend->setColor(color);
const TextureRect sourceRect = _font->getSourceRect();
for (std::string::const_iterator i = text.begin(); i != text.end(); ++i) {
const unsigned char chr = *i;
if (chr == '\n') {
x = beginX;
yShift += _fontDefPtr->getHeight();
continue;
} else if (chr == '\t') {
x += 4 * _fontDefPtr->getFontChar(' ')->getWidth();
continue;
}
const FontChar* fontChr = _fontDefPtr->getFontChar(chr);
if (fontChr == nullptr) {
x += _fontDefPtr->getFontChar(' ')->getWidth();
continue;
}
if (maxLength <= 0 || x + fontChr->getWidth() - beginX <= maxLength) {
_font->setRect(sourceRect.x + fontChr->getX(), sourceRect.y + fontChr->getY(), fontChr->getW(), fontChr->getH());
const int letterAngleMod = x + fontChr->getOX() + y + yShift + _fontDefPtr->getHeight() - fontChr->getOY() + fontChr->getW() + fontChr->getH();
const int angle = rotate ? RadiansToDegrees(cos(static_cast<double>(letterAngleMod * 100 + _time + _rand) / 100.0) / 6.0) : 0;
_frontend->renderImage(_font.get(), x + fontChr->getOX(), y + yShift + _fontDefPtr->getHeight() - fontChr->getOY(), fontChr->getW(), fontChr->getH(), angle, color[3]);
}
x += fontChr->getWidth();
}
_font->setRect(sourceRect.x, sourceRect.y, sourceRect.w, sourceRect.h);
_frontend->resetColor();
return yShift;
}
void ClusterGrenade::Draw() const
{
glPushMatrix();
glTranslatef(GetPosition().x, GetPosition().y, 0.0f);
glRotatef(RadiansToDegrees(GetAngle()), 0, 0, 1);
glBindTexture(GL_TEXTURE_2D, m_xTexture->tex);
glBegin(GL_QUADS);
glTexCoord2i(0, 1);
glVertex2f(-m_fHalfWidth, -m_fHalfHeight);
glTexCoord2i(1, 1);
glVertex2f(m_fHalfWidth, -m_fHalfHeight);
glTexCoord2i(1, 0);
glVertex2f(m_fHalfWidth, m_fHalfHeight);
glTexCoord2i(0, 0);
glVertex2f(-m_fHalfWidth, m_fHalfHeight);
glEnd();
glPopMatrix();
}
void Map::updateEntity (int clientMask, const IEntity& entity) const
{
const EntityAngle angle = static_cast<EntityAngle>(RadiansToDegrees(entity.getAngle()));
const UpdateEntityMessage msg(entity.getID(), entity.getCol(), entity.getRow(), angle, entity.getState());
_serviceProvider->getNetwork().sendToClients(clientMask, msg);
}
const Vector4 ToAxisAngleDegrees() const
{
return Vector4(RadiansToDegrees(2 * acos(W)), X, Y, Z);
}
void DriveControl::Update()
{
m_robot->Lock();
m_robot->m_lcd->PrintfLine(DriverStationLCD::kUser_Line6, "gyro: %f deg",RadiansToDegrees(m_robot->GetGyroValue()));
//assumption stuff
m_robot->updateAssumption();
//printf("left: %f right: %f gyro: %f\n",MetersToFeet(m_robot->GetLeftDistance()),MetersToFeet(m_robot->GetRightDistance()), RadiansToDegrees(m_robot->GetGyroValue()));
//printf("assumed x: %f assumed y: %f assumed angle: %f\n",MetersToFeet(m_robot->assumed_xpos), MetersToFeet(m_robot->assumed_ypos), RadiansToDegrees(m_robot->assumed_theta));
// allow to override if the joysticks are moved
// NOTE: the control board is not updated during autonomous
// so this will not override the control loops during autonomous
if(m_robot->isControlLoopDriving && (m_robot->turnPower!=0 || m_robot->straightDrivePower!=0)) {
printf("JOYSTICKS HIT, DRIVE CONTROLS CANCELLING\n");
m_robot->straightDistanceGoal=0;
m_robot->isControlLoopDriving=false;
}
if(m_robot->resetDriveControl) {
Reset();
m_robot->resetDriveControl = false;
}
if(m_robot->isControlLoopDriving)
{
m_straightFilter->CalcSystem(m_robot->straightDistanceGoal - m_currentX, m_currentV, m_robot->straightDistanceGoalVelocity, m_robot->straightDistanceMaxAcceleration, m_robot->straightDistanceMaxVelocity, m_robot->dt);
m_currentA=m_straightFilter->GetCurrAcc();
m_currentV=m_straightFilter->GetCurrVel();
m_currentX=m_straightFilter->GetCurrPos();
m_turnFilter->CalcSystem(m_robot->turnAngleGoal - m_filtered_angle, m_currentAngularV, m_robot->turnAngleGoalVelocity, 8 , M_PI * 2.0 / 3.0, m_robot->dt);
m_currentAngularA = m_turnFilter->GetCurrAcc();
m_currentAngularV = m_turnFilter->GetCurrVel();
m_filtered_angle = m_turnFilter->GetCurrPos();
static const double robotWidth = 0.61799;
double theta_gyro = m_robot->GetGyroValue();
double theta_measured = (m_robot->GetRightDistance()-m_robot->GetLeftDistance())/robotWidth;
double kI=0.017;
//double kI=0.015;
// If we are at the goal (theoretically...), add in an integral.
// Kill this integral as soon as we try to move to not change the real goal.
// When the robot turns, the extra offset won't really help.
// But kill it gradually to not upset the controller and cause it to pulse the angle.
// Enable the I term when we are close.
if (fabs(m_robot->turnAngleGoal - m_filtered_angle) < 0.0001
&& fabs(RadiansToDegrees((m_robot->turnAngleGoal + m_robot->turnOffset) - theta_measured)) < 18.0) {
double KiTurn;
// If the arm is down, it's harder to turn... Not sure why.
// This might be componded by something else in the turn to grab the tube that was causing it to be slow.
//if (m_robot->armGoal < 0.0) {
//KiTurn = 0.0254;
//KiTurn = 0.040;
//} else {
//KiTurn = 0.0230;
KiTurn = 0.0254;
//KiTurn = 0.0200;
//}
m_sumStoppedError += ((m_robot->turnAngleGoal + m_robot->turnOffset) - theta_measured) * KiTurn;
} else {
// Derate the integral if we are turning so it doesn't take effect any more.
// Gradually so it doesn't cause the bot to rapidly turn.
if (!(fabs(m_robot->turnAngleGoal - m_filtered_angle) < 0.0001)) {
m_sumStoppedError *= 0.97;
}
}
// Limit the change in the offset to 0.01 rad / 100 of a second.
// This will prevent the -big gyro bug from showing up.
// I'm seeing a very large gyro value occasionally which is messing up
// the offset and not letting it recover for a while. Not good.
double doffset = ((theta_measured-m_robot->turnOffset)-theta_gyro)*kI;
if (doffset > 0.01) {
doffset = 0.01;
} else if (doffset < -0.01) {
doffset = -0.01;
}
m_robot->turnOffset += doffset;
static int i=0;
static bool printing=false;
if(printing && i%1==0) {
printf("left: %f right: %f gyro: %f\n",m_robot->GetLeftDistance(),m_robot->GetRightDistance(),m_robot->GetGyroValue());
printf("offset: %f stoppederror: %f\n",m_robot->turnOffset,m_sumStoppedError);
printf("error: %f ",(theta_measured-m_robot->turnOffset)-theta_gyro);
//printf("offset: %f ",m_robot->turnOffset);
//printf("ioffset: %f ",m_sumStoppedError);
printf("measured: %f ",theta_measured);
printf("gyro: %f ",theta_gyro);
//printf("drive err %f ",m_robot->straightDistanceGoal - (m_robot->GetLeftDistance()+m_robot->GetRightDistance())/2);
printf("angle err %f ",(m_robot->turnAngleGoal + m_robot->turnOffset) - theta_measured);
//printf("angle goal: %f ",RadiansToDegrees(m_robot->turnAngleGoal));
printf("\n");
}
i++;
double angleFactor = m_filtered_angle*robotWidth/2;
double angularVelocityFactor = m_currentAngularV*robotWidth/2;
//log stuff
if(m_robot->isAutonomous && !m_robot->isDisabled) {
positionlogger << m_robot->GetTime() << ", "; //1
positionlogger << m_robot->assumed_xpos << ", "; //2
positionlogger << m_robot->assumed_ypos << ", "; //3
positionlogger << theta_gyro << ", "; //4
positionlogger << m_robot->turnOffset << ", "; //5
positionlogger << m_robot->GetLeftDistance() << ", "; //6
positionlogger << m_robot->GetRightDistance() << ", "; //7
positionlogger << (theta_measured-m_robot->turnOffset)-theta_gyro << ", "; //8
positionlogger << (theta_measured-m_robot->turnOffset) << ", "; //9
positionlogger << m_currentX-(m_robot->turnOffset + m_sumStoppedError)*robotWidth/2.0-angleFactor << ", "; //10
positionlogger << m_currentX+(m_robot->turnOffset + m_sumStoppedError)*robotWidth/2.0+angleFactor << ", "; //11
positionlogger << m_sumStoppedError << endl; //12
}
//setup the output matrix
struct matrix* outputs;
outputs = init_matrix(num_outputs, 1);
flash_matrix(m_r,m_currentX-(m_robot->turnOffset + m_sumStoppedError)/2.0-angleFactor,m_currentV-angularVelocityFactor,m_currentX+(m_robot->turnOffset + m_sumStoppedError)/2.0+angleFactor,m_currentV+angularVelocityFactor);
flash_matrix(m_y,m_robot->GetLeftDistance(),m_robot->GetRightDistance());
m_ssc.update(outputs,m_r,m_y);
if (m_robot->ignoreTurnControlLoop) {
outputs->data[0] = outputs->data[1] = (outputs->data[0] + outputs->data[1]) / 2.0;
}
if (maximum(fabs(outputs->data[0]), fabs(outputs->data[1])) > 12.0) {
//printf("scaling\n");
//double turnPower = outputs->data[0] - outputs->data[1];
//double drivePower = outputs->data[0] + outputs->data[1];
double scaleFactor = 12.0 / maximum(fabs(outputs->data[0]), fabs(outputs->data[1]));
/*
if (fabs(turnPower) < 0.5 * fabs(drivePower)) {
double deltaTurn = turnPower / 2.0 / scaleFactor * 0.4;
outputs->data[0] += deltaTurn;
outputs->data[1] -= deltaTurn;
scaleFactor = 12.0 / maximum(fabs(outputs->data[0]), fabs(outputs->data[1]));
} else if (0.5 * fabs(turnPower) > fabs(drivePower)) {
double deltaDrive = turnPower / 2.0 / scaleFactor * 0.4;
outputs->data[0] += deltaDrive;
outputs->data[1] += deltaDrive;
scaleFactor = 12.0 / maximum(fabs(outputs->data[0]), fabs(outputs->data[1]));
}*/
outputs->data[0] *= scaleFactor;
outputs->data[1] *= scaleFactor;
}
double outl=outputs->data[0]/12;
double outr=outputs->data[1]/12;
if(fabs(outl)>1.0)
outl/=fabs(outl);
if(fabs(outr)>1.0)
outr/=fabs(outr);
if(printing && i%1==0) {
printf("left pwm: %f right pwm: %f\n",outl,outr);
}
m_robot->SetLeftMotor_Linearized(outl);
m_robot->SetRightMotor_Linearized(outr);
free_matrix(outputs);
} else {
double throttle = -m_robot->straightDrivePower;
double wheel = m_robot->turnPower;
bool isQuickTurn = m_robot->isQuickTurn;
bool isHighGear = m_robot->isHighGear;
//printf("Drive Distance ld %f rd %f\n", m_robot->GetLeftDistance(), m_robot->GetRightDistance());
double wheelNonLinearity;
double neg_inertia = wheel - m_old_wheel;
m_old_wheel = wheel;
//triple sine wave ftw!
if (isHighGear) {
wheelNonLinearity = m_csvReader->GetValueWithDefault("TURN_NONLIN_HIGH", 0.1);
// Apply a sin function that's scaled to make it feel better.
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
} else {
wheelNonLinearity = m_csvReader->GetValueWithDefault("TURN_NONLIN_LOW", 0.1);
// Apply a sin function that's scaled to make it feel better.
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
}
double left_pwm, right_pwm, overPower;
float sensitivity = 1.7;
float angular_power;
float linear_power;
//negative inertia!
static double neg_inertia_accumulator = 0.0;
double neg_inertia_scalar;
if (isHighGear) {
neg_inertia_scalar = m_csvReader->GetValueWithDefault("NEG_INERTIA_HIGH", 0.0);
sensitivity = m_csvReader->GetValueWithDefault("SENSE_HIGH", 1.7);
} else {
if (wheel * neg_inertia > 0) {
neg_inertia_scalar = m_csvReader->GetValueWithDefault("NEG_INERTIA_LOW_MORE", 0.0);
} else {
if (fabs(wheel) > 0.65) {
neg_inertia_scalar = m_csvReader->GetValueWithDefault("NEG_INERTIA_LOW_LESS_EXT", 0.0);
} else {
neg_inertia_scalar = m_csvReader->GetValueWithDefault("NEG_INERTIA_LOW_LESS", 0.0);
}
}
sensitivity = m_csvReader->GetValueWithDefault("SENSE_LOW", 1.2);
if (fabs(throttle) > m_csvReader->GetValueWithDefault("SENSE_CUTTOFF", 0.1)) {
sensitivity = 1 - (1 - sensitivity) / fabs(throttle);
}
}
double neg_inertia_power=neg_inertia * neg_inertia_scalar;
neg_inertia_accumulator+=neg_inertia_power;
wheel = wheel + neg_inertia_accumulator;
if(neg_inertia_accumulator>1)
neg_inertia_accumulator-=1;
else if (neg_inertia_accumulator<-1)
neg_inertia_accumulator+=1;
else
neg_inertia_accumulator=0;
linear_power = throttle;
//quickturn!
if (isQuickTurn) {
overPower = 1.0;
if (isHighGear) {
sensitivity = 1.0;
} else {
sensitivity = 1.0;
}
angular_power = wheel;
} else {
overPower = 0.0;
angular_power = fabs(throttle) * wheel * sensitivity;
}
right_pwm = left_pwm = linear_power;
left_pwm += angular_power;
right_pwm -= angular_power;
if (left_pwm > 1.0) {
right_pwm -= overPower*(left_pwm - 1.0);
left_pwm = 1.0;
} else if (right_pwm > 1.0) {
left_pwm -= overPower*(right_pwm - 1.0);
right_pwm = 1.0;
} else if (left_pwm < -1.0) {
right_pwm += overPower*(-1.0 - left_pwm);
left_pwm = -1.0;
} else if (right_pwm < -1.0) {
left_pwm += overPower*(-1.0 - right_pwm);
right_pwm = -1.0;
}
//printf("left pwm: %f right pwm: %f\n",left_pwm,right_pwm);
//printf("left wheel: %f right wheel: %f\n",m_robot->GetLeftDistance(),m_robot->GetRightDistance());
m_robot->SetLeftMotor_Linearized(left_pwm);
m_robot->SetRightMotor_Linearized(right_pwm);
m_robot->SetHighGear(isHighGear);
}
m_robot->Unlock();
}
std::ostream &operator<<(std::ostream &lhs, const Quaternion &rhs)
{
float angle;
Vector3 axis;
rhs.ToAngleAxis(angle, axis);
lhs << "Quaternion(" << rhs.x << ", " << rhs.y << ", " << rhs.z << ", " << rhs.w << "; " << RadiansToDegrees(angle) << " " << axis << ")";
return lhs;
}
//-*****************************************************************************
double XformSample::getAngle() const
{
Imath::Quatd q = Imath::extractQuat( this->getMatrix() );
return RadiansToDegrees( q.angle() );
}
/*
* Spotlight Cone Angle
*/
void * SpotlightConeAngle_Query( int column, int row, LWItemID id, LWTime time ) {
double edge;
light_info->coneAngles( id, time, &value_float, &edge );
value_float = RadiansToDegrees( value_float );
return &value_float;
}
float cQuaternion::GetAngleDegrees() const
{
return RadiansToDegrees(GetAngleRadians());
}
void Render()
{
GLfloat m[9];
GLfloat v[3];
float fRadius = 5.0f;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear buffers
glLoadIdentity(); // Load identity matrix
GLfloat glfLight[] = {-4.0f, 4.0f, 4.0f, 0.0f};
glLightfv(GL_LIGHT0, GL_POSITION, glfLight);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glPushMatrix();
// draw timecode
glPushMatrix();
glTranslatef(2400.f, -1750.f, -5000.0f);
glPrint(0.0f,0.0f,szTimecode);
glPopMatrix();
// Position and rotate the camera
#if 1
glTranslatef(0.0f, -1000.0f, -5000.0f);
#else
// ori
float quat[4];
quat[0] = rigidbody[3];
quat[1] = rigidbody[4];
quat[2] = rigidbody[5];
quat[3] = rigidbody[6];
float yaw, pitch, roll;
GetEulers(quat[0], quat[1], quat[2], quat[3], &yaw, &pitch, &roll);
glRotatef(-roll, 1.0f, 0.0f, 0.0f);
glRotatef(-yaw, 0.0f, 1.0f, 0.0f);
glRotatef(-pitch, 0.0f, 0.0f, 1.0f);
/*
EulerAngles eulers = Eul_FromQuat(q, order);
eulers.x = RadiansToDegrees(eulers.x);
eulers.y = RadiansToDegrees(eulers.y);
eulers.z = RadiansToDegrees(eulers.z);
*/
// pos
float x = rigidbody[0];
float y = rigidbody[1];
float z = rigidbody[2];
x *= g_unitConversion; // convert to mm
y *= g_unitConversion;
z *= g_unitConversion;
//glTranslatef(0.0f, -100.0f, -5000.0f); // origin offset
glTranslatef(-x, -y, -z);
#endif
//draw axis
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3f(0,0,0);
glVertex3f(300,0,0);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3f(0,0,0);
glVertex3f(0,300,0);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3f(0,0,0);
glVertex3f(0,0,300);
glEnd();
// draw grid
DrawGrid();
int i;
#if 1
// draw rigid bodies
for (i=0;i<rigidbodyCount;i++)
{
int index = 7*i; // a rigid body pos/ori consists of 7 values : [x,y,x,qx,qy,qz,qw]
GLfloat x = rigidbody[index], y = rigidbody[index+1], z = rigidbody[index+2];
x *= g_unitConversion; // convert to mm
y *= g_unitConversion;
z *= g_unitConversion;
GLfloat qx = rigidbody[index+3], qy = rigidbody[index+4], qz = rigidbody[index+5], qw = rigidbody[index+6];
glColor4f(0.0f,0.0f,1.0f,1.0f);
drawbox(x,y,z,qx,qy,qz,qw);
glColor4f(0.0f,0.0f,0.0f,1.0f);
// Convert quaternion to eulers. Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
Quat q;
q.x = qx; q.y = qy; q.z = qz; q.w = qw;
int order = EulOrdXYZr;
EulerAngles ea = Eul_FromQuat(q, order);
ea.x = RadiansToDegrees(ea.x);
ea.y = RadiansToDegrees(ea.y);
ea.z = RadiansToDegrees(ea.z);
float angle1, angle2, angle3;
GetEulers(qx, qy, qz, qw, &angle1, &angle2, &angle3);
glPrint(x,y,"RB %d (yaw:%3.1f, pitch:%3.1f, roll:%3.1f) (yaw:%3.1f, pitch:%3.1f, roll:%3.1f)", rigidbodyids[i], ea.x, ea.y, ea.z, angle1, angle2, angle3 );
}
#endif
#if 1
// draw markers
// [optional] local coordinate support : get first rb's pos/ori (face support only- assume 1st is root)
if(rigidbodyCount==1)
{
GLfloat q[4] = {rigidbody[3], rigidbody[4], rigidbody[5], rigidbody[6]};
quatToMatrix(q, m);
}
for (i=0;i<markerCount;i++)
{
int index = 3*i;
v[0] = markerPos[index];
v[1] = markerPos[index+1];
v[2] = markerPos[index+2];
// [optional] local coordinate support : inherit (accumulate) parent's RB pos/ori ("root") if using local marker position
if(rigidbodyCount==1)
{
vec3MatrixMult(v,m);
v[0] += rigidbody[0];
v[1] += rigidbody[1];
v[2] += rigidbody[2];
}
glPushMatrix();
glTranslatef(v[0],v[1],v[2]);
drawsphere(1,fRadius);
glPopMatrix();
}
#endif
#if 1 //draw labeled markers
for (i=0;i<nLabeledMarkers;i++)
{
v[0] = labeledMarkers[i].x * g_unitConversion;
v[1] = labeledMarkers[i].y * g_unitConversion;
v[2] = labeledMarkers[i].z * g_unitConversion;
fRadius = labeledMarkers[i].size * g_unitConversion;
glPushMatrix();
glTranslatef(v[0],v[1],v[2]);
glColor4f(1.0f,0.0f,0.0f,1.0f);
drawsphere(1,fRadius);
/*
glPrint(v[0],v[1], "Marker: %d (%3.1f, %3.1f, %3.1f)",
labeledMarkers[i].ID, labeledMarkers[i].x,labeledMarkers[i].y,labeledMarkers[i].z );
*/
glPopMatrix();
}
#endif
glPopMatrix();
glFlush();
render = false;
}
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
void tNavigationPage::paint( QPainter* pPainter, const QStyleOptionGraphicsItem* pOption, QWidget* pWidget )
{
Q_UNUSED( pOption );
Q_UNUSED( pWidget );
//Draw table
QList<tDataType> displayDataTypes;
//TODO: need to do FTW
displayDataTypes << DATA_TYPE_COG << DATA_TYPE_SPEED_SOG << DATA_TYPE_BEARING << DATA_TYPE_DISTANCE_TO_TURN << DATA_TYPE_FUEL_TO_TURN;
QRect tableRect = AvailableRect();
//tableRect.setBottom(tableRect.bottom()-50);
tableRect.setLeft(tableRect.left()+90);
tableRect.setWidth(161);
tableRect.setHeight(142);
pPainter->setPen( QPen( Qt::black, 2.0, Qt::SolidLine, Qt::RoundCap ) );
//draw left and top borders
pPainter->drawLine(tableRect.topLeft(), tableRect.topRight());
pPainter->drawLine(tableRect.topLeft(), tableRect.bottomLeft());
static int lineHeight = 28;
int lineOffset = 0;
for (QList<tDataType>::iterator dataTypeIter = displayDataTypes.begin(); dataTypeIter != displayDataTypes.end(); ++dataTypeIter)
{
//Text
QString currentLabel = tDigitalData::Caption( *dataTypeIter, true );
QRect textRect = tableRect;
textRect.setTop(tableRect.top() + lineOffset);
textRect.setLeft(tableRect.left() + 1);
textRect.setHeight(lineHeight);
textRect.setWidth(52);
//pPainter->setPen( QPen( QColor(128,128,128) ) );
//pPainter->setBrush(Qt::red);
pPainter->fillRect(textRect, QBrush(QColor(187,187,187)));
pPainter->drawText( textRect, Qt::AlignCenter, currentLabel );
//Value
QFont currentFont = pPainter->font();
currentFont.setBold(true);
pPainter->setFont(currentFont);
QRect valueRect = textRect;
valueRect.setLeft( textRect.left()+textRect.width() );
valueRect.setWidth(61);
//TODO: use a timer to only get this when required
QString valueText("-");
float navValue;
bool navValueValid = tDigitalData(tDataId( *dataTypeIter )).ValidValue( navValue );
if ( navValueValid )
{
valueText = QString("%1").arg(navValue);
}
pPainter->drawText( valueRect, Qt::AlignVCenter | Qt::AlignRight, valueText );
currentFont.setBold(false);
pPainter->setFont(currentFont);
//Units
if (navValueValid)
{
QRect unitsRect = valueRect;
unitsRect.setLeft( valueRect.left()+valueRect.width() + 5 );
unitsRect.setWidth(47);
pPainter->drawText( unitsRect, Qt::AlignVCenter | Qt::AlignLeft, tDigitalData(tDataId( *dataTypeIter )).UnitStr() );
}
//Separator
int rowY = tableRect.y() + lineOffset;
QLine rowSeparator(tableRect.x(), rowY, tableRect.right(), rowY );
pPainter->drawLine(rowSeparator);
lineOffset += lineHeight;
//Get position data
tDigitalData position( (tDataId( DATA_TYPE_POSITION )) );
tRCoord posValue(0,0);
bool posValid = position.ValidValue( posValue );
QString latitudeValue("-");
QString longitudeValue("-");
//tCoordinate posCoord = tCoordinate( posValue );
if (posValid)
{
//TODO: find out if this logic exists in a common place
double dblDeg;
int deg;
double min;
char hem;
dblDeg = RadiansToDegrees( posValue.RLat() );
hem = dblDeg < 0 ? 'S' : 'N';
dblDeg = qAbs(dblDeg);
deg = (int)dblDeg;
min = 60 * (dblDeg - deg);
//latitudeValue = QString("%1:%2'%3").arg(deg).arg(min).arg(hem);
latitudeValue = QString("%1 %2").arg( posValue.RLat()).arg(hem);
dblDeg = RadiansToDegrees( posValue.RLon() );
hem = dblDeg < 0 ? 'W' : 'E';
dblDeg = qAbs(dblDeg);
deg = (int)dblDeg;
min = 60 * (dblDeg - deg);
//longitudeValue = QString("%1:%2'%3").arg(deg).arg(min).arg(hem);
longitudeValue = QString("%1 %2").arg(posValue.RLon()).arg(hem);
}
//Lat/Lon display
QRect latTextRect = AvailableRect();
latTextRect.setTop(tableRect.bottom()+10);
latTextRect.setWidth(latTextRect.width()/2);
latTextRect.setHeight(25);
pPainter->drawText( latTextRect, Qt::AlignCenter, QString("Latitude").toUpper() ); //TODO: look at translation?
QRect latValueRect = latTextRect;
latValueRect.setTop(latTextRect.bottom());
latValueRect.setLeft(latTextRect.left()+4);
latValueRect.setWidth(latTextRect.width()-8);
latValueRect.setHeight(latTextRect.height());
pPainter->drawRect(latValueRect);
pPainter->drawText( latValueRect, Qt::AlignCenter, latitudeValue ); //TODO: look at translation?
QRect lonTextRect = latTextRect;
lonTextRect.setLeft(latTextRect.right());
lonTextRect.setTop(latTextRect.top());
lonTextRect.setWidth(latTextRect.width());
pPainter->drawText( lonTextRect, Qt::AlignCenter, QString("Longitude").toUpper() ); //TODO: look at translation?
QRect lonValueRect = lonTextRect;
lonValueRect.setTop(lonTextRect.bottom());
lonValueRect.setLeft(lonTextRect.left()+4);
lonValueRect.setWidth(lonTextRect.width()-8);
lonValueRect.setHeight(lonTextRect.height());
pPainter->drawRect(lonValueRect);
pPainter->drawText( lonValueRect, Qt::AlignCenter, longitudeValue ); //TODO: look at translation?
}
//draw bottom border
pPainter->drawLine(tableRect.bottomLeft(), tableRect.bottomRight());
}
float AngleDegFromVector2D(const Vector2D &vector)
{
return RadiansToDegrees(AngleRadFromVector2D(vector));
}
