void draw_quad(quad q){
glC(1.0,0.0,0.0);
glBegin(GL_QUADS);
set_point(q.p[0]);
set_point(q.p[1]);
set_point(q.p[2]);
set_point(q.p[3]);
glEnd();
}
static void
draw_rect(const struct dfont_rect *rect, int size, struct matrix *mat, uint32_t color) {
float vb[16];
int w = (rect->w -1) * size / FONT_SIZE ;
int h = (rect->h -1) * size / FONT_SIZE ;
set_point(vb+0, mat->m, 0,0, rect->x, rect->y);
set_point(vb+4, mat->m, w*SCREEN_SCALE,0, rect->x+rect->w-1, rect->y);
set_point(vb+8, mat->m, w*SCREEN_SCALE,h*SCREEN_SCALE, rect->x+rect->w-1, rect->y+rect->h-1);
set_point(vb+12, mat->m, 0,h*SCREEN_SCALE, rect->x, rect->y+rect->h-1);
shader_draw(vb, color);
}
static void
draw_rect(const struct dfont_rect *rect, int size, struct matrix *mat, uint32_t color, uint32_t additive) {
struct vertex_pack vb[4];
int w = (rect->w -1) * size / FONT_SIZE ;
int h = (rect->h -1) * size / FONT_SIZE ;
set_point(&vb[0], mat->m, 0,0, rect->x, rect->y);
set_point(&vb[1], mat->m, w*SCREEN_SCALE,0, rect->x+rect->w-1, rect->y);
set_point(&vb[2], mat->m, w*SCREEN_SCALE,h*SCREEN_SCALE, rect->x+rect->w-1, rect->y+rect->h-1);
set_point(&vb[3], mat->m, 0,h*SCREEN_SCALE, rect->x, rect->y+rect->h-1);
shader_draw(vb, color, additive);
}
//-------------------------------------------------------------------------
// Purpose : insert a point into the facet
//
// Special Notes : create two new facets and return them
//
// Creator :
//
// Creation Date :
//-------------------------------------------------------------------------
CubitPoint* CubitFacetData::insert_point( const CubitVector& position,
CubitFacet*& new_tri1_out,
CubitFacet*& new_tri2_out )
{
CubitPointData* new_point = new CubitPointData( position );
CubitFacetData *new_tri1, *new_tri2;
new_tri1 = new CubitFacetData( point(1), point(2), new_point );
new_tri2 = new CubitFacetData( point(2), point(0), new_point );
point(2)->remove_facet( this );
set_point( new_point, 2 );
new_point->add_facet( this );
if ( edge(0) ) {
new_tri1->edge( edge(0), 2 );
new_tri1->edge_use( edge_use(0), 2 );
edge(0)->remove_facet(this);
edge(0)->add_facet(new_tri1);
edge( 0, 0 );
}
if ( edge(1) ) {
new_tri2->edge( edge(1), 2 );
new_tri2->edge_use( edge_use(1), 2 );
edge(1)->remove_facet(this);
edge(1)->add_facet(new_tri2);
edge( 0, 1 );
}
update_plane();
new_tri1_out = new_tri1;
new_tri2_out = new_tri2;
return new_point;
}
void gere_key2(int keycode, t_env *env)
{
if (keycode == 113)
COLOR += 0x010000;
if (keycode == 119)
COLOR += 0x000100;
if (keycode == 101)
COLOR += 0x01;
if (keycode == 97)
COLOR -= 0x00010000;
if (keycode == 115)
COLOR -= 0x00000100;
if (keycode == 100)
COLOR -= 0x00000001;
if (keycode == 65456)
{
COEFFZ = 1;
ANGLEX = 0;
ANGLEZ = 0;
CHOICE = 0;
COLOR = DEF_COLOR;
C = MIN(WIDTH / (MAX_X + 5), HEIGHT / (MAX_Y + 5));
C = C ? C : 1;
OFF = set_point(0, 0, 0);
get_points(env);
}
}
int main(void)
{
float x=1;
float p=10;
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
delay_init(168); //初始化延时函数
EXTIX_Init();
uart_init(115200);
TIM10_PWM_Init(10000-1,336-1); //84M/84=1Mhz的计数频率,重装载值500，所以PWM频率为 1M/500=2Khz.
TIM11_PWM_Init(10000-1,336-1);
TIM9_PWM_Inita(10000-1,336-1);
TIM9_PWM_Initb(10000-1,336-1);
//TIM3_Int_Init(5000-1,8400-1); //定时器时钟84M，分频系数8400，所以84M/8400=10Khz的计数频率，计数5000次为500ms
exp_point=set_point(0,0,0,750);
MPU_Init();
printf("aaa\r\n");
while(mpu_dmp_init()){}
//TIM3_Int_Init(100-1,8400-1);
init_omegax.data=x;
init_omegax.p=p;
init_roll.data=x;
init_roll.p=p;
while(1)
{
LED0=!LED0;//DS0翻转
delay_ms(200);//延时200ms
};
}
/*}}}*/
static void chart_gather(int gen,/*{{{*/
struct data *d,
struct tower_table *table,
int I, int J,
struct tower_data *data)
{
struct odata *od;
od = compute_odata(d, gen);
if (od) {
int i, n;
int max;
int thresh;
n = od->n_lcs;
max = od->rel[0];
thresh = max >> 2;
for (i=0; i<n; i++) {
struct tower2 *tow;
int ti;
struct tower_data *dd;
struct chart *c;
if (od->rel[i] < thresh) break;
tow = cid_to_tower(table, od->lcs[i].lac, od->lcs[i].cid);
if (tow) {
ti = tow->index;
dd = data + ti;
c = (gen == 3) ? dd->c3.central : dd->c2.central;
set_point(c, I, J);
}
}
free_odata(od);
}
}
int main(void)
{
t_point point;
set_point(&point);
return (0);
}
int main(void)
{
t_point point;
set_point(&point);
printf("%d", point.x);
return (0);
}
void drawPath(Location path[], int pathLength) {
int i;
for (i = 0; i < pathLength; i++) {
set_point(path[i].x, path[i].y - 17.00);
}
}
// src is a grid src_rows * src_cols
// dest is a pre-allocated grid, dest_rows*dest_cols
void interpolate_image(float *src, uint8_t src_rows, uint8_t src_cols,
float *dest, uint8_t dest_rows, uint8_t dest_cols) {
float mu_x = (src_cols - 1.0) / (dest_cols - 1.0);
float mu_y = (src_rows - 1.0) / (dest_rows - 1.0);
float adj_2d[16]; // matrix for storing adjacents
for (uint8_t y_idx=0; y_idx < dest_rows; y_idx++) {
for (uint8_t x_idx=0; x_idx < dest_cols; x_idx++) {
float x = x_idx * mu_x;
float y = y_idx * mu_y;
//Serial.print("("); Serial.print(y_idx); Serial.print(", "); Serial.print(x_idx); Serial.print(") = ");
//Serial.print("("); Serial.print(y); Serial.print(", "); Serial.print(x); Serial.print(") = ");
get_adjacents_2d(src, adj_2d, src_rows, src_cols, x, y);
/*
Serial.print("[");
for (uint8_t i=0; i<16; i++) {
Serial.print(adj_2d[i]); Serial.print(", ");
}
Serial.println("]");
*/
float frac_x = x - (int)x; // we only need the ~delta~ between the points
float frac_y = y - (int)y; // we only need the ~delta~ between the points
float out = bicubicInterpolate(adj_2d, frac_x, frac_y);
//Serial.print("\tInterp: "); Serial.println(out);
set_point(dest, dest_rows, dest_cols, x_idx, y_idx, out);
}
}
}
/**
* map_pixel - map pixel on "sensor" surface.
*
* c: coordinates of lower left corner of mapped pixel
* x: pixel x in [0:W-1]
* y: pixel y in [0:H-1]
*
*/
static void map_pixel(struct coord *c, int x, int y)
{
set_point(c,
((double) x) * fac_xy + off_x,
((double) y) * fac_xy + off_y,
-focal
);
}
static void
heightmap_to_screen(void) {
int i, e;
for (e = 0; e < HEIGHT; ++e)
for (i = 0; i < WIDTH; ++i)
set_point(i, e, height_to_colour(heightmap[i][e]));
}
/* sets the tool to a brush */
gint
set_brush(void)
{
set_point();
utensil = CARMEN_MAP_BRUSH;
gtk_label_set_text(GTK_LABEL(tool_label), "brush");
return 1;
}
/* sets the tool to a eye dropper */
gint
set_zoom(void)
{
set_point();
utensil = CARMEN_MAP_ZOOM;
gtk_label_set_text(GTK_LABEL(tool_label), "zoom");
return 1;
}
void set_circle(Level *lvl, unsigned x, unsigned y, char value) {
register int tx, ty;
for(ty=-3; ty<=3; ty++) {
for(tx=-3; tx<=3; tx++) {
if((tx==-3 || tx==3) && (ty==-3 || ty==3)) continue;
set_point(lvl, x+tx, y+ty, value);
}
}
}
/*!
Copies the point and dir pointers from \a p. Also copies the values
of empty and gdType .
*/
sphericalGraspDirection::sphericalGraspDirection(GraspDirection* p) : GraspDirection()
{
point = new spherical_coordinates();
set_point(p->get_point());
dir = new spherical_coordinates();
set_dir(p->get_dir());
empty = p->get_empty();
set_gdType(p->get_gdType());
}
void Line::draw(Frame* test) {
if (!test->on_frame(start_x, start_y) || !test->on_frame(end_x, end_y)) {
throw std::runtime_error("Die Punkte liegen nicht im Frame!\n");
}
if(end_x < start_x){ //wenn endpunkt links von startpunkt ist, punkte umdrehen
int tmpx = start_x;
int tmpy = start_y;
start_x = end_x;
start_y = end_y;
end_x = tmpx;
end_y = tmpy;
}
int sx = start_x;
int sy = start_y;
int ex = end_x;
int ey = end_y;
bool fliphor = false; //spiegeln
bool flipbisec = false; //spiegeln
if(ey < sy){ //wenn endpunkt ueber startpunkt liegt, horizontal spiegeln
ey *= -1;
sy *= -1;
fliphor = true;
}
if((ex-sx)<(ey-sy)){ //wenn Steigung groeßer als 45° spiegeln an der Winkelhalbierenden
int tmp = sx;
sx = sy;
sy = tmp;
tmp = ex;
ex = ey;
ey = tmp;
flipbisec = true;
}
set_point(test, sx, sy, fliphor, flipbisec); //plotten des startpunktes
while(sx < ex){
++sx;
if((ey-sy)*2 > (ex-sx)){//wenn auch nach y gelaufen wird
++sy;
}
set_point(test, sx, sy, fliphor, flipbisec);
}
}
int main(){
connect_to_robot();
initialize_robot();
set_origin();
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
initialize_maze();
reset_motor_encoders();
int i;
for (i = 0; i < 17; i++){
set_point(nodes[i]->x, nodes[i]->y);
}
double curr_coord[2] = {0, 0};
map(curr_coord, nodes[0]);
breadthFirstSearch(nodes[0]);
reversePath(nodes[16]);
printPath(nodes[0]);
struct point* tail = malloc(sizeof(struct point));
tail->x = nodes[0]->x;
tail->y = nodes[0]->y;
struct point* startpoint = tail;
build_path(tail, nodes[0]);
// Traverse to end node.
while(tail->next){
set_point(tail->x, tail->y); // Visual display for Simulator only.
tail = tail->next;
}
tail->next = NULL; // Final node point to null.
printf("tail: X = %f Y = %f \n", tail->x, tail->y);
parallel(curr_coord);
spin(curr_coord, to_rad(180));
sleep(2);
set_ir_angle(LEFT, 45);
set_ir_angle(RIGHT, -45);
mazeRace(curr_coord, nodes[0]);
return 0;
}
void printMap(double map[MAP_SIZE][2])
{
//prints the built map in the simulator.
int i = 1;
while(map[i][0] != 0 || map[i][1] != 0) //when another [0,0] is reached, that means the trail has reached its end
{
set_point(map[i][0], map[i][1]);
i++;
}
}
void return_to_origin()
{
set_point(0, 0);
//returns the robot to the origin position.
turnRight(PHASE1_SPEED, PHASE1_SLOW, (2 * M_PI - bearing) * RADIANS_TO_DEGREES); //get the robot to face origin
double dist_to_origin = getDistance(xpos, ypos, 0, 0);
moveForwards(PHASE1_SPEED, PHASE1_SLOW, dist_to_origin * CM_TO_ENCODER); //move to origin
fixBearing(0); //resets robot's bearing to face upwards
}
void ft_parser(t_env *e)
{
char **spl_line;
char **spl_line2;
int y;
y = 0;
get_next_line(e->fd, &(e->file));
spl_line = ft_strsplit(e->file, ' ');
while (get_next_line(e->fd, &(e->file)))
{
ft_putendl(e->file);
spl_line2 = ft_strsplit(e->file, ' ');
set_point(e, spl_line, y);
set_point2(e, spl_line, spl_line2, y);
spl_line = spl_line2;
y += 50;
}
set_point(e, spl_line, y);
//ft_free_tab(spl_line);
//ft_free_tab(spl_line2);
}
void set_ipoint(struct ipoint *i, const struct shape *shp,
const struct ray *ray, enum side s, double k)
{
i->shape = shp;
i->ray = *ray;
i->side = s;
i->k = k;
set_point((struct coord*) i,
k * Vx + Sx,
k * Vy + Sy,
k * Vz + Sz
);
}
void gere_key3(int keycode, t_env *env)
{
if (keycode == 65463)
ANGLEX -= 0.05;
if (keycode == 65465)
ANGLEX += 0.05;
if (keycode == 65459)
ANGLEZ -= 0.05;
if (keycode == 65457)
ANGLEZ += 0.05;
if (keycode == 65469 && CHOICE == 1)
{
CHOICE = 0;
OFF = set_point(0, 0, 0);
get_points(env);
}
if (keycode == 65455 && CHOICE == 0)
{
CHOICE = 1;
OFF = set_point(0, 0, 0);
get_points(env);
}
}
void setSquareCenters()
{
adjustStart();
set_origin();
resetDist();
int i, j;
for (i = 0; i < 240; i += 60) {
for (j = 60; j < 300; j += 60) {
set_point(i, j);
}
}
}
int set_point_shift(Quark *pset, int seti, const VVector *vshift)
{
WPoint wp;
VPoint vp;
if (get_point(pset, seti, &wp) == RETURN_SUCCESS &&
Wpoint2Vpoint(pset, &wp, &vp) == RETURN_SUCCESS) {
vp.x += vshift->x;
vp.y += vshift->y;
Vpoint2Wpoint(pset, &vp, &wp);
return set_point(pset, seti, &wp);
} else {
return RETURN_FAILURE;
}
}
void print_square_centres()
{
//prints the centre of each square to the simulator.
int x = 0;
while(x <= SQUARE_DIST * 3)
{
int y = SQUARE_DIST - START_OFFSET;
while(y <= SQUARE_DIST * 4 - START_OFFSET)
{
set_point(x, y);
y += SQUARE_DIST;
}
x += SQUARE_DIST;
}
}
void setup()
{
connect_to_robot();
initialize_robot();
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
set_origin();
set_point(0, 0);
print_square_centres();
get_motor_encoders(&leftprev, &rightprev);
moveBackwards(PHASE1_SPEED, PHASE1_SLOW, START_OFFSET * CM_TO_ENCODER); //move robot to centre of first square.
}
/**
* set a integer var to the engine
*/
size_t engine_setint(const char * name, struct request_packet_t *req) {
size_t retval = 0;
if (!strcmp(name, engine->rpm_name)) {
set_rpm(req);
}
if (!strcmp(name, "setpoint")) {
set_point(req);
}
if (!strcmp(name, engine->rpm_max_name)) {
set_rpm_max(req);
}
return retval;
}
void smycka()
{
int x1,y1,z1;
int x2;
for(y1= 351;y1>178;y1--)
{
z1= vypocet_z(y1);
for(x1= 0;x1<639;x1++)
{
x2= vypocet_x(x1-70,z1);
set_point(x1,y1+9+16,get_point(x2,z1+350));
}
/*if (!(y1 & 7))*/{put_picture(0,0,out);showview(0,0,0,0);}
}
}