Esempio n. 1
0
inline double interp(double *img, int w, int h, double x, double y)
{
  register int x0 = (int) x, y0 = (int) y, x1 = x0+1, y1 = y0+1;
  register double rx0 = x-x0, rx1 = 1-rx0, ry = y-y0;
  return ((rx1*ARR(img,y0,x0,h,w) + rx0*ARR(img,y0,x1,h,w))*(1-ry) +
          (rx1*ARR(img,y1,x0,h,w) + rx0*ARR(img,y1,x1,h,w))*ry);
}
Esempio n. 2
0
static void
gen_tf (const struct parse_tf *tf, FILE *out, FILE *f_strs, const array_t *arrs,
        int narrs)
{
    char *buf_deps, *buf_ports;
    size_t sz_deps, sz_ports;
    FILE *f_ports = open_memstream (&buf_ports, &sz_ports);
    FILE *f_deps = open_memstream (&buf_deps, &sz_deps);

    int start = ftell (out);
    struct tf hdr = {ftell (f_strs) + VALID_OFS, tf->nrules};

    if (tf->prefix) fwrite (tf->prefix, 1, strlen (tf->prefix) + 1, f_strs);
    else hdr.prefix = 0;
    fwrite (&hdr, sizeof hdr, 1, out);
    /* TODO: Alignment? */

    struct rule rules[hdr.nrules];
    memset (rules, 0, sizeof rules);

    int i = 0;
    for (struct parse_rule *r = tf->rules.head; r; r = r->next, i++) {
        struct rule *tmp = &rules[i];
        tmp->idx = r->idx;
        tmp->in = gen_ports (ARR (r->in), r->in.n, f_ports);
        tmp->out = gen_ports (ARR (r->out), r->out.n, f_ports);
        tmp->match = arr_find (r->match, arrs, narrs);
        tmp->mask = arr_find (r->mask, arrs, narrs);
        tmp->rewrite = arr_find (r->rewrite, arrs, narrs);
        if (r->deps.head) tmp->deps = gen_deps (&r->deps, f_deps, f_ports, arrs, narrs);
        //tmp->desc = barfoo;
    }
    fclose (f_ports);
    fclose (f_deps);

    qsort (rules, hdr.nrules, sizeof *rules, rule_cmp);
    fwrite (rules, hdr.nrules, sizeof *rules, out);

    hdr.map_ofs = ftell (out) - start;
    gen_map (out, &tf->in_map, rules, ARR_LEN (rules));

    hdr.ports_ofs = ftell (out) - start;
    fwrite (buf_ports, 1, sz_ports, out);
    free (buf_ports);

    hdr.deps_ofs = ftell (out) - start;
    fwrite (buf_deps, 1, sz_deps, out);
    free (buf_deps);

    int end = ftell (out);
    fseek (out, start, SEEK_SET);
    fwrite (&hdr, sizeof hdr, 1, out);
    fseek (out, end, SEEK_SET);
}
Esempio n. 3
0
/*
 * Creates a new named scaffold.
 *
 * Returns scaffold pointer on success.
 *         NULL on failure
 */
scaffold_t *scaffold_new(GapIO *io, char *name) {
    tg_rec rec;
    scaffold_t *f, init_f;

    if (!io->db->scaffold)
	return NULL;

    memset(&init_f, 0, sizeof(scaffold_t));
    init_f.name = name;

    /* Allocate our contig */
    rec = cache_item_create(io, GT_Scaffold, &init_f);

    /* Initialise it */
    f = (scaffold_t *)cache_search(io, GT_Scaffold, rec);
    f = cache_rw(io, f);

    if (name)
        scaffold_set_name(io, &f, name);
    else
        f->name = NULL;

    /* Add it to the scaffold order too */
    io->scaffold = cache_rw(io, io->scaffold);
    io->db = cache_rw(io, io->db);
    ARR(tg_rec, io->scaffold, io->db->Nscaffolds++) = rec;

    /* Add to the new contigs list */
    if (name)
	add_to_list("new_scaffolds", name);

    return f;
}
// ============================================================================
int main(int argc,char **argv){
  CarSimulator sim;

  arr u = {.1, .2}; // fixed control signal
  arr z;

  ExtendedKalmanFilter ekf = ExtendedKalmanFilter(sim);
  ekf.mu = {0., 0., 0.};
  ekf.Sigma = eye(3);
  // TODO set the matrices R, Q, and I of the ekf
  // sim.observationNoise (use when evaluating a particle likelihood)
  // sim.dynamicsNoise (use when propagating a particle)

  sim.gl->watch();
  for (uint t=0;t<1000;t++) {
    sim.step(u);
    sim.getRealNoisyObservation(z);

    // EKF
    ekf.predict(u);
    ekf.correct(z);

    // draw the belief
    sim.gaussiansToDraw.resize(1);
    sim.gaussiansToDraw(0).a = ekf.mu.sub(0, 1);
    sim.gaussiansToDraw(0).A = ekf.Sigma.sub(0, 1, 0, 1);

    arr tmp = ARR(sim.x, sim.y);
    cout << "err trans: " << length(ekf.mu.sub(0, 1) - tmp) << endl;;
  }

  return 0;
}
Esempio n. 5
0
void simpleArrayOperations() {
    arr x;
    x = ARR(.1, .2, .3);         //directly setting the array
    cout <<"x = " <<x <<endl;

    x += ARR(2., 2., 2.);        //adding to an array
    cout <<"x = " <<x <<endl;

    x *= 1.;
    cout <<"x = " <<x <<endl;

    arr y = ARR(-.3,-.2,-.1);
    y.append(x);                 //appending a vector to a vector
    cout <<"y = " <<y <<endl;

    arr M(4,3);                  //some 4 x 3 matrix
    M[0] = ARR(1, 0, 0);         //setting the first row
    M[1] = ARR(0, 1, 0);
    M[2] = ARR(0, 0, 1);
    M[3] = ARR(1, 0, 0);

    cout <<"M =\n" <<M <<endl;

    cout <<"transpose M =\n" <<~M <<endl;  //matrix transpose

    cout <<"M*x = " <<M*x <<endl;          //matrix-vector product

    cout <<"M^-1 =\n" <<inverse(M) <<endl; //matrix inverse

    M.append(M);                 //appending a matrix to a matrix
    cout <<"M =\n" <<M <<endl;
}
Esempio n. 6
0
File: parse.c Progetto: NetSys/sts
static void
add_rule (struct parse_tf *tf, struct parse_rule *r)
{
  r->idx = ++tf->nrules;
  list_append (&tf->rules, r);

  for (int i = 0; i < r->in.n; i++) {
    struct map_elem *e = map_find_create (&tf->in_map, ARR (r->in)[i]);
    struct map_val *tmp = xmalloc (sizeof *tmp);
    tmp->val = r;
    list_append (&e->vals, tmp);
  }
}
Esempio n. 7
0
File: NCCE.c Progetto: 8l/rose
int main(int argc, char const *argv[], char const *envp[]) {
  PRE();
  DCL();
  EXP();
  INT();
  FLP();
  ARR();
  STR();
  MEM();

  ENV(envp);
  SIG();
  ERR();
  MSC();
  POS();

  return 0;
}
Esempio n. 8
0
void
generate_soil( uint8_t *dst[3], int wid, int hgt )
{
    int     row;
    int     col;
    int     component_idx;
    
    // Clear the frames
    for (row = 0; row < hgt; ++row)
    {
       for (col = 0; col < wid; ++col)
       {
            for (component_idx = 0; component_idx < 3; ++component_idx)
            {
                ARR( dst[component_idx], col, row, wid ) = RND_RANGE(0,16);
            }
       }
    }
}
Esempio n. 9
0
python::object TailGF::__factory_from_dict__(const python::object & dic){
  python::dict d(dic);
  int omin= python::extract<int>(d["OrderMinMIN"]);
  int omax= python::extract<int>(d["OrderMaxMAX"]);
  int omax2= python::extract<int>(d["OrderMax"]);
  python::list indL, indR;
  if (d.has_key("Indices")) {
    indL = obj_or_string_to_list( d["Indices"]);
    //indL = python::list(d["Indices"]);
    indR = indL;
  } else {
    indL = obj_or_string_to_list(d["IndicesL"]);
    indR = obj_or_string_to_list(d["IndicesR"]);
    //indL = python::list(d["IndicesL"]);
    //indR = python::list(d["IndicesR"]);
  }
  python::object arr(d["array"]);
  PyArray<COMPLEX,3> ARR(arr);
  if (ARR.extent(0) !=( omax - omin +1)) TRIQS_RUNTIME_ERROR<<"Reconstruction : Inconsistent data for TailGF";
  return python::object(TailGF(omin,omax,arr,omax2,indL,indR));
}
Esempio n. 10
0
// ============================================================================
int main(int argc,char **argv){
  CarSimulator sim;

  arr u = {.1, .2}; // fixed control signal
  arr z;

  ExtendedKalmanFilter ekf = ExtendedKalmanFilter(sim);
  ekf.mu = {0., 0., 0.};
  ekf.Sigma = eye(3);
  ekf.I = eye(3);
  ekf.R = sim.dynamicsNoise * eye(3,3);
  ekf.Q = sim.observationNoise * eye(4,4);
  std::cout << "R: " << ekf.R << std::endl;
  std::cout << "Q: " << ekf.Q << std::endl;
  // sim.observationNoise (use when evaluating a particle likelihood)
  // sim.dynamicsNoise (use when propagating a particle)

  sim.gl->watch();
  for (uint t=0;t<1000;t++) {
    sim.step(u);
    sim.getRealNoisyObservation(z);

    // EKF
    ekf.predict(u);
    ekf.correct(z);

    // draw the belief
    sim.gaussiansToDraw.resize(1);
    sim.gaussiansToDraw(0).a = ekf.mu.sub(0, 1);
    sim.gaussiansToDraw(0).A = ekf.Sigma.sub(0, 1, 0, 1);

    arr tmp = ARR(sim.x, sim.y);
    cout << "err trans: " << length(ekf.mu.sub(0, 1) - tmp) << endl;

    // sleep for a few milliseconds or else the animation will be too fast to watch
    usleep(7e3);
  }

  return 0;
}
Esempio n. 11
0
File: parse.c Progetto: NetSys/sts
static struct arr_ptr_uint32_t
read_array (char *s, uint32_t *res)
{
  uint32_t buf[MAX_ARR_SIZE];
  if (!res) res = buf;

  int end, n = 0;
  if (*s == '[') { s++; s[strlen (s) - 1] = 0; }
  while (sscanf (s, " %" SCNu32 "%n", &res[n], &end) == 1) {
    n++; s += end;
    if (*s == ',') s++;
  }

  struct arr_ptr_uint32_t tmp = {0};
  if (!n) return tmp;

  qsort (res, n, sizeof *res, int_cmp);
  tmp.n = n;
  if (res == buf) {
    ARR_ALLOC (tmp, n);
    memcpy (ARR (tmp), buf, n * sizeof *buf);
  }
  return tmp;
}
Esempio n. 12
0
void ors::Mesh::setImplicitSurface(ScalarFunction& f, double lo, double hi, uint res) {
  MarchingCubes mc(res, res, res);
  mc.init_all() ;
  
  //compute data
  uint k=0, j=0, i=0;
  float x=lo, y=lo, z=lo;
  for(k=0; k<res; k++) {
    z = lo+k*(hi-lo)/res;
    for(j=0; j<res; j++) {
      y = lo+j*(hi-lo)/res;
      for(i=0; i<res; i++) {
        x = lo+i*(hi-lo)/res;
        mc.set_data(f.fs(NoArr, NoArr, ARR(x, y, z)), i, j, k) ;
      }
    }
  }
  
  mc.run();
  mc.clean_temps();
  
  //convert to Mesh
  clear();
  V.resize(mc.nverts(), 3);
  T.resize(mc.ntrigs(), 3);
  for(i=0; i<V.d0; i++) {
    V(i, 0)=lo+mc.vert(i)->x*(hi-lo)/res;
    V(i, 1)=lo+mc.vert(i)->y*(hi-lo)/res;
    V(i, 2)=lo+mc.vert(i)->z*(hi-lo)/res;
  }
  for(i=0; i<T.d0; i++) {
    T(i, 0)=mc.trig(i)->v1;
    T(i, 1)=mc.trig(i)->v2;
    T(i, 2)=mc.trig(i)->v3;
  }
}
Esempio n. 13
0
#include "vario.h"

