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); }
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); }
/* * 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; }
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; }
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); } }
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; }
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); } } } }
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)); }
// ============================================================================ 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; }
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; }
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; } }
#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();
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)
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; }
// // 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); }
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; }
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; }
// // 柦椷幚峴 // 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; }
#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; }
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; }
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; }
//////////////////////////////////////////////////// // 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; } } }
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; }
#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
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 ); }