int YARPMEIDeviceDriver::vMove(void *spds) { int16 rc = 0; double *cmds = (double *) spds; for(int i = 0; i < _njoints; i++) rc = v_move(i, cmds[i], _ref_accs[i]); return rc; }
int YARPMEIDeviceDriver::safeVMove(void *spds) { int16 rc = 0; double *cmds = (double *) spds; for(int i = 0; i < _njoints; i++) { if (!frames_left(i)) rc = v_move(i, cmds[i], _ref_accs[i]); } return rc; }
//-------------------------------------------------------------------------- void Omu_Integrator::solve(int kk, double tstart, double tend, const Omu_VariableVec &x, const Omu_VariableVec &u, Omu_Program *sys, Omu_DependentVec &Ft, Omu_StateVec &xt) { _kk = kk; // store references to data required by residual() for sys->continuous calls _sys = sys; _x_ptr = &x; _u_ptr = &u; _Ft_ptr = &Ft; _xt_ptr = &xt; // prepare call arguments for high-level solve v_move(xt, _nd, _n, _xc, 0); if (_sa) { m_move(xt.Sx, _nd, 0, _n, _nx, _xc.Sq, 0, 0); m_move(xt.Su, _nd, 0, _n, _nu, _xc.Sq, 0, _nx); } v_move(xt, 0, _nx, _q, 0); v_move(u, 0, _nu, _q, _nx); // call high-level solve solve(kk, tstart, tend, _xc, _dxc, _q, _Fcs[_k]); // return results v_move(_xc, 0, _n, xt, _nd); if (_sa) { m_move(_xc.Sq, 0, 0, _n, _nx, xt.Sx, _nd, 0); m_move(_xc.Sq, 0, _nx, _n, _nu, xt.Su, _nd, 0); } // release references to data required by residual() _sys = NULL; _x_ptr = NULL; _u_ptr = NULL; _Ft_ptr = NULL; _xt_ptr = NULL; }
//-------------------------------------------------------------------------- void Omu_Integrator::residual(int kk, double t, const Omu_StateVec &xc, const Omu_StateVec &dxc, const Omu_Vec &q, Omu_DependentVec &Fc) { if (!_sys) { m_error(E_NULL, "Omu_Integrator::residual"); } // prepare call arguments Omu_DependentVec &Ft = *_Ft_ptr; Omu_StateVec &xt = *_xt_ptr; v_move(q, 0, _nd, xt, 0); v_move(xc, 0, _n, xt, _nd); v_move(dxc, 0, _n, _dxt, _nd); v_move(q, _nx, _nu, _ut, 0); Ft.set_required_J(Fc.is_required_J()); // note: treat seed derivatives xc.Sq and dxc.Sq to support forward mode // call continuous of Omu_Program _sys->continuous(kk, t, xt, _ut, _dxt, Ft); // return results v_move(Ft, _nd, _n, Fc, 0); if (Fc.is_required_J()) { if (!Fc.Jx.is_constant()) m_move(Ft.Jx, _nd, _nd, _n, _n, Fc.Jx, 0, 0); if (!Fc.Jdx.is_constant()) m_move(Ft.Jdx, _nd, _nd, _n, _n, Fc.Jdx, 0, 0); if (!Fc.Jq.is_constant()) { m_move(Ft.Jx, _nd, 0, _n, _nd, Fc.Jq, 0, 0); m_move(Ft.Ju, _nd, 0, _n, _nu, Fc.Jq, 0, _nx); } } }
//-------------------------------------------------------------------------- void Omu_Integrator::solve(int kk, double tstart, double tend, Omu_StateVec &xc, Omu_StateVec &dxc, Omu_Vec &q, Omu_DependentVec &Fc) { // call depreciated solve and store results in _xc Omu_StateVec &xt = *_xt_ptr; Omu_SVarVec &sx = (Omu_SVarVec &)*_x_ptr; if (_sa) { solve(kk, tstart, tend, sx, *_u_ptr, _sys, xt, xt.Sx, xt.Su); m_move(xt.Sx, _nd, 0, _n, _nx, _xc.Sq, 0, 0); m_move(xt.Su, _nd, 0, _n, _nu, _xc.Sq, 0, _nx); } else { solve(kk, tstart, tend, sx, *_u_ptr, _sys, xt, MNULL, MNULL); } v_move(xt, _nd, _n, _xc, 0); }
int main() { int i; struct VEngine veng; struct World world; JoyState* joy; v_init(&veng); joy_init(); /* for a random seed, wait for user input */ i = 0; while (1) { vid_vsync(); key_poll(); v_putbgtile(5, 4, 37); v_putbgtile(6, 4, 48); v_putbgtile(7, 4, 51); v_putbgtile(8, 4, 51); v_putbgtile(9, 4, 51); if (key_hit(KEY_A) || key_hit(KEY_START)) { break; } i ++; } /* sqran(i); */ sqran(4); init_world(&world, &veng); /* obj_set_pos(dude, (16 * 7), (16 * 4)); */ v_draw_bg(world.map, &veng, world.map->dudex, world.map->dudey); while(1) { vid_vsync(); joy = joy_poll(); key_poll(); if (key_hit(KEY_A)) { /* v_draw_more_bg(world.map, veng.mapofsx, veng.mapofsy - 4, 15, 4, world.map->dudex - 6 - veng.dudeposx, world.map->dudey - 4 - 4 - veng.dudeposy); */ } if (key_hit(KEY_START)) { v_init(&veng); init_world(&world, &veng); /* obj_set_pos(veng.obj_buffer[veng.spr_dude], (16 * 7), (16 * 4)); */ v_draw_bg(world.map, &veng, world.map->dudex, world.map->dudey); } /* if (key_hit(KEY_LEFT)) { switch (r_move_left(&world)) { case 1: v_move_left(&veng); break; case 2: v_shake_dude(&veng, 1, 0); break; } } else if (key_hit(KEY_RIGHT)) { switch (r_move_right(&world)) { case 1: v_move_right(&veng); break; case 2: v_shake_dude(&veng, 1, 0); break; } } else if (key_hit(KEY_UP)) { switch (r_move_up(&world)) { case 1: v_move_up(&veng); break; case 2: v_shake_dude(&veng, 0, 1); break; } } else if (key_hit(KEY_DOWN)) { switch (r_move_down(&world)) { case 1: v_move_down(&veng); break; case 2: v_shake_dude(&veng, 0, 1); break; } } */ if (joy->moving) { switch (r_move(&world, joy->dx, joy->dy)) { case 1: v_move(&veng, joy->dx, joy->dy); break; case 2: v_shake_dude(&veng, 0, 1); break; } joy_handled(); } v_move_co(&veng); v_scroll_at_edge(&veng, &world); v_draw_dude(&veng); for (i = 0; i < world.monstercount; i ++) { v_draw_monster(&veng, &world, i); } oam_copy(oam_mem, veng.obj_buffer, 1 + world.monstercount); } return 0; }