void turn_at_magnetic_angle(double angle) {
double turn_angle = 0;
mag_info current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid);
double current_angle = compute_angle(current_mg_i.y, current_mg_i.x);
turn_angle = compute_turn_angle(angle, current_angle);
PWM_Set(0, 15);
if (turn_angle > 0) {
Kierunek(RIGHT_ENGINES, REVERSE_GEAR);
Kierunek(LEFT_ENGINES, FORWARD_GEAR);
} else {
Kierunek(RIGHT_ENGINES, FORWARD_GEAR);
Kierunek(LEFT_ENGINES, REVERSE_GEAR);
}
do {
current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid);
current_angle = compute_angle(current_mg_i.y, current_mg_i.x);
turn_angle = compute_turn_angle(angle, current_angle);
waitms(5);
} while (ABS(turn_angle) > 1);
Kierunek(RIGHT_ENGINES, STOP_GEAR);
Kierunek(LEFT_ENGINES, STOP_GEAR);
}
/*
Compute the angle for each step. This will be needed for the leg rotations!
*/
void glRouteWithFeet::compute_angles( )
{
for (int i=0; i<m_left_steps.size(); i++)
compute_angle( m_left_steps[i] );
for (int i=0; i<m_right_steps.size(); i++)
compute_angle( m_right_steps[i] );
}
void
elastic_poly(int x1, int y1, int x2, int y2, int numsides)
{
register float angle;
register int nx, ny, i;
double dx, dy;
double init_angle, mag;
int ox, oy;
dx = x2 - x1;
dy = y2 - y1;
mag = sqrt(dx * dx + dy * dy);
init_angle = compute_angle(dx, dy);
ox = x2;
oy = y2;
/* now append numsides points */
for (i = 1; i < numsides; i++) {
angle = (float)(init_angle - M_2PI * (double) i / (double) numsides);
if (angle < 0)
angle += M_2PI;
nx = x1 + round(mag * cos((double) angle));
ny = y1 + round(mag * sin((double) angle));
pw_vector(canvas_win, nx, ny, ox, oy, INV_PAINT, 1,
RUBBER_LINE, 0.0, DEFAULT);
ox = nx;
oy = ny;
}
pw_vector(canvas_win, ox, oy, x2, y2, INV_PAINT, 1,
RUBBER_LINE, 0.0, DEFAULT);
}
void compute_state_imu(){
compute_position_acceleration();
compute_position_velocity();
compute_position();
compute_angle(); //use gyro + mag
compute_angular_velocity(); // differentiate above
compute_angular_acceleration(); // differentiate above
}
int
compute_direction(F_pos p1, F_pos p2, F_pos p3)
{
double diff, dx, dy, alpha, theta;
dx = p2.x - p1.x;
dy = p1.y - p2.y; /* because origin of the screen is on the
* upper left corner */
alpha = compute_angle(dx, dy);
dx = p3.x - p2.x;
dy = p2.y - p3.y;
theta = compute_angle(dx, dy);
diff = theta - alpha;
if ((0 < diff && diff < M_PI) || diff < -M_PI) {
return (1); /* counterclockwise */
}
return (0); /* clockwise */
}
void CodDirsCanvas::update_pos(const QPoint & link_start,
const QPoint & link_end)
{
hide();
angle = compute_angle(link_end.x() - link_start.x(),
link_end.y() - link_start.y());
QRect r = rect();
r.moveCenter((link_start + link_end) / 2);
moveBy(r.x(), r.y());
if (label != 0)
update_label_pos(label, TRUE);
if (backward_label != 0)
update_label_pos(backward_label, FALSE);
show();
canvas()->update();
}
void draw_fiend_beam(BITMAP *dest, BITMAP *img, int x1, int y1,int x2, int y2)
{
int i;
V3D_f point[4];
V3D_f temp_point[2];
VERTEX the_point[4];
float angle;
angle = compute_angle(x2,y2,x1,y1);
// set points of a point...
temp_point[0].y = 0;
temp_point[0].x = -img->w/2;
temp_point[1].y = 0;
temp_point[1].x = img->w/2;
//rotate the points
for(i=0;i<2;i++)
{
point[i].x = (temp_point[i].x*COS(angle)-temp_point[i].y*SIN(angle));
point[i].y = (temp_point[i].y*COS(angle)+temp_point[i].x*SIN(angle));
}
//give the other point thses cords...
point[2].x = point[0].x;
point[2].y = point[0].y;
point[3].x = point[1].x;
point[3].y = point[1].y;
//transform to world space..
point[0].x += x1;
point[0].y += y1;
point[1].x += x1;
point[1].y += y1;
point[2].x += x2;
point[2].y += y2;
point[3].x += x2;
point[3].y += y2;
//set the u,v cords (texture cords)
for(i=0;i<4;i++)
point[i].c = makecol(255,0,0);
point[0].u = 0;
point[0].v = 0;
point[1].u = img->w;
point[1].v = 0;
point[2].u = 0;
point[2].v = distance(point[0].x,point[0].y,point[2].x,point[2].y );
point[3].u = img->w;
point[3].v = distance(point[1].x,point[1].y,point[3].x,point[3].y );
for(i=0;i<4;i++)
{
the_point[i].x = point[i].x;
the_point[i].y = point[i].y;
the_point[i].u = point[i].u;
the_point[i].v = point[i].v;
}
quad3d_f(dest,POLYTYPE_ATEX_MASK,img, &point[0], &point[1], &point[3],&point[2] );
//fiend_quad3d(dest,img, &the_point[0], &the_point[1], &the_point[3],&the_point[2] );
//for(i=0;i<4;i++)
//circlefill(dest,point[i].x,point[i].y,3,makecol(255,0,255));
}
