void GUI::set_orientation(int orientation)
{
switch(orientation)
{
case 0:
set_angle(0);
break;
case 1:
set_angle(90);
break;
}
this->orientation = orientation;
}
LVAL iview_spin_allocate(V)
{
LVAL object;
int vars, i, show, ascent, height;
IVIEW_WINDOW w;
StGWWinInfo *gwinfo;
object = xlgaobject();
show = xsboolkey(sk_show, TRUE);
gwinfo = StGWObWinInfo(object);
get_iview_ivars(object, &vars);
if (vars < 3) xlfail("too few variables");
w = IViewNew(object);
initialize_iview(w, object);
for (i = 0; i < vars; i++)
IViewSetScaledRange(w, i, -sqrt((double) vars), sqrt((double) vars));
set_content_variables(object, 0, 1, 2);
IViewSetIdentityTransformation(w);
set_rotation_type(object, Rolling);
set_angle(object, ALPHA);
ascent = StGWTextAscent(gwinfo);
height = (ascent > SPIN_CONTROL_SIZE) ? 2 * ascent : SPIN_CONTROL_HEIGHT;
StGrSetMargin(gwinfo, 0, 0, 0, height);
/* use StShowWindow to show (map) window but NOT send :resize or :redraw */
if (show) StShowWindow(w);
return(object);
}
/**
* @brief Calculates the direction and the speed of the movement
* depending on the target.
*/
void TargetMovement::recompute_movement() {
if (target_entity != NULL) {
// the target may be a moving entity
target_x = target_entity->get_x();
target_y = target_entity->get_y();
}
if (get_x() != target_x || get_y() != target_y) {
finished = false;
double angle = Geometry::get_angle(get_x(), get_y(), target_x, target_y);
int dx = target_x - get_x();
int dy = target_y - get_y();
sign_x = (dx >= 0) ? 1 : -1;
sign_y = (dy >= 0) ? 1 : -1;
if (std::fabs(angle - get_angle()) > 1E-6 || get_speed() < 1E-6) {
set_speed(speed);
set_angle(angle);
set_max_distance((int) Geometry::get_distance(
get_x(), get_y(), target_x, target_y));
}
}
}
/*
setup a channels aux servo function
*/
void RC_Channel_aux::aux_servo_function_setup(void)
{
switch (function) {
case RC_Channel_aux::k_flap:
case RC_Channel_aux::k_flap_auto:
case RC_Channel_aux::k_egg_drop:
set_range_out(0,100);
break;
case RC_Channel_aux::k_heli_rsc:
case RC_Channel_aux::k_heli_tail_rsc:
set_range_out(0,1000);
break;
case RC_Channel_aux::k_aileron_with_input:
case RC_Channel_aux::k_elevator_with_input:
set_angle(4500);
break;
case RC_Channel_aux::k_aileron:
case RC_Channel_aux::k_elevator:
case RC_Channel_aux::k_dspoiler1:
case RC_Channel_aux::k_dspoiler2:
case RC_Channel_aux::k_rudder:
case RC_Channel_aux::k_steering:
case RC_Channel_aux::k_flaperon1:
case RC_Channel_aux::k_flaperon2:
set_angle_out(4500);
break;
default:
break;
}
if (function < k_nr_aux_servo_functions) {
_function_mask |= (1ULL<<(uint8_t)function);
}
}
LVAL iview_spin_angle(V)
{
LVAL object;
object = xlgaobject();
if (moreargs()) set_angle(object, makefloat(xlgetarg()));
xllastarg();
return(slot_value(object, s_rotation_angle));
}
void OrientationFilter::pointToNext(std::vector<geometry_msgs::PoseStamped>& path, int index)
{
double x0 = path[ index ].pose.position.x,
y0 = path[ index ].pose.position.y,
x1 = path[index+1].pose.position.x,
y1 = path[index+1].pose.position.y;
double angle = atan2(y1-y0,x1-x0);
set_angle(&path[index], angle);
}
StraightMovement::StraightMovement(double angle, double speed) :
Movement(),
move_y(0),
move_x(0),
next_move_date_x(System::now()),
next_move_date_y(System::now()),
finished(false) {
set_speed(speed);
set_angle(Geometry::degrees_to_radians(angle));
//set_angle(angle);
}
void OrientationFilter::interpolate(std::vector<geometry_msgs::PoseStamped>& path,
int start_index, int end_index)
{
double start_yaw = getYaw(path[start_index]),
end_yaw = getYaw(path[end_index ]);
double diff = angles::shortest_angular_distance(start_yaw, end_yaw);
double increment = diff/(end_index-start_index);
for(int i=start_index; i<=end_index; i++){
double angle = start_yaw + increment * i;
set_angle(&path[i], angle);
}
}
void SpriteRender::set_to_face_player(Player* player) {
// set this sprite's angle to point toward player
fvec3 dir = pos - player->get_viewpoint();
dir = glm::normalize(dir);
Rotation* lookdir = player->get_viewpoint_angle();
fvec3 up = glm::cross(lookdir->get_right(), dir);
up = glm::normalize(up);
Rotation pointing = Rotation();
pointing.set_to_point(dir, up);
set_angle(pointing);
}
/**
* \brief Combine with an other set of attributes.
* \param that The attributes to combine with.
*
* The attributes changed by this method are : is_flipped(), is_mirrored(), the
* intensities, the opacity and the angle. The size is not changed.
*/
void bear::visual::bitmap_rendering_attributes::combine
( const bitmap_rendering_attributes& that )
{
flip( that.is_flipped() ^ is_flipped() );
mirror( that.is_mirrored() ^ is_mirrored() );
set_intensity
( that.get_red_intensity() * get_red_intensity(),
that.get_green_intensity() * get_green_intensity(),
that.get_blue_intensity() * get_blue_intensity()
);
set_opacity( that.get_opacity() * get_opacity() );
set_angle( that.get_angle() + get_angle() );
} // bitmap_rendering_attributes::combine()
int Entity::load_self(IODataObject* obj) {
//up = obj->read_value<fvec3>();
set_pos(obj->read_value<fvec3>());
set_angle(obj->read_value<Rotation>());
bounds = obj->read_value<bounding_box>();
center_pos = obj->read_value<fvec3>();
weight = obj->read_value<int>();
health = obj->read_value<int>();
return 0;
}
void ray_options::restore()
{
blockSignals(true);
ui->red_slider->setValue(initial_color.red());
ui->green_slider->setValue(initial_color.green());
ui->blue_slider->setValue(initial_color.blue());
ui->x_spin_box->setValue(initial_pos.x());
ui->y_spin_box->setValue(initial_pos.y());
set_color(initial_color.red(),
initial_color.green(),
initial_color.blue());
set_angle(qRadiansToDegrees(initial_dir));
set_pos(initial_pos.x(), initial_pos.y());
blockSignals(false);
}
/*
setup a channels aux servo function
*/
void SRV_Channel::aux_servo_function_setup(void)
{
if (type_setup) {
return;
}
switch (function) {
case k_flap:
case k_flap_auto:
case k_egg_drop:
set_range(100);
break;
case k_heli_rsc:
case k_heli_tail_rsc:
case k_motor_tilt:
case k_boost_throttle:
set_range(1000);
break;
case k_aileron_with_input:
case k_elevator_with_input:
case k_aileron:
case k_elevator:
case k_dspoilerLeft1:
case k_dspoilerLeft2:
case k_dspoilerRight1:
case k_dspoilerRight2:
case k_rudder:
case k_steering:
case k_flaperon_left:
case k_flaperon_right:
case k_tiltMotorLeft:
case k_tiltMotorRight:
case k_elevon_left:
case k_elevon_right:
case k_vtail_left:
case k_vtail_right:
set_angle(4500);
break;
case k_throttle:
case k_throttleLeft:
case k_throttleRight:
// fixed wing throttle
set_range(100);
break;
default:
break;
}
}
/**
* \brief Changes the movement of the entity depending on the direction wanted.
*
* This function is called when the direction is changed.
*/
void PlayerMovement::compute_movement() {
// compute the speed vector corresponding to the direction wanted by the player
if (direction8 == -1) {
// no movement
stop();
}
else { // the directional keys currently pressed define a valid movement
set_speed(moving_speed);
set_angle(Geometry::degrees_to_radians(direction8 * 45));
}
// notify the entity that its movement has just changed:
// indeed, the entity may need to update its sprites
notify_movement_changed();
}
/**
* \brief Combine with an other set of attributes.
* \param that The attributes to combine with.
* The attributes changed by this method are : get_flipped_status(),
* get_mirrored_status(), the intensities, the opacity and the angle.
* The size is not changed.
*/
void bf::bitmap_rendering_attributes::combine
( const bitmap_rendering_attributes& that )
{
flip( trinary_logic::from_bool
(trinary_logic::to_bool(that.get_flipped_status()) ^
trinary_logic::to_bool(get_flipped_status())) );
mirror( trinary_logic::from_bool
(trinary_logic::to_bool(that.get_mirrored_status()) ^
trinary_logic::to_bool(get_mirrored_status())) );
m_color.set_intensity
( that.get_color().get_red_intensity() * m_color.get_red_intensity(),
that.get_color().get_green_intensity() * m_color.get_green_intensity(),
that.get_color().get_blue_intensity() * m_color.get_blue_intensity()
);
m_color.set_opacity( that.get_color().get_opacity() * m_color.get_opacity() );
set_angle( that.get_angle() + get_angle() );
} // bitmap_rendering_attributes::combine()
void Active::set_direction(int value, bool set_movement)
{
value &= 31;
FrameObject::set_direction(value, set_movement);
if (active_flags & AUTO_ROTATE) {
set_angle((value * 360) / 32);
value = 0;
}
if (value == animation_direction)
return;
animation_direction = value;
Direction * old_dir = direction_data;
Direction * new_dir = get_direction_data();
if (old_dir == new_dir)
return;
update_direction(new_dir);
}
/*
setup a channels aux servo function
*/
void RC_Channel_aux::aux_servo_function_setup(void)
{
switch (function) {
case RC_Channel_aux::k_flap:
case RC_Channel_aux::k_flap_auto:
case RC_Channel_aux::k_egg_drop:
set_range_out(0,100);
break;
case RC_Channel_aux::k_heli_rsc:
case RC_Channel_aux::k_heli_tail_rsc:
set_range_out(0,1000);
break;
case RC_Channel_aux::k_aileron_with_input:
case RC_Channel_aux::k_elevator_with_input:
set_angle(4500);
break;
case RC_Channel_aux::k_aileron:
case RC_Channel_aux::k_elevator:
case RC_Channel_aux::k_dspoiler1:
case RC_Channel_aux::k_dspoiler2:
case RC_Channel_aux::k_rudder:
case RC_Channel_aux::k_steering:
case RC_Channel_aux::k_flaperon1:
case RC_Channel_aux::k_flaperon2:
set_angle_out(4500);
break;
case RC_Channel_aux::k_motor_tilt:
// tenth percentage tilt
set_range_out(0,1000);
break;
case RC_Channel_aux::k_throttle:
// fixed wing throttle
set_range_out(0,100);
break;
default:
break;
}
if (function < k_nr_aux_servo_functions) {
set_function_mask((uint8_t)function.get());
}
}
/**
* \brief Chooses a new direction for the movement.
*/
void RandomMovement::set_next_direction() {
double angle;
if (get_entity() == NULL
|| max_radius == 0 // means no limit
|| bounds.contains(get_x(), get_y())) {
// we are inside the bounds (or there is no bound): pick a random direction
angle = Geometry::degrees_to_radians(Random::get_number(8) * 45 + 22.5);
}
else {
// we are outside the bounds: get back into the rectangle to avoid going too far
angle = Geometry::get_angle(get_x(), get_y(), bounds.get_x() + bounds.get_width() / 2, bounds.get_y() + bounds.get_height() / 2);
}
set_angle(angle);
next_direction_change_date = System::now() + 500 + Random::get_number(1500); // change again in 0.5 to 2 seconds
notify_movement_changed();
}
void Entity::set_parent(SuperObject* parent) {
// transform entity rotation/pos into this object's frame
// after transformation, the rwc center position should be the same
fvec3 global_center = get_world_pos();
Rotation global_rotation = get_world_rotation();
fvec3 oac;
if (parent) {
parent->transform_into_my_coordinates(&oac, global_center.x, global_center.y, global_center.z);
}
else {
oac = global_center;
}
this->parent = parent;
set_pos(oac - get_center_offset());
Rotation aligned_rot;
if (parent) {
parent->transform_into_my_rotation(&aligned_rot, global_rotation);
}
else {
aligned_rot = global_rotation;
}
set_angle(aligned_rot);
}
AngularIntegration::AngularIntegration(dFloat angle) {
set_angle(angle);
}
void ray_options::dial_changed(int deg)
{
int ia = (deg + 360 - 90) % 360;
set_angle(ia);
}
void ray_options::spin_box_changed(double deg)
{
if (deg < 0.0) deg += 360;
else if (deg >= 360) deg -= 360;
set_angle(deg);
}
void main()
{
auto int chan, ang, rvalue;
auto float dist;
auto int pwm, r_head, r_head2, head;
auto long freq;
auto char tmpbuf[128], c;
char gpsString[MAX_SENTENCE];
GPSPosition testPoint, testPoint_old;
brdInit();
serPORT(BAUDGPS);
//serCopen(BAUDGPS);
serMode(0);
serCrdFlush();// main loop
initGPS();
freq = pwm_init(450ul);
printf("pick heading: ");
r_head = atoi(gets(tmpbuf));
//while(1)
// {
/*goal.lat_degrees = 43;
goal.lat_minutes = 7036;
goal.lon_degrees = 72;
goal.lon_minutes = 2811;
goal.lat_direction = 'N';
goal.lon_direction = 'W';
goal.sog = 0;
//goal.tog =
*/
if ((r_head > 90) || (r_head < -90))
{
printf("\nbad heading ");
}
else {
while (1)
{
costate GPSRead always_on
{
// wait until gps mode is engaged and a gps string has been recieved
waitfor(serCrdUsed() > 0);
//printf("gps read: ");
// read until finding the beginning of a gps string then wait
while(1)
{
//int test2;
c = serCgetc();
if (c == -1)
{
serCrdFlush();
abort;
}
else if (c == '$')
{
waitfor(DelayMs(20)); //should only take 5ms or so at 115200 baud
break;
}
}//end while(1)
// now that 20 ms have passed, read in the gps string (it must all
// be there by now, since so much time has passed
getgps(gpsString);
rvalue = gps_get_position(&testPoint,gpsString);
if( rvalue != -1)
{
printGPSPosition(&testPoint);
}
//printf("gps: %u \n",strlen(test2));
printf("gps: %s ",gpsString);
//printGPSPosition(&testPoint);
//puts(gpsString);
//puts(": end gps \n");
//dist = gps_ground_distance(&testPoint, &testPoint_old);
//head = (int)gps_bearing(&testPoint, &testPoint_old, dist);
//testPoint_old = testPoint;
head = testPoint.tog; // grab current heading
r_head2 = r_head-head;
pwm = set_angle(0, r_head2);
printf("angle: %d, head %d \n",pwm, head);
}//end costate
} // end while
} // end else
//} // end first while
} // end main
AngularIntegration::AngularIntegration() {
set_angle(0.0f);
}
void quat::scale_angle(float f)
{
set_angle( f * get_angle() );
}
void
Rotation::set_angle_in_degrees(PrecisionType value)
{
set_angle(value * CE3D2::PI / 180.0f);
}
BodyDescription::BodyDescription(const PhysicsContext &pc, const std::string &resource_id, const XMLResourceDocument &resources)
: impl(new BodyDescription_Impl(pc.impl->get_owner()))
{
/* example resource entry with all parameters:
<body2d name="TestBody" type="static">
<position x="0" y="0"/>
<rotation angle="180"/>
<velocity x="0" y="0" angular="0"/>
<damping linear="0" angular="0"/>
<parameters awake="true" can_sleep="true" bullet="false" active="true" />
</body2d>
*/
XMLResourceNode resource = resources.get_resource(resource_id);
if (resource.get_type() != "body2d" && resource.get_type() != "body2d_description")
throw Exception(string_format("Resource '%1' is not of type 'body2d' or 'body2d_description'", resource_id));
DomNode cur_node = resource.get_element().get_first_child();
//Body type
std::string body_type = resource.get_element().get_attribute("type","static");
if(body_type == "kinematic") set_type(body_kinematic);
else if(body_type == "dynamic") set_type(body_dynamic);
else set_type(body_static);
while(!cur_node.is_null())
{
if (!cur_node.is_element())
continue;
DomElement cur_element = cur_node.to_element();
std::string tag_name = cur_element.get_tag_name();
//<position x="0" y="0"/>
if(tag_name == "position")
{
float pos_x = 0.0f;
float pos_y = 0.0f;
if(cur_element.has_attribute("x"))
{
pos_x = StringHelp::text_to_float(cur_element.get_attribute("x"));
}
if(cur_element.has_attribute("y"))
{
pos_y = StringHelp::text_to_float(cur_element.get_attribute("y"));
}
set_position(pos_x, pos_y);
}
//<rotation angle="180"/>
else if(tag_name == "rotation")
{
Angle angle(0.0f, angle_degrees);
if(cur_element.has_attribute("angle"))
{
angle = Angle(StringHelp::text_to_float(cur_element.get_attribute("angle")), angle_degrees);
}
set_angle(angle);
}
//<velocity x="0" y="0" angular="0"/>
else if(tag_name == "velocity")
{
Vec2f velocity(0.0f, 0.0f);
Angle angular_velocity(0.0f, angle_degrees);
if(cur_element.has_attribute("x"))
{
velocity.x = StringHelp::text_to_float(cur_element.get_attribute("x"));
}
if(cur_element.has_attribute("y"))
{
velocity.y = StringHelp::text_to_float(cur_element.get_attribute("y"));
}
if(cur_element.has_attribute("angular"))
{
angular_velocity = Angle(StringHelp::text_to_float(cur_element.get_attribute("angular")), angle_degrees);
}
set_linear_velocity(velocity);
set_angular_velocity(angular_velocity);
}
//<damping linear="0" angular="0"/>
else if(tag_name == "damping")
{
float linear;
float angular;
if(cur_element.has_attribute("linear"))
{
linear = StringHelp::text_to_float(cur_element.get_attribute("linear"));
}
if(cur_element.has_attribute("angular"))
{
angular = StringHelp::text_to_float(cur_element.get_attribute("angular"));
}
set_linear_damping(linear);
set_angular_damping(angular);
}
//<parameters awake="true" can_sleep="true" bullet="false" active="true" />
else if(tag_name == "parameters")
{
bool value;
if(cur_element.has_attribute("awake"))
{
value = true;
value = StringHelp::text_to_bool(cur_element.get_attribute("awake"));
set_awake(value);
}
if(cur_element.has_attribute("active"))
{
value = true;
value = StringHelp::text_to_bool(cur_element.get_attribute("active"));
set_active(value);
}
if(cur_element.has_attribute("bullet"))
{
value = false;
value = StringHelp::text_to_bool(cur_element.get_attribute("bullet"));
set_as_bullet(value);
}
if(cur_element.has_attribute("can_sleep"))
{
value = true;
value = StringHelp::text_to_bool(cur_element.get_attribute("can_sleep"));
allow_sleep(value);
}
}
cur_node = cur_node.get_next_sibling();
}
}
// Control test
// Feedback control test
void ControlLogging(UINT8 node_id) {
// Logging parameters
UINT8 i2c_status;
float curr_ang, curry_time = 0.0, curr_voltage = 0.0, log_time = 2.0;
UINT32 start_millis, start_log_millis, log_Ts = 10;
float reference = 1.5708;
//#define PROGRAM_NEW_PARAMS
#ifdef PROGRAM_NEW_PARAMS
control_params_struct newControlParams =
{
.Fs = 625,
.nd = 2, .d = {-0.6327,0.8222},
.nc = 2, .c = {-0.6327,0.8222},
.nf = 1, .f = {-0.9810}
};
i2c_status = program_control_params(node_id,&newControlParams);
UINT8 prog_status;
if (i2c_status == I2C_STATUS_SUCCESFUL) {
i2c_status = read_control_prog_status(1, &prog_status);
if(i2c_status == I2C_STATUS_SUCCESFUL && prog_status == MOTOR_CONTROL_PROGRAMMING_STATUS_SUCCESS) {
putsUART1("Control programming succesful!\n\r");
} else {
putsUART1("Control programming failed, quitting...\n\r");
return;
}
}
#else
//set_default_control_params(node_id);
#endif
sprintf(buf,"\nreference: %0.2f rad, log time: %0.1f sec, Ts: %d ms\n\n\r", reference, log_time, log_Ts);
putsUART1(buf);
putsUART1("| Time | motor angle | motor voltage |\n\n\r");
delay_ms(100);
putsUART1("starting logging:\n\r");
i2c_status = calibrate_encoder_zero(node_id);
if (i2c_status != I2C_STATUS_SUCCESFUL) {
sprintf(buf,"Motor with id %d not found on the bus, quitting\n\r",node_id);
putsUART1(buf);
return;
}
set_angle(node_id,0.0);
start_millis = millis;
unsigned char log_stage = 0;
while(curry_time < (1.0 + 2.0*log_time)) {
start_log_millis = millis;
curry_time = ((float)(start_log_millis - start_millis))/1000.0;
if ( (log_stage == 0) && (curry_time > 1.0) ){
i2c_status = set_angle(node_id,reference);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
log_stage = 1;
}
}
if ( (log_stage == 1) && curry_time > (1.0 + log_time)) {
i2c_status = set_angle(node_id,0.0);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
log_stage = 2;
}
}
i2c_status = get_angle(node_id,&curr_ang);
i2c_status |= get_voltage(node_id,&curr_voltage);
if (i2c_status == I2C_STATUS_SUCCESFUL) {
sprintf(buf,"%f, %f, %f\n\r", curry_time, curr_ang, curr_voltage);
putsUART1(buf);
}
// Wait until Ts milliseconds has passed since start
while( millis - start_log_millis < log_Ts);
}
putsUART1("logging completed!\n\r");
set_calibration_status_unknown(node_id);
}
SharpEdgeFilter(double min_angle_deg=60) : _max_dot(0) {
set_angle(min_angle_deg);
}