#include "protocols/protocol.h"

#include "logger/logger.h"

#include "../gui/gui_dialog.h"
#include "../gui/widgets/acc.h"

volatile flight_computer_data_t fc;

Timer fc_meas_timer;

extern KalmanFilter kalmanFilter;

MK_SEQ(gps_ready, ARR({750, 0, 750, 0, 750, 0}), ARR({250, 150, 250, 150, 250, 150}));

#define FC_LOG_BATTERY_EVERY	(5 * 60 * 1000ul)
uint32_t fc_log_battery_next = 0;

void fc_init()
{
    DEBUG(" *** Flight computer init ***\n");

    //start values
    active_page = config.gui.last_page;
    if (active_page >= config.gui.number_of_pages)
        active_page = 0;

    //reset flight status
    fc_reset();
Esempio n. 14
0
	R13SVC, R14SVC, R13ABT, R14ABT,
	R13IRQ, R14IRQ, R13UND, R14UND,
	SPSRFIQ, SPSRSVC, SPSRABT, SPSRIRQ, SPSRUND,
	R8USR, R9USR, R10USR, R11USR, R12USR,
	R8FIQ, R9FIQ, R10FIQ, R11FIQ, R12FIQ
};
u32int r[16], cpsr, spsr;
u32int saver[R12FIQ+1];
u32int curpc;
int irq;

u32int instr0, instr1, pipel = -1;
int cyc, trace;

Var cpuvars[] = {
	ARR(r), VAR(cpsr), VAR(spsr), ARR(saver), VAR(irq),
	VAR(instr0), VAR(instr1), VAR(pipel),
	{nil, 0, 0},
};

#define pipeflush() {io(); pipel = -1;}
#define io() cyc++

static int steparm(void);
static int stepthumb(void);
int (*step)(void);

