inline Angle::Angle (double a ,bool israd)
{
if(israd)
setAngle (a);
else
setAngle(a*DEGREE2RADIAN_CONSTANT);
}
void shootGun ()
{
// Calibration values for location of gun (in m) with respect to center of LIDAR sensor.
float x_offset = 0;
float y_offset = 0;
int angle;
//align shooter with object
if(angle < 90) {
pc.printf("IF\r\n");
angle = (int) (180.0/3.14159*atan((yball +y_offset)/(xball + x_offset))) - currentAngle;
} else {
pc.printf("ELSE\r\n");
angle = (int) (180.0/3.14159*atan((yball +y_offset)/(xball + x_offset)));
}
if(xball>0) {
angle = angle -30;
} else {
angle = angle + 30;
}
setAngle(-angle, 1);
pc.printf("Angle: %d\r\n", angle);
//Shoot code here
//return back to 0 angle position
setAngle(angle, 0.5);
}
void nTransformable::setForward(nVec3 forward) {
forward.normalize();
if(forward.z() > 0.99999) {
setAngle(nQuaternion::fromEuler(nVec3(0, 90, 0)));
} else {
float a = acos(forward.x());
nVec3 axis = nVec3(0, -forward.z(), forward.y()).normalized() * sin(a / 2);
setAngle(nQuaternion(axis.x(), axis.y(), axis.z(), cos(a / 2)));
}
}
void turnTurtleRightTest() {
enterSuite("Turning Turtle ClockWise");
setAngle(0);
testVal((int) turnTurtleRight(90),90,"Valid: Turning turtle facing 0 degrees clockwise 90 degrees",EQUALS);
testVal((int) turnTurtleRight(360),90,"Valid: Turning turtle facing 90 degrees clockwise 360 degrees",EQUALS);
testVal((int) turnTurtleRight(270),0,"Valid: Turning turtle facing 90 degrees clockwise 270 degrees",EQUALS);
setAngle(330);
testVal((int) turnTurtleRight(100),70,"Valid: Turning turtle facing 330 degrees clockwise 100 degrees",EQUALS);
initTurtle();
leaveSuite();
}
bool Aimer::init(bool isRight, float upper, float lower)
{
_back = Sprite::create("aimerback.png");
addChild(_back);
//const Point mid(getContentSize()/2);
_pointer = Sprite::create("pointer.png");
_pointer->setAnchorPoint(Point::ANCHOR_MIDDLE_LEFT);
//_pointer->setPosition(mid);
_pointer->setScaleY(0.5);
_pointer->setOpacity(200);
_green = ProgressTimer::create(Sprite::create("aimergreen.png"));
//_green->setPosition(mid);
_green->setType(ProgressTimer::Type::RADIAL);
_green->setOpacity(150);
_crosshair=Sprite::create("crosshair.png");
addChild(_crosshair);
addChild(_green);
addChild(_pointer);
upperLimit = upper;
lowerLimit = lower;
reversed = !isRight;
//find out the percentage
if(!isRight)
setAngle((upperLimit+lowerLimit)/2);
else{
setAngle(-180-((upperLimit+lowerLimit)/2));
}
_green->setPercentage((lowerLimit-upperLimit)/3.6);
//_green->setRotation(-upperLimit);
auto listener = EventListenerTouchOneByOne::create();
listener->onTouchBegan = CC_CALLBACK_2(Aimer::onTouchBegan, this);
listener->onTouchEnded = CC_CALLBACK_2(Aimer::onTouchEnded, this);
listener->onTouchMoved = CC_CALLBACK_2(Aimer::onTouchMoved, this);
_eventDispatcher->addEventListenerWithSceneGraphPriority(listener, this);
listener->setSwallowTouches(true);
_back->setRotation((upperLimit+lowerLimit)/2);
return true;
}
void OblatedEqArea::_loadFromParams()
{
Projection::_loadFromParams();
setShapeM(m_gctpParams[2]);
setShapeN(m_gctpParams[3]);
setAngle(m_gctpParams[8]);
}
void servo(int angle,int pin){
pin3 = 3;
pin9 = 9;
init(pin3, pin9, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH,
DEFAULT_WAIT_DISABLE_PWM);
setAngle(angle,pin);
}
void NumberAnimation::setParametr(const std::string & _param, double _value)
{
m_currentParam = _param;
if (_param == "scale")
{
m_params[_param] = _value;
setScale();
}
else if (_param == "angle")
{
m_params[_param] = _value;
setAngle();
}
else if (_param == "alpha")
{
m_params[_param] = _value;
setAlpha();
}
else if (_param == "width")
{
m_params[_param] = _value;
setWidth();
}
else if (_param == "height")
{
m_params[_param] = _value;
setHeight();
}
else
{
m_currentParam = "";
}
}
/**
* Set the latitude given a value in the Planetographic coordinate system
*
* @param latitude The planetographic latitude to set ourselves to
* @param units The angular units latitude is in
*/
void Latitude::setPlanetographic(double latitude, Angle::Units units) {
if (m_equatorialRadius == NULL || m_polarRadius == NULL) {
IString msg = "Latitude [" + IString(latitude) + "] cannot be "
"converted to Planetocentic without the planetary radii, please use "
"the other Latitude constructor";
throw IException(IException::Programmer, msg, _FILEINFO_);
}
Angle inputAngle(latitude, units);
if (inputAngle > Angle(90.0, Angle::Degrees) ||
inputAngle < Angle(-90.0, Angle::Degrees)) {
IString msg = "Latitudes outside of the -90/90 range cannot be converted "
"between Planetographic and Planetocentric";
throw IException(IException::Programmer, msg, _FILEINFO_);
}
if(IsSpecial(latitude)) {
IString msg = "Invalid planetographic latitudes are not currently "
"supported";
throw IException(IException::Programmer, msg, _FILEINFO_);
}
double ocentricLatitude = atan(tan(inputAngle.radians()) *
(*m_polarRadius / *m_equatorialRadius) *
(*m_polarRadius / *m_equatorialRadius));
// Sometimes the trig functions return the negative of the expected value at the pole.
if ((ocentricLatitude > 0) != (inputAngle.radians() > 0)) {
ocentricLatitude *= -1;
}
setAngle(ocentricLatitude, Angle::Radians);
}
void WGraphicsRectItem::mouseReleaseEvent(WGraphicsSceneMouseEvent* event)
{
if (isSelected() && (event->button() == Ws::LeftButton) && _dragging)
{
_dragging = false;
WWorldPointF pos = event->scenePos();
WMatrix matrix = getRotateMatrix_1(); //获取逆向矩阵
if (_select_flag != SF_ROTATE) //矩形不是在旋转
{
if (_select_flag != SF_CONTENT || isMovable())
{
const int (&f)[4] = ADJUST_TABLE[bit(_select_flag)];
WWorldPointF offset = pos - scene()->dragStartPos();
WWorldRectF rect = data()->rect.adjusted(offset.x() * f[0], offset.y() * f[1], offset.x() * f[2], offset.y() * f[3]);
setRect(rect, false, true);
}
}
else //矩形在旋转
{
WWorldPointF c = data()->rect.center();
double ag = WWorldLineF(c, pos).angle() - 90;
setAngle(data()->angle + ag, false, true);
}
scene()->update();
}
}
BossBall::BossBall(GameController &controller):
controller(controller)
{
setData(GD_type, GO_BossBall);
pixMap.load("://images/BossBall1.png");
setAngle(0);
}
void Munizione::onTick()
{
MyAllegro* m =dynamic_cast<MyAllegro*>(MyAllegro::getInstance());
if(tick%m->getSpeed()==0)
{
if(checkColpito())
{
hit=false;
}
else
{
if(direzione == DESTRA)
posizioneX+=velocita;
else
posizioneX-=velocita;
angle += velocitaRotazione;
setLeftTopCorner(Point(posizioneX-getWidth()/2,posizioneY-getHeight()/2));
setAngle(angle);
}
}
}
Astroid::Astroid(float x, float y) {
setX(x);
setY(y);
setWidth(kWidth);
setHeight(kHeight);
setAngle(rand() % 360);
}
void Explore::publishPose(float x, float y, float theta, bool robot){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = x;
goal.target_pose.pose.position.y = y;
geometry_msgs::Quaternion qMsg;
setAngle(theta, qMsg);
goal.target_pose.pose.orientation = qMsg;
goal.target_pose.header.stamp = ros::Time::now();
if(robot)
{
goal.target_pose.header.frame_id ="/base_link";
} else {
goal.target_pose.header.frame_id ="/map";
}
ROS_INFO("Sending goal...");
ac_.sendGoalAndWait(goal,ros::Duration(TIME_FOR_GOAL),ros::Duration(0.1) );
firstGoalSend = true;
}
double turnLeft( turtle* t, double angle ) {
double new_angle;
new_angle = t->side * angle;
setAngle(t, t->angle - toRadians(new_angle));
}
void Aimer::onTouchMoved(cocos2d::Touch *touch, cocos2d::Event *event)
{
float a = CC_RADIANS_TO_DEGREES((touch->getLocation()-convertToWorldSpace(getPosition())).getAngle());
setAngle(-a-getParent()->getParent()->getRotation());
}
void TWRotateMultiSprite::update(Uint32 ticks) {
advanceFrame(ticks);
Vector2f incr = getVelocity() * static_cast<float>(ticks) * 0.001;
setPosition(getPosition() + incr);
setAngle(getAngle() + getAngleInc() );
if ( Y() < 0) {
velocityY( abs( velocityY() ) );
}
if ( Y() > worldHeight-frameHeight) {
velocityY( -abs( velocityY() ) );
}
if ( X() < 0) {
velocityX( abs( velocityX() ) );
flag = true;
}
if ( X() > worldWidth-frameWidth) {
velocityX( -abs( velocityX() ) );
flag = false;
}
}
void GoToSelectedBall::publishPose(float x, float y){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = x;
goal.target_pose.pose.position.y = y;
geometry_msgs::Quaternion qMsg;
setAngle(getRobotAngleInMap(), qMsg);
goal.target_pose.pose.orientation = qMsg;
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.header.frame_id ="/odom";
ROS_INFO("Sending goal...");
ac.sendGoal(goal);
firstGoalSent = true;
}
void PLEN2::JointController::resetSettings()
{
#if DEBUG
volatile Utility::Profiler p(F("JointController::resetSettings()"));
#endif
EEPROM[INIT_FLAG_ADDRESS()] = INIT_FLAG_VALUE();
eeprom_busy_wait();
for (int index = 0; index < sizeof(Shared::m_SETTINGS_INITIAL); index++)
{
EEPROM[SETTINGS_HEAD_ADDRESS() + index] = pgm_read_byte(
reinterpret_cast<const char*>(Shared::m_SETTINGS_INITIAL) + index
);
eeprom_busy_wait();
}
for (char joint_id = 0; joint_id < SUM; joint_id++)
{
m_SETTINGS[joint_id].MIN = pgm_read_word(Shared::m_SETTINGS_INITIAL + joint_id * 3 + 0);
m_SETTINGS[joint_id].MAX = pgm_read_word(Shared::m_SETTINGS_INITIAL + joint_id * 3 + 1);
m_SETTINGS[joint_id].HOME = pgm_read_word(Shared::m_SETTINGS_INITIAL + joint_id * 3 + 2);
setAngle(joint_id, m_SETTINGS[joint_id].HOME);
}
}
bool CannonField::qt_invoke( int _id, QUObject* _o )
{
switch ( _id - staticMetaObject()->slotOffset() ) {
case 0:
setAngle((int)static_QUType_int.get(_o+1));
break;
case 1:
setForce((int)static_QUType_int.get(_o+1));
break;
case 2:
shoot();
break;
case 3:
newTarget();
break;
case 4:
setGameOver();
break;
case 5:
restartGame();
break;
case 6:
moveShot();
break;
default:
return QWidget::qt_invoke( _id, _o );
}
return TRUE;
}
void PLEN2::JointController::resetSettings()
{
#if DEBUG
PROFILING("JointController::resetSettings()");
#endif
EEPROM[INIT_FLAG_ADDRESS] = INIT_FLAG_VALUE;
eeprom_busy_wait();
for (uint8_t index = 0; index < sizeof(Shared::m_SETTINGS_INITIAL); index++)
{
EEPROM[SETTINGS_HEAD_ADDRESS + index] = pgm_read_byte(
reinterpret_cast<const uint8_t*>(Shared::m_SETTINGS_INITIAL) + index
);
eeprom_busy_wait();
}
for (uint8_t joint_id = 0; joint_id < JOINTS_SUM; joint_id++)
{
m_SETTINGS[joint_id].MIN = pgm_read_word(Shared::m_SETTINGS_INITIAL + joint_id * 3 + 0);
m_SETTINGS[joint_id].MAX = pgm_read_word(Shared::m_SETTINGS_INITIAL + joint_id * 3 + 1);
m_SETTINGS[joint_id].HOME = pgm_read_word(Shared::m_SETTINGS_INITIAL + joint_id * 3 + 2);
setAngle(joint_id, m_SETTINGS[joint_id].HOME);
}
}
SpeakerItem::SpeakerItem( double _angle, double _radius)
{
radius = _radius > 0 ? _radius : 20;
setFlag( QGraphicsItem::ItemIsMovable);
setAngle( _angle);
emit valueChanged( getAngle());
}
void
cpBodySetAngle(cpBody *body, cpFloat angle)
{
cpBodyActivate(body);
cpBodyAssertSane(body);
setAngle(body, angle);
}
/*
* Similar to the Ultrasonic.
*/
char processSteeringCommand(char commandCode, void* commandData, Response* responseData) {
switch(commandCode) {
case SET_ANGLE:
setAngle(((unsigned char*)commandData));
responseData->size = 0;
break;
case GET_ANGLE:
responseData->size = 2;
getAngle((unsigned char*) responseData->payload);
break;
case GET_DESIRED_ANGLE:
responseData->size = 2;
getDesiredAngle((unsigned char*) responseData);
break;
case CHANGE_PID:
changePID(((char*)commandData)[0], ((char*)commandData)[1], ((char*)commandData)[2]);
break;
case SET_LIMITS:
setLimits(((char*)commandData)[0], ((char*)commandData)[1]);
break;
case RESET_MOTOR_DRIVER:
PORTJ ^= (1 << PJ4);
break;
}
return 1;
}
void PLEN2::JointController::loadSettings()
{
#if DEBUG
PROFILING("JointController::loadSettings()");
#endif
uint8_t* filler = reinterpret_cast<uint8_t*>(m_SETTINGS);
if (EEPROM[INIT_FLAG_ADDRESS] != INIT_FLAG_VALUE)
{
EEPROM[INIT_FLAG_ADDRESS] = INIT_FLAG_VALUE;
eeprom_busy_wait();
for (uint8_t index = 0; index < sizeof(m_SETTINGS); index++)
{
EEPROM[SETTINGS_HEAD_ADDRESS + index] = filler[index];
eeprom_busy_wait();
}
}
else
{
for (uint8_t index = 0; index < sizeof(m_SETTINGS); index++)
{
filler[index] = EEPROM[SETTINGS_HEAD_ADDRESS + index];
}
}
for (uint8_t joint_id = 0; joint_id < JOINTS_SUM; joint_id++)
{
setAngle(joint_id, m_SETTINGS[joint_id].HOME);
}
/*
@brief Configure timer 1
@attention
It might be easy to understand compare-matched output is LOW,
but in that case, PWM is outputting at switching multiplexer's output,
so the signal gets an impulse noise.
*/
cli();
TCCR1A =
_BV(WGM11) | _BV(WGM10) | // Set mode to "10bit fast PWM".
_BV(COM1A1) | _BV(COM1A0) | // Set OC1A to HIGH when compare matched.
_BV(COM1B1) | _BV(COM1B0) | // Set OC1B to HIGH when compare matched.
_BV(COM1C1) | _BV(COM1C0); // Set OC1C to HIGH when compare matched.
TCCR1B =
_BV(WGM12) | // Set mode to "10bit fast PWM".
_BV(CS11) | _BV(CS10); // Set prescaler to 64.
TIFR1 = _BV(OCF1A) | _BV(OCF1B) | _BV(OCF1C) | _BV(TOV1); // Clearing interruption flag.
sei();
TIMSK1 = _BV(TOIE1); // Begin timer 1.
}
void flagLayer::resetVars() {
flagObject::resetVars();
setScale(DEFAULT_SCALE);
setMaxColors(1);
setHeight(DEFAULT_HEIGHT);
setAngle(DEFAULT_ANGLE);
setCenter(ofPoint(DEFAULT_CENTER_X, DEFAULT_CENTER_Y));
}
//set the angle and set a timer
void ROservo::setTime(int angle, int duration)
{
if(angle < 0 || angle > _angles) return;
if(duration <= 0) return;
_angle = angle;
setAngle(angle);
_timer = millis()+duration;
}
void
GlobeMapMovingInterface::setPoint(const MC2Coordinate& newCoord,
const MC2Point& screenPoint,
double angleDegrees )
{
setPoint( newCoord, screenPoint );
setAngle( angleDegrees, screenPoint );
}
bool TargetData::setSlotAngle(const Basic::Angle* const msg)
{
bool ok = false;
if (msg != 0) {
ok = setAngle( Basic::Degrees::convertStatic(*msg) );
}
return ok;
}
bool TargetData::setSlotAngle(const Basic::Number* const msg)
{
bool ok = false;
if (msg != 0) {
ok = setAngle(msg->getDouble());
}
return ok;
}
