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