void
undefined(u32int instr)
{
	if((cpsr & FLAGT) != 0)
Esempio n. 15
0
void soc::SocSystem_Ors::initStandardBenchmark(uint rand_seed){
  uint K = MT::getParameter<uint>("segments");
  uint T = MT::getParameter<uint>("trajectoryLength");
  dynamic = MT::getParameter<bool>("isDynamic");
  real margin = MT::getParameter<real>("margin");
  bool useTruncation = MT::getParameter<bool>("useTruncation");

  //generate the configuration
  ors::Body *b,*target,*endeff;  ors::Shape *s;  ors::Joint *j;
  MT::String str;
  ors=new ors::Graph;
  //the links
  for(uint k=0;k<=K;k++){
    b=new ors::Body(ors->bodies);
    b->name = STRING("body"<<k);
    if(!k) b->fixed=true;
    s=new ors::Shape(ors->shapes,b);
    s->type=2;
    s->size[0]=.0; s->size[1]=.0; s->size[2]=1./K; s->size[3]=.2/K;
    s->rel.setText(STRING("<t(0 0 "<<.5/K<<")>"));
    if(k&1){ s->color[0]=.5; s->color[1]=.2; s->color[2]=.2; }
    else   { s->color[0]=.2; s->color[1]=.2; s->color[2]=.2; }
    s->cont=true;
    if(k){
      j=new ors::Joint(ors->joints,ors->bodies(k-1),ors->bodies(k));
      j->type = JHINGE;
      j->Q.setText("<d(45 1 0 0)>");
      if(k&1){ //odd -> rotation around z
        j->A.setText(STRING("<t(0 0 "<<1./K<<") d(-90 0 1 0)>"));
        j->B.setText("<d(90 0 1 0)>");
      }else{  //even -> rotation around x
        j->A.setText(STRING("<t(0 0 "<<1./K<<")>"));
      }
    }
  }
  endeff=b;
  //the target
  b=new ors::Body(ors->bodies);
  b->name = "target";
  b->X.setText("<t(.2 0 0)>");
  s=new ors::Shape(ors->shapes,b);
  s->read(STREAM("type=1 size=[.0 .0 .1 .02] color=[0 0 1]"));
  s=new ors::Shape(ors->shapes,b);
  s->read(STREAM("type=0 rel=<t(0 -.1 0)> size=[.2 .01 .3 .0] color=[0 0 0] contact"));
  s=new ors::Shape(ors->shapes,b);
  s->read(STREAM("type=0 rel=<t(0 .1 0)> size=[.2 .01 .3 .0] color=[0 0 0] contact"));
  s=new ors::Shape(ors->shapes,b);
  s->read(STREAM("type=0 rel=<t(.1 0 0)> size=[.01 .2 .3 .0] color=[0 0 0] contact"));
  graphMakeLists(ors->bodies,ors->joints);
  ors->calcNodeFramesFromEdges();
  target=b;
  
  if(rand_seed>0){
    rnd.seed(rand_seed);
    target->X.p(0) += .05*rnd.gauss();
    target->X.p(1) += .05*rnd.gauss();
    //target->X.p(2) += .05*rnd.gauss();
  }
  
  swift=new SwiftModule;
  swift->init(*ors,margin);
  
  gl = new OpenGL;
  gl->add(glStandardScene);
  gl->add(ors::glDrawGraph,ors);
  gl->camera.setPosition(0,-5,5);
  gl->camera.focus(0,0,.5);
  gl->watch("loaded configuration - press ENTER");

  if(dynamic){
    initPseudoDynamic(ors,swift,gl,3.,T);
  }else{
    initKinematic(ors,swift,gl,T);
  }
  os=&std::cout;
  
  real endPrec=MT::getParameter<real>("endPrec");
  real midPrec=MT::getParameter<real>("midPrec");
  real colPrec=MT::getParameter<real>("colPrec");
   //-- setup the control variables (problem definition)
  TaskVariable *pos = new TaskVariable("position" , *ors, posTVT, endeff->name, STRING("<t(0 0 "<<.5/K<<")>"),0,0, ARR());
  TaskVariable *col;
  if(!useTruncation) col = new TaskVariable("collision", *ors, collTVT,0,0,0,0,ARR(margin));
  else               col = new TaskVariable("collision", *ors, colConTVT,0,0,0,0,ARR(margin));
  setTaskVariables(TUPLE(pos,col));
      
  pos->y_target = arr(ors->getName("target")->X.p.v,3);
  pos->setInterpolatedTargetsEndPrecisions(T,midPrec,endPrec,0.,10*endPrec);
  if(col->type==collTVT){
    col->y        = ARR(0.);
    col->y_target = ARR(0.);
    col->setInterpolatedTargetsConstPrecisions(T,colPrec,0.);
  }else col->active=true;
}
Esempio n. 16
0
//
//  Clonify: C
//
// Clone the series embedded in a value *if* it's in the given set of types
// (and if "cloning" makes sense for them, e.g. they are not simple scalars).
//
// Note: The resulting clones will be managed.  The model for lists only
// allows the topmost level to contain unmanaged values...and we *assume* the
// values we are operating on here live inside of an array.
//
void Clonify(
    REBVAL *v,
    REBFLGS flags,
    REBU64 types
){
    if (C_STACK_OVERFLOWING(&types))
        Fail_Stack_Overflow();

    // !!! It may be possible to do this faster/better, the impacts on higher
    // quoting levels could be incurring more cost than necessary...but for
    // now err on the side of correctness.  Unescape the value while cloning
    // and then escape it back.
    //
    REBCNT num_quotes = VAL_NUM_QUOTES(v);
    Dequotify(v);

    enum Reb_Kind kind = cast(enum Reb_Kind, KIND_BYTE_UNCHECKED(v));
    assert(kind < REB_MAX_PLUS_MAX); // we dequoted it (pseudotypes ok)

    if (types & FLAGIT_KIND(kind) & TS_SERIES_OBJ) {
        //
        // Objects and series get shallow copied at minimum
        //
        REBSER *series;
        if (ANY_CONTEXT(v)) {
            INIT_VAL_CONTEXT_VARLIST(
                v,
                CTX_VARLIST(Copy_Context_Shallow_Managed(VAL_CONTEXT(v)))
            );
            series = SER(CTX_VARLIST(VAL_CONTEXT(v)));
        }
        else {
            if (IS_SER_ARRAY(VAL_SERIES(v))) {
                series = SER(
                    Copy_Array_At_Extra_Shallow(
                        VAL_ARRAY(v),
                        0, // !!! what if VAL_INDEX() is nonzero?
                        VAL_SPECIFIER(v),
                        0,
                        NODE_FLAG_MANAGED
                    )
                );

                INIT_VAL_NODE(v, series); // copies args

                // If it was relative, then copying with a specifier
                // means it isn't relative any more.
                //
                INIT_BINDING(v, UNBOUND);
            }
            else {
                series = Copy_Sequence_Core(
                    VAL_SERIES(v),
                    NODE_FLAG_MANAGED
                );
                INIT_VAL_NODE(v, series);
            }
        }

        // If we're going to copy deeply, we go back over the shallow
        // copied series and "clonify" the values in it.
        //
        if (types & FLAGIT_KIND(kind) & TS_ARRAYS_OBJ) {
            REBVAL *sub = KNOWN(ARR_HEAD(ARR(series)));
            for (; NOT_END(sub); ++sub)
                Clonify(sub, flags, types);
        }
    }
    else if (types & FLAGIT_KIND(kind) & FLAGIT_KIND(REB_ACTION)) {
        //
        // !!! While Ren-C has abandoned the concept of copying the body
        // of functions (they are black boxes which may not *have* a
        // body), it would still theoretically be possible to do what
        // COPY does and make a function with a new and independently
        // hijackable identity.  Assume for now it's better that the
        // HIJACK of a method for one object will hijack it for all
        // objects, and one must filter in the hijacking's body if one
        // wants to take more specific action.
        //
        assert(false);
    }
    else {
        // We're not copying the value, so inherit the const bit from the
        // original value's point of view, if applicable.
        //
        if (NOT_CELL_FLAG(v, EXPLICITLY_MUTABLE))
            v->header.bits |= (flags & ARRAY_FLAG_CONST_SHALLOW);
    }

    Quotify(v, num_quotes);
}
Esempio n. 17
0
bool GameProject::Load()
{
    QFile m_file(m_path+"project.erp");
    if (!m_file.open(QFile::ReadOnly | QFile::Text))
        return false;
    QTextStream in(&m_file);
    QString f_text;
    in >> f_text;
    picojson::value j_project;
    const char *json = f_text.toStdString().c_str();
    QString err = QString::fromStdString(picojson::parse(j_project,json,json+strlen(json)));
    if (!err.isEmpty())
        return false;
    m_gametitle = j_project.get("GameTitle").get<std::string>().c_str();
    m_tilesize = (int)j_project.get("TileSize").get<double>();
#define ARR(var,list,key)picojson::array var = j_project.get(key).get<picojson::array>(); for (picojson::array::iterator iter = var.begin(); iter != var.end(); ++iter) list.push_back((*iter).get<std::string>().c_str());
    ARR(j_backdrop,m_backdrop,"Backdrop");
    ARR(j_battle,m_battle,"Battle");
    ARR(j_battle2,m_battle2,"Battle2");
    ARR(j_battlecharset,m_battlecharset,"BattleCharSet");
    ARR(j_battleweapon,m_battleweapon,"BattleWeapon");
    ARR(j_charset,m_charset,"CharSet");
    ARR(j_chipset,m_chipset,"ChipSet");
    ARR(j_faceset,m_faceset,"FaceSet");
    ARR(j_frame,m_frame,"Frame");
    ARR(j_gameover,m_gameover,"GameOver");
    ARR(j_monster,m_monster,"Monster");
    ARR(j_movie,m_movie,"Movie");
    ARR(j_music,m_music,"Music");
    ARR(j_background,m_background,"Background");
    ARR(j_picture,m_picture,"Picture");
    ARR(j_sound,m_sound,"Sound");
    ARR(j_system,m_system,"System");
    ARR(j_system2,m_system2,"System2");
    ARR(j_title,m_title,"Title");
#undef ARR
    m_file.close();
    return true;
}
Esempio n. 18
0
void soc::SocSystem_Ors::initStandardReachProblem(uint rand_seed,uint T){
  MT::String orsFile = MT::getParameter<MT::String>("orsFile");
  if(!T) T = MT::getParameter<uint>("trajectoryLength");
  dynamic = MT::getParameter<bool>("isDynamic");
  real margin = MT::getParameter<real>("margin");
  MT::String endeffName= MT::getParameter<MT::String>("endeffName");
  MT::String endeffRel = MT::getParameter<MT::String>("endeffRel");
  bool standOnFoot = MT::getParameter<bool>("standOnFoot");
  bool useTruncation = MT::getParameter<bool>("useTruncation");

  
  //-- setup the simulator, swift, opengl
  WS->newedOrs=true;
  ors=new ors::Graph;
  ors->init(orsFile);
  if(standOnFoot){
    ors->reconfigureRoot(ors->getName("rfoot"));
    ors->calcNodeFramesFromEdges();
  }
  if(rand_seed>0){
    rnd.seed(rand_seed);
    ors::Body &t=*ors->getName("target");
    t.X.p(0) += .05*rnd.gauss();
    t.X.p(1) += .05*rnd.gauss();
    t.X.p(2) += .05*rnd.gauss();
  }
  
  swift=new SwiftModule;
  swift->init(*ors,margin);
  
  gl = new OpenGL;
  gl->add(glStandardScene);
  gl->add(ors::glDrawGraph,ors);
  gl->camera.setPosition(5,-10,10);
  gl->camera.focus(0,0,1);
  //gl->watch("loaded configuration - press ENTER");

  if(dynamic){
    initPseudoDynamic(ors,swift,gl,3.,T);
  }else{
    initKinematic(ors,swift,gl,T);
  }
  os=&std::cout;
  
  real endPrec=MT::getParameter<real>("endPrec");
  real midPrec=MT::getParameter<real>("midPrec");
  real colPrec=MT::getParameter<real>("colPrec");
  real balPrec=MT::getParameter<real>("balPrec");
   //-- setup the control variables (problem definition)
  TaskVariable *pos = new TaskVariable("position" , *ors, posTVT, endeffName, endeffRel,0,0, ARR());
  TaskVariable *col;
  if(!useTruncation) col = new TaskVariable("collision", *ors, collTVT,0,0,0,0,ARR(margin));
  else               col = new TaskVariable("collision", *ors, colConTVT,0,0,0,0,ARR(margin));
  TaskVariable *com = new TaskVariable("balance", *ors, comTVT,0,0,0,0,ARR());
  setTaskVariables(TUPLE(pos,col,com));
      
  pos->y_target = arr(ors->getName("target")->X.p.v,3);
  pos->setInterpolatedTargetsEndPrecisions(T,midPrec,endPrec,0.,10*endPrec);
  if(col->type==collTVT){
    col->y        = ARR(0.);
    col->y_target = ARR(0.);
    col->setInterpolatedTargetsConstPrecisions(T,colPrec,0.);
  }else col->active=true;
  if(balPrec){
    com->y_target = com->y;
    com->setInterpolatedTargetsConstPrecisions(T,balPrec,0.);
  }else com->active=false;
}
Esempio n. 19
0
//
// 柦椷幚峴
//
INT	CPU::EXEC( INT request_cycles )
{
BYTE	opcode;		// 僆儁僐乕僪
INT	OLD_cycles = TOTAL_cycles;
INT	exec_cycles;
BYTE	nmi_request, irq_request;
BOOL	bClockProcess = m_bClockProcess;

// TEMP
register WORD	EA;
register WORD	ET;
register WORD	WT;
register BYTE	DT;

	while( request_cycles > 0 ) {
		exec_cycles = 0;

		if( DMA_cycles ) {
			if( request_cycles <= DMA_cycles ) {
				DMA_cycles -= request_cycles;
				TOTAL_cycles += request_cycles;

				// 僋儘僢僋摨婜張棟
				mapper->Clock( request_cycles );
#if	DPCM_SYNCCLOCK
				apu->SyncDPCM( request_cycles );
#endif
				if( bClockProcess ) {
					nes->Clock( request_cycles );
				}
//				nes->Clock( request_cycles );
				goto	_execute_exit;
			} else {
				exec_cycles += DMA_cycles;
//				request_cycles -= DMA_cycles;
				DMA_cycles = 0;
			}
		}

		nmi_request = irq_request = 0;
		opcode = OP6502( R.PC++ );

		if( R.INT_pending ) {
			if( R.INT_pending & NMI_FLAG ) {
				nmi_request = 0xFF;
				R.INT_pending &= ~NMI_FLAG;
			} else
			if( R.INT_pending & IRQ_MASK ) {
				R.INT_pending &= ~IRQ_TRIGGER2;
				if( !(R.P & I_FLAG) && opcode != 0x40 ) {
					irq_request = 0xFF;
					R.INT_pending &= ~IRQ_TRIGGER;
				}
			}
		}

		//增加指令预测忽略功能

		//opcode
		BYTE iInstructionLen =1;
		switch (TraceAddrMode[opcode])
		{
			case IND:
			case ADR:
			case ABS:
			case ABX:
			case ABY:
				iInstructionLen = 3;
				break;
			case IMM:
			case ZPG:
			case ZPX:
			case ZPY:
			case INX:
			case INY:
				iInstructionLen = 2;
				break;
			case IMP:case ACC:case ERR:  break;
			case REL:iInstructionLen = 2;break;
			}

		if( ((TraceArr[opcode][0]=='*') ||
			 (TraceArr[opcode][1]=='?'))&&
			(!Config.emulator.bIllegalOp) )
		{
			//这里可以优化输出信息
			//char str[111];
			//DecodeInstruction (R.PC-1, str);			 
			//DEBUGOUT( "Bad Instruction:%s\n",str);
			R.PC=(R.PC-1)+iInstructionLen;
			ADD_CYCLE(iInstructionLen*2);
			goto end_is;
		}
		//
		

		switch( opcode ) {
			case	0x69: // ADC #$??
				MR_IM(); ADC();
				ADD_CYCLE(2);
				break;
			case	0x65: // ADC $??
				MR_ZP(); ADC();
				ADD_CYCLE(3);
				break;
			case	0x75: // ADC $??,X
				MR_ZX(); ADC();
				ADD_CYCLE(4);
				break;
			case	0x6D: // ADC $????
				MR_AB(); ADC();
				ADD_CYCLE(4);
				break;
			case	0x7D: // ADC $????,X
				MR_AX(); ADC(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0x79: // ADC $????,Y
				MR_AY(); ADC(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0x61: // ADC ($??,X)
				MR_IX(); ADC();
				ADD_CYCLE(6);
				break;
			case	0x71: // ADC ($??),Y
				MR_IY(); ADC(); CHECK_EA();
				ADD_CYCLE(4);
				break;

			case	0xE9: // SBC #$??
				MR_IM(); SBC();
				ADD_CYCLE(2);
				break;
			case	0xE5: // SBC $??
				MR_ZP(); SBC();
				ADD_CYCLE(3);
				break;
			case	0xF5: // SBC $??,X
				MR_ZX(); SBC();
				ADD_CYCLE(4);
				break;
			case	0xED: // SBC $????
				MR_AB(); SBC();
				ADD_CYCLE(4);
				break;
			case	0xFD: // SBC $????,X
				MR_AX(); SBC(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0xF9: // SBC $????,Y
				MR_AY(); SBC(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0xE1: // SBC ($??,X)
				MR_IX(); SBC();
				ADD_CYCLE(6);
				break;
			case	0xF1: // SBC ($??),Y
				MR_IY(); SBC(); CHECK_EA();
				ADD_CYCLE(5);
				break;

			case	0xC6: // DEC $??
				MR_ZP(); DEC();	MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0xD6: // DEC $??,X
				MR_ZX(); DEC(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0xCE: // DEC $????
				MR_AB(); DEC(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0xDE: // DEC $????,X
				MR_AX(); DEC(); MW_EA();
				ADD_CYCLE(7);
				break;

			case	0xCA: // DEX
				DEX();
				ADD_CYCLE(2);
				break;
			case	0x88: // DEY
				DEY();
				ADD_CYCLE(2);
				break;

			case	0xE6: // INC $??
				MR_ZP(); INC(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0xF6: // INC $??,X
				MR_ZX(); INC(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0xEE: // INC $????
				MR_AB(); INC(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0xFE: // INC $????,X
				MR_AX(); INC(); MW_EA();
				ADD_CYCLE(7);
				break;

			case	0xE8: // INX
				INX();
				ADD_CYCLE(2);
				break;
			case	0xC8: // INY
				INY();
				ADD_CYCLE(2);
				break;

			case	0x29: // AND #$??
				MR_IM(); AND();
				ADD_CYCLE(2);
				break;
			case	0x25: // AND $??
				MR_ZP(); AND();
				ADD_CYCLE(3);
				break;
			case	0x35: // AND $??,X
				MR_ZX(); AND();
				ADD_CYCLE(4);
				break;
			case	0x2D: // AND $????
				MR_AB(); AND();
				ADD_CYCLE(4);
				break;
			case	0x3D: // AND $????,X
				MR_AX(); AND(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0x39: // AND $????,Y
				MR_AY(); AND(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0x21: // AND ($??,X)
				MR_IX(); AND();
				ADD_CYCLE(6);
				break;
			case	0x31: // AND ($??),Y
				MR_IY(); AND(); CHECK_EA();
				ADD_CYCLE(5);
				break;

			case	0x0A: // ASL A
				ASL_A();
				ADD_CYCLE(2);
				break;
			case	0x06: // ASL $??
				MR_ZP(); ASL(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0x16: // ASL $??,X
				MR_ZX(); ASL(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0x0E: // ASL $????
				MR_AB(); ASL(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x1E: // ASL $????,X
				MR_AX(); ASL(); MW_EA();
				ADD_CYCLE(7);
				break;

			case	0x24: // BIT $??
				MR_ZP(); BIT();
				ADD_CYCLE(3);
				break;
			case	0x2C: // BIT $????
				MR_AB(); BIT();
				ADD_CYCLE(4);
				break;

			case	0x49: // EOR #$??
				MR_IM(); EOR();
				ADD_CYCLE(2);
				break;
			case	0x45: // EOR $??
				MR_ZP(); EOR();
				ADD_CYCLE(3);
				break;
			case	0x55: // EOR $??,X
				MR_ZX(); EOR();
				ADD_CYCLE(4);
				break;
			case	0x4D: // EOR $????
				MR_AB(); EOR();
				ADD_CYCLE(4);
				break;
			case	0x5D: // EOR $????,X
				MR_AX(); EOR(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0x59: // EOR $????,Y
				MR_AY(); EOR(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0x41: // EOR ($??,X)
				MR_IX(); EOR();
				ADD_CYCLE(6);
				break;
			case	0x51: // EOR ($??),Y
				MR_IY(); EOR(); CHECK_EA();
				ADD_CYCLE(5);
				break;

			case	0x4A: // LSR A
				LSR_A();
				ADD_CYCLE(2);
				break;
			case	0x46: // LSR $??
				MR_ZP(); LSR(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0x56: // LSR $??,X
				MR_ZX(); LSR(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0x4E: // LSR $????
				MR_AB(); LSR(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x5E: // LSR $????,X
				MR_AX(); LSR(); MW_EA();
				ADD_CYCLE(7);
				break;

			case	0x09: // ORA #$??
				MR_IM(); ORA();
				ADD_CYCLE(2);
				break;
			case	0x05: // ORA $??
				MR_ZP(); ORA();
				ADD_CYCLE(3);
				break;
			case	0x15: // ORA $??,X
				MR_ZX(); ORA();
				ADD_CYCLE(4);
				break;
			case	0x0D: // ORA $????
				MR_AB(); ORA();
				ADD_CYCLE(4);
				break;
			case	0x1D: // ORA $????,X
				MR_AX(); ORA(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0x19: // ORA $????,Y
				MR_AY(); ORA(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0x01: // ORA ($??,X)
				MR_IX(); ORA();
				ADD_CYCLE(6);
				break;
			case	0x11: // ORA ($??),Y
				MR_IY(); ORA(); CHECK_EA();
				ADD_CYCLE(5);
				break;

			case	0x2A: // ROL A
				ROL_A();
				ADD_CYCLE(2);
				break;
			case	0x26: // ROL $??
				MR_ZP(); ROL(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0x36: // ROL $??,X
				MR_ZX(); ROL(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0x2E: // ROL $????
				MR_AB(); ROL(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x3E: // ROL $????,X
				MR_AX(); ROL(); MW_EA();
				ADD_CYCLE(7);
				break;

			case	0x6A: // ROR A
				ROR_A();
				ADD_CYCLE(2);
				break;
			case	0x66: // ROR $??
				MR_ZP(); ROR(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0x76: // ROR $??,X
				MR_ZX(); ROR(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0x6E: // ROR $????
				MR_AB(); ROR(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x7E: // ROR $????,X
				MR_AX(); ROR(); MW_EA();
				ADD_CYCLE(7);
				break;

			case	0xA9: // LDA #$??
				MR_IM(); LDA();
				ADD_CYCLE(2);
				break;
			case	0xA5: // LDA $??
				MR_ZP(); LDA();
				ADD_CYCLE(3);
				break;
			case	0xB5: // LDA $??,X
				MR_ZX(); LDA();
				ADD_CYCLE(4);
				break;
			case	0xAD: // LDA $????
				MR_AB(); LDA();
				ADD_CYCLE(4);
				break;
			case	0xBD: // LDA $????,X
				MR_AX(); LDA(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0xB9: // LDA $????,Y
				MR_AY(); LDA(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0xA1: // LDA ($??,X)
				MR_IX(); LDA();
				ADD_CYCLE(6);
				break;
			case	0xB1: // LDA ($??),Y
				MR_IY(); LDA(); CHECK_EA();
				ADD_CYCLE(5);
				break;

			case	0xA2: // LDX #$??
				MR_IM(); LDX();
				ADD_CYCLE(2);
				break;
			case	0xA6: // LDX $??
				MR_ZP(); LDX();
				ADD_CYCLE(3);
				break;
			case	0xB6: // LDX $??,Y
				MR_ZY(); LDX();
				ADD_CYCLE(4);
				break;
			case	0xAE: // LDX $????
				MR_AB(); LDX();
				ADD_CYCLE(4);
				break;
			case	0xBE: // LDX $????,Y
				MR_AY(); LDX(); CHECK_EA();
				ADD_CYCLE(4);
				break;

			case	0xA0: // LDY #$??
				MR_IM(); LDY();
				ADD_CYCLE(2);
				break;
			case	0xA4: // LDY $??
				MR_ZP(); LDY();
				ADD_CYCLE(3);
				break;
			case	0xB4: // LDY $??,X
				MR_ZX(); LDY();
				ADD_CYCLE(4);
				break;
			case	0xAC: // LDY $????
				MR_AB(); LDY();
				ADD_CYCLE(4);
				break;
			case	0xBC: // LDY $????,X
				MR_AX(); LDY(); CHECK_EA();
				ADD_CYCLE(4);
				break;

			case	0x85: // STA $??
				EA_ZP(); STA(); MW_ZP();
				ADD_CYCLE(3);
				break;
			case	0x95: // STA $??,X
				EA_ZX(); STA(); MW_ZP();
				ADD_CYCLE(4);
				break;
			case	0x8D: // STA $????
				EA_AB(); STA(); MW_EA();
				ADD_CYCLE(4);
				break;
			case	0x9D: // STA $????,X
				EA_AX(); STA(); MW_EA();
				ADD_CYCLE(5);
				break;
			case	0x99: // STA $????,Y
				EA_AY(); STA(); MW_EA();
				ADD_CYCLE(5);
				break;
			case	0x81: // STA ($??,X)
				EA_IX(); STA(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x91: // STA ($??),Y
				EA_IY(); STA(); MW_EA();
				ADD_CYCLE(6);
				break;

			case	0x86: // STX $??
				EA_ZP(); STX(); MW_ZP();
				ADD_CYCLE(3);
				break;
			case	0x96: // STX $??,Y
				EA_ZY(); STX(); MW_ZP();
				ADD_CYCLE(4);
				break;
			case	0x8E: // STX $????
				EA_AB(); STX(); MW_EA();
				ADD_CYCLE(4);
				break;

			case	0x84: // STY $??
				EA_ZP(); STY(); MW_ZP();
				ADD_CYCLE(3);
				break;
			case	0x94: // STY $??,X
				EA_ZX(); STY(); MW_ZP();
				ADD_CYCLE(4);
				break;
			case	0x8C: // STY $????
				EA_AB(); STY(); MW_EA();
				ADD_CYCLE(4);
				break;

			case	0xAA: // TAX
				TAX();
				ADD_CYCLE(2);
				break;
			case	0x8A: // TXA
				TXA();
				ADD_CYCLE(2);
				break;
			case	0xA8: // TAY
				TAY();
				ADD_CYCLE(2);
				break;
			case	0x98: // TYA
				TYA();
				ADD_CYCLE(2);
				break;
			case	0xBA: // TSX
				TSX();
				ADD_CYCLE(2);
				break;
			case	0x9A: // TXS
				TXS();
				ADD_CYCLE(2);
				break;

			case	0xC9: // CMP #$??
				MR_IM(); CMP_();
				ADD_CYCLE(2);
				break;
			case	0xC5: // CMP $??
				MR_ZP(); CMP_();
				ADD_CYCLE(3);
				break;
			case	0xD5: // CMP $??,X
				MR_ZX(); CMP_();
				ADD_CYCLE(4);
				break;
			case	0xCD: // CMP $????
				MR_AB(); CMP_();
				ADD_CYCLE(4);
				break;
			case	0xDD: // CMP $????,X
				MR_AX(); CMP_(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0xD9: // CMP $????,Y
				MR_AY(); CMP_(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0xC1: // CMP ($??,X)
				MR_IX(); CMP_();
				ADD_CYCLE(6);
				break;
			case	0xD1: // CMP ($??),Y
				MR_IY(); CMP_(); CHECK_EA();
				ADD_CYCLE(5);
				break;

			case	0xE0: // CPX #$??
				MR_IM(); CPX();
				ADD_CYCLE(2);
				break;
			case	0xE4: // CPX $??
				MR_ZP(); CPX();
				ADD_CYCLE(3);
				break;
			case	0xEC: // CPX $????
				MR_AB(); CPX();
				ADD_CYCLE(4);
				break;

			case	0xC0: // CPY #$??
				MR_IM(); CPY();
				ADD_CYCLE(2);
				break;
			case	0xC4: // CPY $??
				MR_ZP(); CPY();
				ADD_CYCLE(3);
				break;
			case	0xCC: // CPY $????
				MR_AB(); CPY();
				ADD_CYCLE(4);
				break;

			case	0x90: // BCC
				MR_IM(); BCC();
				ADD_CYCLE(2);
				break;
			case	0xB0: // BCS
				MR_IM(); BCS();
				ADD_CYCLE(2);
				break;
			case	0xF0: // BEQ
				MR_IM(); BEQ();
				ADD_CYCLE(2);
				break;
			case	0x30: // BMI
				MR_IM(); BMI();
				ADD_CYCLE(2);
				break;
			case	0xD0: // BNE
				MR_IM(); BNE();
				ADD_CYCLE(2);
				break;
			case	0x10: // BPL
				MR_IM(); BPL();
				ADD_CYCLE(2);
				break;
			case	0x50: // BVC
				MR_IM(); BVC();
				ADD_CYCLE(2);
				break;
			case	0x70: // BVS
				MR_IM(); BVS();
				ADD_CYCLE(2);
				break;

			case	0x4C: // JMP $????
				JMP();
				ADD_CYCLE(3);
				break;
			case	0x6C: // JMP ($????)
				JMP_ID();
				ADD_CYCLE(5);
				break;

			case	0x20: // JSR
				JSR();
				ADD_CYCLE(6);
				break;

			case	0x40: // RTI
				RTI();
				ADD_CYCLE(6);
				break;
			case	0x60: // RTS
				RTS();
				ADD_CYCLE(6);
				break;

	// 僼儔僌惂屼宯
			case	0x18: // CLC
				CLC();
				ADD_CYCLE(2);
				break;
			case	0xD8: // CLD
				CLD();
				ADD_CYCLE(2);
				break;
			case	0x58: // CLI
				CLI();
				ADD_CYCLE(2);
				break;
			case	0xB8: // CLV
				CLV();
				ADD_CYCLE(2);
				break;

			case	0x38: // SEC
				SEC();
				ADD_CYCLE(2);
				break;
			case	0xF8: // SED
				SED();
				ADD_CYCLE(2);
				break;
			case	0x78: // SEI
				SEI();
				ADD_CYCLE(2);
				break;

	// 僗僞僢僋宯
			case	0x48: // PHA
				PUSH( R.A );
				ADD_CYCLE(3);
				break;
			case	0x08: // PHP
				PUSH( R.P | B_FLAG );
				ADD_CYCLE(3);
				break;
			case	0x68: // PLA (N-----Z-)
				R.A = POP();
				SET_ZN_FLAG(R.A);
				ADD_CYCLE(4);
				break;
			case	0x28: // PLP
				R.P = POP() | R_FLAG;
				ADD_CYCLE(4);
				break;

	// 偦偺懠
			case	0x00: // BRK
				BRK();
				ADD_CYCLE(7);
				break;

			case	0xEA: // NOP
				ADD_CYCLE(2);
				break;

	// 枹岞奐柦椷孮
			case	0x0B: // ANC #$??
			case	0x2B: // ANC #$??
				MR_IM(); ANC();
				ADD_CYCLE(2);
				break;

			case	0x8B: // ANE #$??
				MR_IM(); ANE();
				ADD_CYCLE(2);
				break;

			case	0x6B: // ARR #$??
				MR_IM(); ARR();
				ADD_CYCLE(2);
				break;

			case	0x4B: // ASR #$??
				MR_IM(); ASR();
				ADD_CYCLE(2);
				break;

			case	0xC7: // DCP $??
				MR_ZP(); DCP(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0xD7: // DCP $??,X
				MR_ZX(); DCP(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0xCF: // DCP $????
				MR_AB(); DCP(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0xDF: // DCP $????,X
				MR_AX(); DCP(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0xDB: // DCP $????,Y
				MR_AY(); DCP(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0xC3: // DCP ($??,X)
				MR_IX(); DCP(); MW_EA();
				ADD_CYCLE(8);
				break;
			case	0xD3: // DCP ($??),Y
				MR_IY(); DCP(); MW_EA();
				ADD_CYCLE(8);
				break;

			case	0xE7: // ISB $??
				MR_ZP(); ISB(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0xF7: // ISB $??,X
				MR_ZX(); ISB(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0xEF: // ISB $????
				MR_AB(); ISB(); MW_EA();
				ADD_CYCLE(5);
				break;
			case	0xFF: // ISB $????,X
				MR_AX(); ISB(); MW_EA();
				ADD_CYCLE(5);
				break;
			case	0xFB: // ISB $????,Y
				MR_AY(); ISB(); MW_EA();
				ADD_CYCLE(5);
				break;
			case	0xE3: // ISB ($??,X)
				MR_IX(); ISB(); MW_EA();
				ADD_CYCLE(5);
				break;
			case	0xF3: // ISB ($??),Y
				MR_IY(); ISB(); MW_EA();
				ADD_CYCLE(5);
				break;

			case	0xBB: // LAS $????,Y
				MR_AY(); LAS(); CHECK_EA();
				ADD_CYCLE(4);
				break;


			case	0xA7: // LAX $??
				MR_ZP(); LAX();
				ADD_CYCLE(3);
				break;
			case	0xB7: // LAX $??,Y
				MR_ZY(); LAX();
				ADD_CYCLE(4);
				break;
			case	0xAF: // LAX $????
				MR_AB(); LAX();
				ADD_CYCLE(4);
				break;
			case	0xBF: // LAX $????,Y
				MR_AY(); LAX(); CHECK_EA();
				ADD_CYCLE(4);
				break;
			case	0xA3: // LAX ($??,X)
				MR_IX(); LAX();
				ADD_CYCLE(6);
				break;
			case	0xB3: // LAX ($??),Y
				MR_IY(); LAX(); CHECK_EA();
				ADD_CYCLE(5);
				break;

			case	0xAB: // LXA #$??
				MR_IM(); LXA();
				ADD_CYCLE(2);
				break;

			case	0x27: // RLA $??
				MR_ZP(); RLA(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0x37: // RLA $??,X
				MR_ZX(); RLA(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0x2F: // RLA $????
				MR_AB(); RLA(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x3F: // RLA $????,X
				MR_AX(); RLA(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0x3B: // RLA $????,Y
				MR_AY(); RLA(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0x23: // RLA ($??,X)
				MR_IX(); RLA(); MW_EA();
				ADD_CYCLE(8);
				break;
			case	0x33: // RLA ($??),Y
				MR_IY(); RLA(); MW_EA();
				ADD_CYCLE(8);
				break;

			case	0x67: // RRA $??
				MR_ZP(); RRA(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0x77: // RRA $??,X
				MR_ZX(); RRA(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0x6F: // RRA $????
				MR_AB(); RRA(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x7F: // RRA $????,X
				MR_AX(); RRA(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0x7B: // RRA $????,Y
				MR_AY(); RRA(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0x63: // RRA ($??,X)
				MR_IX(); RRA(); MW_EA();
				ADD_CYCLE(8);
				break;
			case	0x73: // RRA ($??),Y
				MR_IY(); RRA(); MW_EA();
				ADD_CYCLE(8);
				break;

			case	0x87: // SAX $??
				MR_ZP(); SAX(); MW_ZP();
				ADD_CYCLE(3);
				break;
			case	0x97: // SAX $??,Y
				MR_ZY(); SAX(); MW_ZP();
				ADD_CYCLE(4);
				break;
			case	0x8F: // SAX $????
				MR_AB(); SAX(); MW_EA();
				ADD_CYCLE(4);
				break;
			case	0x83: // SAX ($??,X)
				MR_IX(); SAX(); MW_EA();
				ADD_CYCLE(6);
				break;

			case	0xCB: // SBX #$??
				MR_IM(); SBX();
				ADD_CYCLE(2);
				break;

			case	0x9F: // SHA $????,Y
				MR_AY(); SHA(); MW_EA();
				ADD_CYCLE(5);
				break;
			case	0x93: // SHA ($??),Y
				MR_IY(); SHA(); MW_EA();
				ADD_CYCLE(6);
				break;

			case	0x9B: // SHS $????,Y
				MR_AY(); SHS(); MW_EA();
				ADD_CYCLE(5);
				break;

			case	0x9E: // SHX $????,Y
				MR_AY(); SHX(); MW_EA();
				ADD_CYCLE(5);
				break;

			case	0x9C: // SHY $????,X
				MR_AX(); SHY(); MW_EA();
				ADD_CYCLE(5);
				break;

			case	0x07: // SLO $??
				MR_ZP(); SLO(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0x17: // SLO $??,X
				MR_ZX(); SLO(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0x0F: // SLO $????
				MR_AB(); SLO(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x1F: // SLO $????,X
				MR_AX(); SLO(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0x1B: // SLO $????,Y
				MR_AY(); SLO(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0x03: // SLO ($??,X)
				MR_IX(); SLO(); MW_EA();
				ADD_CYCLE(8);
				break;
			case	0x13: // SLO ($??),Y
				MR_IY(); SLO(); MW_EA();
				ADD_CYCLE(8);
				break;

			case	0x47: // SRE $??
				MR_ZP(); SRE(); MW_ZP();
				ADD_CYCLE(5);
				break;
			case	0x57: // SRE $??,X
				MR_ZX(); SRE(); MW_ZP();
				ADD_CYCLE(6);
				break;
			case	0x4F: // SRE $????
				MR_AB(); SRE(); MW_EA();
				ADD_CYCLE(6);
				break;
			case	0x5F: // SRE $????,X
				MR_AX(); SRE(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0x5B: // SRE $????,Y
				MR_AY(); SRE(); MW_EA();
				ADD_CYCLE(7);
				break;
			case	0x43: // SRE ($??,X)
				MR_IX(); SRE(); MW_EA();
				ADD_CYCLE(8);
				break;
			case	0x53: // SRE ($??),Y
				MR_IY(); SRE(); MW_EA();
				ADD_CYCLE(8);
				break;

			case	0xEB: // SBC #$?? (Unofficial)
				MR_IM(); SBC();
				ADD_CYCLE(2);
				break;

			case	0x1A: // NOP (Unofficial)
			case	0x3A: // NOP (Unofficial)
			case	0x5A: // NOP (Unofficial)
			case	0x7A: // NOP (Unofficial)
			case	0xDA: // NOP (Unofficial)
			case	0xFA: // NOP (Unofficial)
				ADD_CYCLE(2);
				break;
			case	0x80: // DOP (CYCLES 2)
			case	0x82: // DOP (CYCLES 2)
			case	0x89: // DOP (CYCLES 2)
			case	0xC2: // DOP (CYCLES 2)
			case	0xE2: // DOP (CYCLES 2)
				R.PC++;
				ADD_CYCLE(2);
				break;
			case	0x04: // DOP (CYCLES 3)
			case	0x44: // DOP (CYCLES 3)
			case	0x64: // DOP (CYCLES 3)
				R.PC++;
				ADD_CYCLE(3);
				break;
			case	0x14: // DOP (CYCLES 4)
			case	0x34: // DOP (CYCLES 4)
			case	0x54: // DOP (CYCLES 4)
			case	0x74: // DOP (CYCLES 4)
			case	0xD4: // DOP (CYCLES 4)
			case	0xF4: // DOP (CYCLES 4)
				R.PC++;
				ADD_CYCLE(4);
				break;
			case	0x0C: // TOP
			case	0x1C: // TOP
			case	0x3C: // TOP
			case	0x5C: // TOP
			case	0x7C: // TOP
			case	0xDC: // TOP
			case	0xFC: // TOP
				R.PC+=2;
				ADD_CYCLE(4);
				break;

			case	0x02:  /* JAM */
			case	0x12:  /* JAM */
			case	0x22:  /* JAM */
			case	0x32:  /* JAM */
			case	0x42:  /* JAM */
			case	0x52:  /* JAM */
			case	0x62:  /* JAM */
			case	0x72:  /* JAM */
			case	0x92:  /* JAM */
			case	0xB2:  /* JAM */
			case	0xD2:  /* JAM */
			case	0xF2:  /* JAM */
			default:
				if( !Config.emulator.bIllegalOp ) 
				{	
					throw	CApp::GetErrorString( IDS_ERROR_ILLEGALOPCODE );
					goto	_execute_exit;
				} 
				else 
				{
					R.PC--;
					ADD_CYCLE(4);
				}
				break;
//			default:
//				__assume(0);
		}

		end_is: __asm nop;

		if( nmi_request ) {
			_NMI();
		} else
		if( irq_request ) {
			_IRQ();
		}

		request_cycles -= exec_cycles;
		TOTAL_cycles += exec_cycles;

		// 僋儘僢僋摨婜張棟
		mapper->Clock( exec_cycles );
#if	DPCM_SYNCCLOCK
		apu->SyncDPCM( exec_cycles );
#endif
		if( bClockProcess ) {
			nes->Clock( exec_cycles );
		}
//		nes->Clock( exec_cycles );
	}
_execute_exit:

#if	!DPCM_SYNCCLOCK
	apu->SyncDPCM( TOTAL_cycles - OLD_cycles );
#endif

	return	TOTAL_cycles - OLD_cycles;
}
Esempio n. 20
0
#include "../drivers/audio/sequencer.h"
#include "../fc/conf.h"

char gui_value_label[20];
uint8_t gui_value_type;
uint8_t gui_value_index;
char gui_value_format[16];

float gui_value_min;
float gui_value_max;
float gui_value_step;
volatile float gui_value_tmp;

float_cb * gui_value_cb;

MK_SEQ(audio_feedback, ARR({600}), ARR({750}));

void gui_value_conf_P(const char * label, uint8_t type, const char * format, float start, float min, float max, float step, float_cb * cb)
{
	strcpy_P(gui_value_label, label);
	gui_value_type = type;
	strcpy_P(gui_value_format, format);

	gui_value_min = min;
	gui_value_max = max;
	gui_value_step = step;

	gui_value_tmp = start;
	gui_value_cb = cb;
	gui_value_index = 0;
}
Esempio n. 21
0
int main(int argn,char **argv){
  MT::initCmdLine(argn,argv);
  cout <<USAGE <<endl;

  //-- setup the simulator, swift, opengl, and the SocAbstraction
  ors::Graph ors;
  ors.init(MT::getParameter<MT::String>("orsfile",MT::String("test.ors")));
  //cout <<"read configuration:\n" <<ors <<endl;

  SwiftModule swift;
  swift.init(ors,.5);
  
  OpenGL gl;
  gl.add(glStandardScene);
  gl.add(ors::glDrawGraph,&ors);
  gl.camera.setPosition(5,-10,10);
  gl.camera.focus(0,0,1);
  gl.watch("loaded configuration - press ENTER");

  uint T=200;
  soc::SocSystem_Ors soc;
  if(MT::getParameter<bool>("dynamic",false)){
    soc.initPseudoDynamic(&ors,&swift,&gl,3.,T);
  }else{
    soc.initKinematic(&ors,&swift,&gl,T);
  }
  soc.os=&std::cout;

  //-- setup the control variables (problem definition)
  TaskVariable *pos = new TaskVariable("position",ors, posTVT,"endeff","<t(0 0 .2)>",0,0,ARR());
  pos->setGainsAsAttractor(20,.2);
  pos->y_target = arr(ors.getName("target")->X.p.v,3);
  
  TaskVariable *col = new TaskVariable("collision",ors, collTVT,0,0,0,0,ARR(.05));
  col->setGains(.5,.0);
  col->y_prec=1e-0;
  col->y_target = ARR(0.);

  soc.setTaskVariables(TUPLE(pos,col));
  
  //-- feedback control (kinematic or dynamic) to reach the targets
  arr q,dq,qv;
  soc.getq0(q);
  soc.getqv0(qv);
  for(uint t=0;t<T;t++){
    //soc::bayesianIKControl(soc,dq,0);
    //q += dq;
    if(!soc.dynamic){
      soc::bayesianIKControl2(soc,q,q,0);
      soc.setq(q);
    }else{
      soc::bayesianDynamicControl(soc,qv,qv,0);
      soc.setqv(qv);
    }
    //soc.reportOnState(cout); //->would generate detailed ouput on the state of all variables...
    gl.update(STRING("bayesian Inverse Kinematics: iteration "<<t));
    //gl.watch();
  }
  gl.watch("<press ENTER>");
  
  //-- planning (AICO) to generate an optimal (kinematic) trajectory
  soc.getq0(q);
  soc.setq(q);
  pos->setInterpolatedTargetsEndPrecisions(T,1e-3,1e3,0.,1e3);
  col->setInterpolatedTargetsConstPrecisions(T,1e-2,0.);
  
  q.clear();
  AICO_solver(soc,q,1e-2,.7,.01,0,0);
  ofstream os("z.traj"); q.writeRaw(os); os.close();
  for(;;) soc.displayTrajectory(q,NULL,1,"AICO (planned trajectory)");
  
  return 0;
}
Esempio n. 22
0
bool GameProject::Save()
{
    picojson::object j_project;
    j_project["GameTitle"] = picojson::value(m_gametitle.toStdString());
    j_project["TileSize"] = picojson::value((double)m_tilesize);
#define ARR(var,list,key) picojson::array var;for (int i = 0; i < list.size(); i++)var.push_back(picojson::value(list[i].toStdString().c_str()));j_project[key] = picojson::value(var);
    ARR(j_backdrop,m_backdrop,"Backdrop");
    ARR(j_battle,m_battle,"Battle");
    ARR(j_battle2,m_battle2,"Battle2");
    ARR(j_battlecharset,m_battlecharset,"BattleCharSet");
    ARR(j_battleweapon,m_battleweapon,"BattleWeapon");
    ARR(j_charset,m_charset,"CharSet");
    ARR(j_chipset,m_chipset,"ChipSet");
    ARR(j_faceset,m_faceset,"FaceSet");
    ARR(j_frame,m_frame,"Frame");
    ARR(j_gameover,m_gameover,"GameOver");
    ARR(j_monster,m_monster,"Monster");
    ARR(j_movie,m_movie,"Movie");
    ARR(j_music,m_music,"Music");
    ARR(j_background,m_background,"Background");
    ARR(j_picture,m_picture,"Picture");
    ARR(j_sound,m_sound,"Sound");
    ARR(j_system,m_system,"System");
    ARR(j_system2,m_system2,"System2");
    ARR(j_title,m_title,"Title");
#undef ARR
    QString f_text = QString::fromStdString(picojson::value(j_project).serialize());
    if (f_text.isEmpty())
        return false;
    QFile m_file(m_path+"project.erp");
    if (!m_file.open(QFile::WriteOnly | QFile::Text))
        return false;
    QTextStream out(&m_file);
    out << f_text;
    m_file.flush();
    m_file.close();
    return true;
}
Esempio n. 23
0
    ////////////////////////////////////////////////////
    //  Run!
    void Cpu::run(timestamp_t runto)
    {
        if(cpuJammed && (curCyc() < runto))
        {
            setMainTimestamp(runto);
        }

        while( curCyc() < runto )
        {
            /////////////////////////////////////
            // Are we to take an interrupt
            if(wantInterrupt)
            {
                performInterrupt(false);
                continue;
            }

            if( *tracer )
                tracer->traceCpuLine( cpu );

            /////////////////////////////////////
            // No interrupt, do an instruction
            u8 opcode = rd( cpu.PC++ );
            switch(opcode)
            {
                /* Branches */
            case 0x10:  adBranch( !cpu.getN() );        break;  /* BPL  */
            case 0x30:  adBranch(  cpu.getN() );        break;  /* BMI  */
            case 0x50:  adBranch( !cpu.getV() );        break;  /* BVC  */
            case 0x70:  adBranch(  cpu.getV() );        break;  /* BVS  */
            case 0x90:  adBranch( !cpu.getC() );        break;  /* BCC  */
            case 0xB0:  adBranch(  cpu.getC() );        break;  /* BCS  */
            case 0xD0:  adBranch( !cpu.getZ() );        break;  /* BNE  */
            case 0xF0:  adBranch(  cpu.getZ() );        break;  /* BEQ  */

                /* Flag flip-flop   */
            case 0x18:  adImplied(); cpu.setC(0);       break;  /* CLC  */
            case 0x38:  adImplied(); cpu.setC(1);       break;  /* SEC  */
            case 0x58:  adImplied(); cpu.setI(0);       break;  /* CLI  */
            case 0x78:  adImplied(); cpu.setI(1);       break;  /* SEI  */
            case 0xB8:  adImplied(); cpu.setV(0);       break;  /* CLV  */
            case 0xD8:  adImplied(); cpu.setD(0);       break;  /* CLD  */
            case 0xF8:  adImplied(); cpu.setD(1);       break;  /* SED  */

                /* Stack push/pull  */
            case 0x08:  adPush( cpu.getStatus(true) );  break;  /* PHP  */
            case 0x28:  cpu.setStatus( adPull() );      break;  /* PLP  */
            case 0x48:  adPush( cpu.A );                break;  /* PHA  */
            case 0x68:  cpu.NZ( cpu.A = adPull() );     break;  /* PLA  */

                /* Reg Xfer         */
            case 0xAA:  adImplied(); cpu.NZ( cpu.X = cpu.A );   break;  /* TAX  */
            case 0xA8:  adImplied(); cpu.NZ( cpu.Y = cpu.A );   break;  /* TAY  */
            case 0xBA:  adImplied(); cpu.NZ( cpu.X = cpu.SP );  break;  /* TSX  */
            case 0x8A:  adImplied(); cpu.NZ( cpu.A = cpu.X );   break;  /* TXA  */
            case 0x9A:  adImplied();        cpu.SP = cpu.X;     break;  /* TXS  */
            case 0x98:  adImplied(); cpu.NZ( cpu.A = cpu.Y );   break;  /* TYA  */

                /* Misc */
            case 0x00:  performInterrupt(true);         break;  /* BRK          */
            case 0x4C:  full_JMP();                     break;  /* JMP $xxxx    */
            case 0x6C:  full_JMP_Indirect();            break;  /* JMP ($xxxx)  */
            case 0x20:  full_JSR();                     break;  /* JSR $xxxx    */
            case 0xEA:  adImplied();                    break;  /* NOP          */
            case 0x40:  full_RTI();                     break;  /* RTI          */
            case 0x60:  full_RTS();                     break;  /* RTS          */

                /* ADC  */
            case 0x69:  ADC( adRdIm() );                break;
            case 0x65:  ADC( adRdZp() );                break;
            case 0x75:  ADC( adRdZx() );                break;
            case 0x6D:  ADC( adRdAb() );                break;
            case 0x7D:  ADC( adRdAx() );                break;
            case 0x79:  ADC( adRdAy() );                break;
            case 0x61:  ADC( adRdIx() );                break;
            case 0x71:  ADC( adRdIy() );                break;
                
                /* AND  */
            case 0x29:  AND( adRdIm() );                break;
            case 0x25:  AND( adRdZp() );                break;
            case 0x35:  AND( adRdZx() );                break;
            case 0x2D:  AND( adRdAb() );                break;
            case 0x3D:  AND( adRdAx() );                break;
            case 0x39:  AND( adRdAy() );                break;
            case 0x21:  AND( adRdIx() );                break;
            case 0x31:  AND( adRdIy() );                break;

                /* ASL  */
            case 0x0A:  adImplied();    ASL(cpu.A);     break;
            case 0x06:  adRWZp( &Cpu::ASL );            break;
            case 0x16:  adRWZx( &Cpu::ASL );            break;
            case 0x0E:  adRWAb( &Cpu::ASL );            break;
            case 0x1E:  adRWAx( &Cpu::ASL );            break;

                /* BIT  */
            case 0x24:  BIT( adRdZp() );                break;
            case 0x2C:  BIT( adRdAb() );                break;
                
                /* CMP  */
            case 0xC9:  CMP( adRdIm() );                break;
            case 0xC5:  CMP( adRdZp() );                break;
            case 0xD5:  CMP( adRdZx() );                break;
            case 0xCD:  CMP( adRdAb() );                break;
            case 0xDD:  CMP( adRdAx() );                break;
            case 0xD9:  CMP( adRdAy() );                break;
            case 0xC1:  CMP( adRdIx() );                break;
            case 0xD1:  CMP( adRdIy() );                break;
                
                /* CPX  */
            case 0xE0:  CPX( adRdIm() );                break;
            case 0xE4:  CPX( adRdZp() );                break;
            case 0xEC:  CPX( adRdAb() );                break;
                
                /* CPY  */
            case 0xC0:  CPY( adRdIm() );                break;
            case 0xC4:  CPY( adRdZp() );                break;
            case 0xCC:  CPY( adRdAb() );                break;
                
                /* DEC  */
            case 0xCA:  adImplied();    DEC(cpu.X);     break;  /* DEX  */
            case 0x88:  adImplied();    DEC(cpu.Y);     break;  /* DEY  */
            case 0xC6:  adRWZp( &Cpu::DEC );            break;
            case 0xD6:  adRWZx( &Cpu::DEC );            break;
            case 0xCE:  adRWAb( &Cpu::DEC );            break;
            case 0xDE:  adRWAx( &Cpu::DEC );            break;
                
                /* EOR  */
            case 0x49:  EOR( adRdIm() );                break;
            case 0x45:  EOR( adRdZp() );                break;
            case 0x55:  EOR( adRdZx() );                break;
            case 0x4D:  EOR( adRdAb() );                break;
            case 0x5D:  EOR( adRdAx() );                break;
            case 0x59:  EOR( adRdAy() );                break;
            case 0x41:  EOR( adRdIx() );                break;
            case 0x51:  EOR( adRdIy() );                break;
                
                /* INC  */
            case 0xE8:  adImplied();    INC(cpu.X);     break;  /* INX  */
            case 0xC8:  adImplied();    INC(cpu.Y);     break;  /* INY  */
            case 0xE6:  adRWZp( &Cpu::INC );            break;
            case 0xF6:  adRWZx( &Cpu::INC );            break;
            case 0xEE:  adRWAb( &Cpu::INC );            break;
            case 0xFE:  adRWAx( &Cpu::INC );            break;
                
                /* LDA  */
            case 0xA9:  cpu.NZ( cpu.A = adRdIm() );     break;
            case 0xA5:  cpu.NZ( cpu.A = adRdZp() );     break;
            case 0xB5:  cpu.NZ( cpu.A = adRdZx() );     break;
            case 0xAD:  cpu.NZ( cpu.A = adRdAb() );     break;
            case 0xBD:  cpu.NZ( cpu.A = adRdAx() );     break;
            case 0xB9:  cpu.NZ( cpu.A = adRdAy() );     break;
            case 0xA1:  cpu.NZ( cpu.A = adRdIx() );     break;
            case 0xB1:  cpu.NZ( cpu.A = adRdIy() );     break;
                
                /* LDX  */
            case 0xA2:  cpu.NZ( cpu.X = adRdIm() );     break;
            case 0xA6:  cpu.NZ( cpu.X = adRdZp() );     break;
            case 0xB6:  cpu.NZ( cpu.X = adRdZy() );     break;
            case 0xAE:  cpu.NZ( cpu.X = adRdAb() );     break;
            case 0xBE:  cpu.NZ( cpu.X = adRdAy() );     break;
                
                /* LDY  */
            case 0xA0:  cpu.NZ( cpu.Y = adRdIm() );     break;
            case 0xA4:  cpu.NZ( cpu.Y = adRdZp() );     break;
            case 0xB4:  cpu.NZ( cpu.Y = adRdZx() );     break;
            case 0xAC:  cpu.NZ( cpu.Y = adRdAb() );     break;
            case 0xBC:  cpu.NZ( cpu.Y = adRdAx() );     break;
                
                /* LSR  */
            case 0x4A:  adImplied();    LSR(cpu.A);     break;
            case 0x46:  adRWZp( &Cpu::LSR );            break;
            case 0x56:  adRWZx( &Cpu::LSR );            break;
            case 0x4E:  adRWAb( &Cpu::LSR );            break;
            case 0x5E:  adRWAx( &Cpu::LSR );            break;
                
                /* ORA  */
            case 0x09:  ORA( adRdIm() );                break;
            case 0x05:  ORA( adRdZp() );                break;
            case 0x15:  ORA( adRdZx() );                break;
            case 0x0D:  ORA( adRdAb() );                break;
            case 0x1D:  ORA( adRdAx() );                break;
            case 0x19:  ORA( adRdAy() );                break;
            case 0x01:  ORA( adRdIx() );                break;
            case 0x11:  ORA( adRdIy() );                break;

                /* ROL  */
            case 0x2A:  adImplied();    ROL(cpu.A);     break;
            case 0x26:  adRWZp( &Cpu::ROL );            break;
            case 0x36:  adRWZx( &Cpu::ROL );            break;
            case 0x2E:  adRWAb( &Cpu::ROL );            break;
            case 0x3E:  adRWAx( &Cpu::ROL );            break;

                /* ROR  */
            case 0x6A:  adImplied();    ROR(cpu.A);     break;
            case 0x66:  adRWZp( &Cpu::ROR );            break;
            case 0x76:  adRWZx( &Cpu::ROR );            break;
            case 0x6E:  adRWAb( &Cpu::ROR );            break;
            case 0x7E:  adRWAx( &Cpu::ROR );            break;
                
                /* SBC  */
            case 0xE9:  SBC( adRdIm() );                break;
            case 0xE5:  SBC( adRdZp() );                break;
            case 0xF5:  SBC( adRdZx() );                break;
            case 0xED:  SBC( adRdAb() );                break;
            case 0xFD:  SBC( adRdAx() );                break;
            case 0xF9:  SBC( adRdAy() );                break;
            case 0xE1:  SBC( adRdIx() );                break;
            case 0xF1:  SBC( adRdIy() );                break;
                
                /* STA  */
            case 0x85:  adWrZp( cpu.A );                break;
            case 0x95:  adWrZx( cpu.A );                break;
            case 0x8D:  adWrAb( cpu.A );                break;
            case 0x9D:  adWrAx( cpu.A );                break;
            case 0x99:  adWrAy( cpu.A );                break;
            case 0x81:  adWrIx( cpu.A );                break;
            case 0x91:  adWrIy( cpu.A );                break;
                
                /* STX  */
            case 0x86:  adWrZp( cpu.X );                break;
            case 0x96:  adWrZy( cpu.X );                break;
            case 0x8E:  adWrAb( cpu.X );                break;
                
                /* STY  */
            case 0x84:  adWrZp( cpu.Y );                break;
            case 0x94:  adWrZx( cpu.Y );                break;
            case 0x8C:  adWrAb( cpu.Y );                break;
                
                /////////////////////////////////////
                // Unofficial ops
                
                /* One offs */
 case 0x0B: case 0x2B:  ANC( adRdIm() );                break;  /* ANC  */
            case 0x4B:  ALR( adRdIm() );                break;  /* ALR  */
            case 0x6B:  ARR( adRdIm() );                break;  /* ARR  */
            case 0xCB:  AXS( adRdIm() );                break;  /* AXS  */
            case 0xBB:  LAS( adRdAy() );                break;  /* LAS  */
            case 0xEB:  SBC( adRdIm() );                break;  /* alternative SBC  */
            case 0x9E:  adWrAy_xxx( cpu.X );            break;  /* SHX  */
            case 0x9C:  adWrAx_xxx( cpu.Y );            break;  /* SHY  */
            case 0x8B:  XAA( adRdIm() );                break;  /* XAA  */

            case 0x9B:  cpu.SP = cpu.A & cpu.X; adWrAy_xxx( cpu.SP );     break;  /* TAS  */
                
                /* AHX  */
            case 0x9F:  adWrAy_xxx( cpu.A & cpu.X );    break;
            case 0x93:  adWrIy_xxx( cpu.A & cpu.X );    break;

                /* DCP  */
            case 0xC7:  adRWZp( &Cpu::DCP );            break;
            case 0xD7:  adRWZx( &Cpu::DCP );            break;
            case 0xCF:  adRWAb( &Cpu::DCP );            break;
            case 0xDF:  adRWAx( &Cpu::DCP );            break;
            case 0xDB:  adRWAy( &Cpu::DCP );            break;
            case 0xC3:  adRWIx( &Cpu::DCP );            break;
            case 0xD3:  adRWIy( &Cpu::DCP );            break;
                
                /* ISC  */
            case 0xE7:  adRWZp( &Cpu::ISC );            break;
            case 0xF7:  adRWZx( &Cpu::ISC );            break;
            case 0xEF:  adRWAb( &Cpu::ISC );            break;
            case 0xFF:  adRWAx( &Cpu::ISC );            break;
            case 0xFB:  adRWAy( &Cpu::ISC );            break;
            case 0xE3:  adRWIx( &Cpu::ISC );            break;
            case 0xF3:  adRWIy( &Cpu::ISC );            break;

                /* LAX  */
            case 0xAB:  LAX( adRdIm() );                break;
            case 0xA7:  LAX( adRdZp() );                break;
            case 0xB7:  LAX( adRdZy() );                break;
            case 0xAF:  LAX( adRdAb() );                break;
            case 0xBF:  LAX( adRdAy() );                break;
            case 0xA3:  LAX( adRdIx() );                break;
            case 0xB3:  LAX( adRdIy() );                break;

                /* NOP  */
            case 0x1A: case 0x3A: case 0x5A: case 0x7A: case 0xDA: case 0xFA:   adImplied();break;
            case 0x04: case 0x44: case 0x64:                                    adRdZp();   break;
            case 0x14: case 0x34: case 0x54: case 0x74: case 0xD4: case 0xF4:   adRdZx();   break;
            case 0x80: case 0x82: case 0x89: case 0xC2: case 0xE2:              adRdIm();   break;
            case 0x0C:                                                          adRdAb();   break;
            case 0x1C: case 0x3C: case 0x5C: case 0x7C: case 0xDC: case 0xFC:   adRdAx();   break;
                
                /* RLA  */
            case 0x27:  adRWZp( &Cpu::RLA );            break;
            case 0x37:  adRWZx( &Cpu::RLA );            break;
            case 0x2F:  adRWAb( &Cpu::RLA );            break;
            case 0x3F:  adRWAx( &Cpu::RLA );            break;
            case 0x3B:  adRWAy( &Cpu::RLA );            break;
            case 0x23:  adRWIx( &Cpu::RLA );            break;
            case 0x33:  adRWIy( &Cpu::RLA );            break;
                
                /* RRA  */
            case 0x67:  adRWZp( &Cpu::RRA );            break;
            case 0x77:  adRWZx( &Cpu::RRA );            break;
            case 0x6F:  adRWAb( &Cpu::RRA );            break;
            case 0x7F:  adRWAx( &Cpu::RRA );            break;
            case 0x7B:  adRWAy( &Cpu::RRA );            break;
            case 0x63:  adRWIx( &Cpu::RRA );            break;
            case 0x73:  adRWIy( &Cpu::RRA );            break;

                /* SAX  */
            case 0x87:  adWrZp( cpu.A & cpu.X );        break;
            case 0x97:  adWrZy( cpu.A & cpu.X );        break;
            case 0x8F:  adWrAb( cpu.A & cpu.X );        break;
            case 0x83:  adWrIx( cpu.A & cpu.X );        break;
                
                /* SLO  */
            case 0x07:  adRWZp( &Cpu::SLO );            break;
            case 0x17:  adRWZx( &Cpu::SLO );            break;
            case 0x0F:  adRWAb( &Cpu::SLO );            break;
            case 0x1F:  adRWAx( &Cpu::SLO );            break;
            case 0x1B:  adRWAy( &Cpu::SLO );            break;
            case 0x03:  adRWIx( &Cpu::SLO );            break;
            case 0x13:  adRWIy( &Cpu::SLO );            break;
                
                /* SRE  */
            case 0x47:  adRWZp( &Cpu::SRE );            break;
            case 0x57:  adRWZx( &Cpu::SRE );            break;
            case 0x4F:  adRWAb( &Cpu::SRE );            break;
            case 0x5F:  adRWAx( &Cpu::SRE );            break;
            case 0x5B:  adRWAy( &Cpu::SRE );            break;
            case 0x43:  adRWIx( &Cpu::SRE );            break;
            case 0x53:  adRWIy( &Cpu::SRE );            break;

                /* STP  */
            case 0x02: case 0x12: case 0x22: case 0x32: case 0x42: case 0x52:
            case 0x62: case 0x72: case 0x92: case 0xB2: case 0xD2: case 0xF2:
                cpuJammed = true;
                setMainTimestamp(runto);
                break;
            }
        }
    }
Esempio n. 24
0
int main(int argn,char **argv){
  MT::initCmdLine(argn,argv);
  cout <<USAGE <<endl;

  //-- setup the simulator, swift, opengl, and the SocAbstraction
  ors::Graph ors;
  ors.init(MT::getParameter<MT::String>("orsfile",MT::String("test.ors")));
  //cout <<"read configuration:\n" <<ors <<endl;

  SwiftModule swift;
  swift.init(ors,.5);
  
  OpenGL gl;
  gl.add(glStandardScene);
  gl.add(ors::glDrawGraph,&ors);
  gl.camera.setPosition(5,-10,10);
  gl.camera.focus(0,0,1);
  gl.watch("loaded configuration - press ENTER");

  uint T=200;
  soc::SocSystem_Ors soc;
  if(MT::getParameter<bool>("dynamic",false)){
    soc.initPseudoDynamic(&ors,&swift,&gl,3.,T);
  }else{
    soc.initKinematic(&ors,&swift,&gl,T);
  }
  soc.os=&std::cout;

  
  //-- setup the control variables (problem definition)
  TaskVariable *pos = new TaskVariable("position",ors, posTVT,"hand","<t(0.055 0 0)>",0,0,ARR());
  pos->setGainsAsAttractor(20,.2);
  pos->y_target = arr(ors.getName("target")->X.p.v,3);
  //pos->active=0;

#if 0
  TaskVariable *align = new TaskVariable("zalign",ors, zalignTVT,"hand","<t(0.055 0 0) d(0 0 1 0)>",0,0,ARR());
  align->setGainsAsAttractor(20,.2);
  align->y_target = 1.;
  ors::Vector target_z;
  ors.getBodyByName("target")->X.r.getZ(target_z);
  align->params = arr(target_z.v,3);
#else
  TaskVariable *align = new TaskVariable("zalign",ors, zalignTVT,"hand","<t(0.055 0 0) d(-90 1 0 0)>",0,0,ARR());
  align->setGainsAsAttractor(20,.2);
  align->y_target = 0.;
  ors::Vector target_z;
  ors.getBodyByName("target")->X.r.getZ(target_z);
  align->params = arr(target_z.v,3);
#endif
                                               
  TaskVariable *col = new TaskVariable("collision",ors, collTVT,0,0,0,0,ARR(.05));
  col->setGains(.5,.0);
  col->y_prec=1e-0;
  col->y_target = ARR(0.);

  arr jointLimits;
  jointLimits.read(MT::String("[-3. 3.; -.2 1.; -.5 .5; -2. 2.; -1. 1.; -2. 2. ]").stream());
  TaskVariable *lim = new TaskVariable("limits",ors, qLimitsTVT,0,0,0,0,jointLimits);
  lim->setGains(.5,.0);
  lim->y_prec=1e-0;
  lim->y_target = ARR(0.);

  soc.setTaskVariables(TUPLE(pos,col,lim,align));
  
  //-- directly setting the state
  arr qq = ARR(.1,.1,.1,.1,.1,.1);
  soc.setq(qq);
  cout <<"value of position TV = " <<pos->y <<endl;
  cout <<"position of shape.. = " <<ors.getShapeByName("gripperL")->X.p <<endl;
  gl.watch("configuration after explicit set");
  
  //-- feedback control (kinematic or dynamic) to reach the targets
  arr q,dq,qv;
  soc.getq0(q);
  soc.getqv0(qv);
  for(uint t=0;t<T;t++){
    //soc::bayesianIKControl(soc,dq,0);
    //q += dq;
    if(!soc.dynamic){
      soc::bayesianIKControl2(soc,q,q,0);
      soc.setq(q);
    }else{
      soc::bayesianDynamicControl(soc,qv,qv,0);
      soc.setqv(qv);
    }
    //soc.reportOnState(cout); //->would generate detailed ouput on the state of all variables...
    gl.update(STRING("bayesian Inverse Kinematics: iteration "<<t));
    //gl.watch();
  }
  gl.watch("<press ENTER>");
  
  //-- planning (AICO) to generate an optimal (kinematic) trajectory
  soc.getq0(q);
  soc.setq(q);
  double effPrec = MT::getParameter<double>("effPrec");
  double limPrec = MT::getParameter<double>("limPrec");
  pos  ->setInterpolatedTargetsEndPrecisions(T,1e-3,effPrec,0.,1e3);
  align->setInterpolatedTargetsEndPrecisions(T,1e-3,effPrec,0.,1e3);
  col  ->setInterpolatedTargetsConstPrecisions(T,1e-1,0.);
  lim  ->setInterpolatedTargetsConstPrecisions(T,limPrec,0.);
  
  q.clear();
  AICO_solver(soc,q,1e-2,.7,.01,0,0);
  soc.analyzeTrajectory(q,true);

  ofstream os("z.traj"); q.writeRaw(os); os.close();
  for(;;) soc.displayTrajectory(q,NULL,1,"AICO (planned trajectory)");
  
  return 0;
}
Esempio n. 25
0
#include "gui_list.h"
#include "gui.h"

gui_list_gen * gui_list_gen_f = NULL;
gui_list_act * gui_list_act_f = NULL;

uint8_t gui_list_size = 0;
uint8_t gui_list_back = 0;
int16_t gui_list_y_offset = 0;
uint8_t gui_list_index[NUMBER_OF_GUI_TASKS];
uint32_t gui_list_middle_hold;

#include "../../drivers/audio/sequencer.h"
MK_SEQ(snd_menu_exit, ARR({800}), ARR({200}));

void gui_list_draw()
{
	char tmp_text[64];
	char tmp_sub_text[32];

	uint8_t flags;

	disp.LoadFont(F_TEXT_M);
	uint8_t t_h = disp.GetTextHeight();

	int16_t y = gui_list_y_offset;
	int8_t height;
	int16_t total_height = 0;
	uint8_t sub_height;

	//emulate middle click
Esempio n. 26
0
void
bmp_write( char *filename, uint8_t *pic_buf[3], int wid, int hgt )
{
    BITMAPFILEHEADER file_hdr;
    BITMAPINFOHEADER info_hdr;

    FILE    *fp;

    int     i;
    int     idx_v;
    int     idx_h;
    int     file_stride;

    int     idx_dummy;
    int     n_dummy;

    file_hdr.bfType = 'B' | ('M' << 8);
    file_hdr.bfSize = FH_SIZE + IH_SIZE + ((wid*3+3)/4)*4*hgt;  // Each row of pixels must be an integer number of DWORDs, which are 4 bytes
    file_hdr.bfReserved1 = 0;
    file_hdr.bfReserved2 = 0;
    file_hdr.bfOffBits = FH_SIZE + IH_SIZE;                     // The offset, in bytes, from the beginning of the BITMAPFILEHEADER structure to the bitmap bits.

    info_hdr.biSize             = IH_SIZE;  // The number of bytes required by the structure
    info_hdr.biWidth            = wid;      // The width of the bitmap, in pixels
    info_hdr.biHeight           = hgt;
    info_hdr.biPlanes           = 1;
    info_hdr.biBitCount         = 24;
    info_hdr.biCompression      = BI_RGB;
    info_hdr.biSizeImage        = 0;        // Not used for uncompressed images
    info_hdr.biXPelsPerMeter    = 2834;     // 72 dpi
    info_hdr.biYPelsPerMeter    = 2834;     // 72 dpi
    info_hdr.biClrUsed          = 0;
    info_hdr.biClrImportant     = 0;

    fp = fopen( filename, "wb" );
    assert( fp != NULL );

    fwrite( &file_hdr.bfType         , sizeof(WORD ), 1, fp );
    fwrite( &file_hdr.bfSize         , sizeof(DWORD), 1, fp );
    fwrite( &file_hdr.bfReserved1    , sizeof(WORD ), 1, fp );
    fwrite( &file_hdr.bfReserved2    , sizeof(WORD ), 1, fp );
    fwrite( &file_hdr.bfOffBits      , sizeof(DWORD), 1, fp );

    fwrite( &info_hdr.biSize         , sizeof(DWORD), 1, fp ); 
    fwrite( &info_hdr.biWidth        , sizeof(LONG ), 1, fp ); 
    fwrite( &info_hdr.biHeight       , sizeof(LONG ), 1, fp ); 
    fwrite( &info_hdr.biPlanes       , sizeof(WORD ), 1, fp ); 
    fwrite( &info_hdr.biBitCount     , sizeof(WORD ), 1, fp ); 
    fwrite( &info_hdr.biCompression  , sizeof(DWORD), 1, fp ); 
    fwrite( &info_hdr.biSizeImage    , sizeof(DWORD), 1, fp ); 
    fwrite( &info_hdr.biXPelsPerMeter, sizeof(LONG ), 1, fp ); 
    fwrite( &info_hdr.biYPelsPerMeter, sizeof(LONG ), 1, fp ); 
    fwrite( &info_hdr.biClrUsed      , sizeof(DWORD), 1, fp ); 
    fwrite( &info_hdr.biClrImportant , sizeof(DWORD), 1, fp ); 

    file_stride = (wid * 3 + 3) / 4 * 4;     // wid * 3, rounded up to a multiple of 4 bytes
    n_dummy = file_stride - wid * 3;           // number of dummy bytes needed to pad at the end of each row

    for (idx_v = hgt-1; idx_v >= 0; --idx_v)
    {
        for (idx_h = 0; idx_h < wid; ++idx_h)
        {
            for (i = 0; i < 3; ++i)
            {
                fwrite( &ARR( pic_buf[i], idx_h, idx_v, wid), 1, 1, fp );    // Write one pixel component
            }
        }
        for (idx_dummy = 0; idx_dummy < n_dummy; ++idx_dummy)
        {
            fwrite(&ARR(pic_buf[i], 0, 0, wid), 1, 1, fp);
        }
    }
    fclose( fp );
}