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);
}
}
}
