void set_velocity() { /* Set the velocity of the motor using one of the control schemes */ uint8_t mode = PARAMETERS[4]; switch (mode) { case SPRING: use_spring(); break; case DAMPER: use_damper(); break; case TEXTURE: use_texture(); break; case WALL: use_wall(); break; default: MD_SPEED.w = 0; md_velocity(&md1, MD_SPEED.w, MD_DIRECTION); return; } // Multiply by appropriate K value and compensate for the dead zone MD_SPEED.w = MD_SPEED.w * PARAMETERS[mode] + 0x1000; // Command motor md_velocity(&md1, MD_SPEED.w, MD_DIRECTION); }
/* * Draw the topography. * Input: time - the timestep number * texture_flag - 0=no texture, 1=texture map * flat_flag - 0=draw w/ topo heights, 1=draw flat */ void draw_topo( Display_Context dtx, int time, int texture_flag, int flat_flag ) { /* MJK 12.02.98 begin */ int i, j, n, ir, ic, nr, nc, nr2, nc2; int_vert2 *verts; int_1 *norms; uint_1 *color; /* MJK 12.02.98 end */ struct Topo *topo; topo = dtx->topo; set_color( 0xffffffff ); if (flat_flag) { if (texture_flag) { /* flat texture map */ use_texture( dtx, time ); texture_quadmeshnorm( topo->qrows, topo->qcols, (void*) topo->TopoFlatVertex, NULL, (void*) topo->TopoTexcoord ); } else { /* draw nothing */ } } else { if (texture_flag) { /* textured topo */ use_texture( dtx, time ); texture_quadmeshnorm( topo->qrows, topo->qcols, (void*) topo->TopoVertex, (void*) topo->TopoNormal, (void*) topo->TopoTexcoord ); } else { /* untextured topo */ uint_1 *indexes; unsigned int *color_table; if (topo->TopoColorVar<0) { color_table = dtx->ColorTable[VIS5D_TOPO_CT]->Colors[MAXVARS*VIS5D_MAX_CONTEXTS]; indexes = topo->TopoIndexes[MAXTIMES]; } else { color_table = dtx->ColorTable[VIS5D_TOPO_CT]->Colors[ topo->TopoColorVarOwner * MAXVARS + topo->TopoColorVar ]; indexes = topo->TopoIndexes[time]; if (!indexes) { indexes = topo->TopoIndexes[MAXTIMES]; } } /* MJK 12.02.98 begin */ if (topo->TopoStripsVerts == NULL) return; if (topo->TopoStripsNorms == NULL) return; verts = topo->TopoStripsVerts; norms = topo->TopoStripsNorms; nr = topo->qrows; nc = topo->qcols; nr2 = nr * 2; nc2 = nc * 2; n = (nr > nc) ? nr : nc; color = (uint_1 *) malloc ((n * 2 * sizeof (uint_1))); if (color == NULL) return; /* topography */ j = 0; i = nc; for (ir = 1; ir < nr; ir++) { n = 0; for (ic = 0; ic < nc; ic++) { color[n++] = indexes[i++]; color[n++] = indexes[j++]; } draw_colored_triangle_strip (nc2, (void *) verts, (void *) norms, color, color_table, 255); verts += nc2 * 3; norms += nc2 * 3; } if (topo->DisplayTopoBase) { unsigned int base_color = TOPO_BASE_COLOR; int norm_dir = 1; /* MJK 3.29.99 */ clipping_off(); n = (nr > nc) ? nr : nc; memset (color, 0, (n * 2 * sizeof (uint_1))); /* MJK reversed this 2.16.99 norm_dir = (topo->TopoBaseLev < 0.0) ? -1 : 1; */ /* MJK 3.29.99 don't know why this is here norm_dir = (topo->TopoBaseLev < 0.0) ? -1 : 1; */ norm_dir = 1; /* north side */ if ((check_face_norm(verts) * norm_dir > 0)) draw_colored_triangle_strip (nc2, (void *) verts, (void *) norms, color, &base_color, 255); verts += nc2 * 3; norms += nc2 * 3; /* south side */ if ((check_face_norm(verts) * norm_dir) > 0) draw_colored_triangle_strip (nc2, (void *) verts, (void *) norms, color, &base_color, 255); verts += nc2 * 3; norms += nc2 * 3; /* west side */ if ((check_face_norm(verts) * norm_dir) > 0) draw_colored_triangle_strip (nr2, (void *) verts, (void *) norms, color, &base_color, 255); verts += nr2 * 3; norms += nr2 * 3; /* east side */ if ((check_face_norm(verts) * norm_dir) > 0) draw_colored_triangle_strip (nr2, (void *) verts, (void *) norms, color, &base_color, 255); verts += nr2 * 3; norms += nr2 * 3; /* bottom */ if ((check_face_norm(verts) * norm_dir) > 0) { float r, g, b, a, fac = 0.90; /* color the bottom slightly darker than the sides */ r = (((float) UNPACK_RED (base_color)) / 255.0) * fac; g = (((float) UNPACK_GREEN (base_color)) / 255.0) * fac; b = (((float) UNPACK_BLUE (base_color)) / 255.0) * fac; a = ((float) UNPACK_ALPHA (base_color)) / 255.0; base_color = PACK_COLOR ((int) (r * 255.0), (int) (g * 255.0), (int) (b * 255.0), (int) (a * 255.0)); for (ir = 1; ir < nr; ir++) { draw_colored_triangle_strip (nc2, (void *) verts, (void *) norms, color, &base_color, 255); verts += nc2 * 3; norms += nc2 * 3; } } /* MJK 3.29.99 */ clipping_on(); } free (color); } } }