//Every 500 us
void __attribute__((__interrupt__, no_auto_psv)) _T3Interrupt(void)
{
_T3IF = 0;
//Latch encoder timestamp on Rising edge.
#ifdef USE_TIMESTAMP_DC
SetTimestampLatch;
ClrTimestampLatch;
#endif
//set_pwm(0,100);
//P1DC1=100;
//P1DC2=100;
//P1DC3=100;
//set_bldc_dir(0);
#if defined USE_ENCODER_VERTX
step_vertx();
#endif
#ifdef USE_CURRENT
step_current();
#endif
#ifdef USE_CONTROL
step_control();
#endif
step_state();
set_bldc_mode(ec_cmd.command[0].bldc_mode);
asm("clrwdt");
//irq_cnt++;
}
/**
* Call to set the desired IMU state. The class should then take whatever action is needed
* to make reality match. This will be asynchronously done, so the caller will probably need
* to be informed of the change after it has been completed, or can go no further.
*
* @param uint8_t The new state desired by the firmware (the Legend).
* @return non-zero on error.
*/
int8_t LSM9DSx_Common::setDesiredState(State nu) {
if (present() && (nu < State::STAGE_1)) {
// If we already know the sensor is there, why go back further than this?
local_log.concatf("%s Trying to move to a state lower than allowed.\n", imu_type());
Kernel::log(&local_log);
return -1;
}
if (desired_state != nu) {
if (!desired_state_attained()) {
// TODO
// The IMU is not at equilibrium. It may be ok to change the desired stage as long as we don't have
// bus operations pending.
if (getVerbosity() > 2) {
//local_log.concatf("%s tried to move to state %s while the IMU is off-balance (%s --> %s). Rejecting request.\n", imu_type(), getStateString(nu), getStateString(), getStateString(desired_state));
local_log.concatf("%s tried to move to state %s while the IMU is off-balance (%s --> %s). We will allow this for now.\n", imu_type(), getStateString(nu), getStateString(), getStateString(desired_state));
Kernel::log(&local_log);
}
//return -2;
}
desired_state = nu;
step_state();
}
return IMU_ERROR_NO_ERROR;
}
int main(int argc, char *argv[])
{
FILE *code_file;
int max_steps = 10000;
// edit by leo
init_sharemem();
// edit end
state_ptr s = new_state(MEM_SIZE);
mem_t saver = copy_reg(s->r);
mem_t savem;
int step = 0;
stat_t e = STAT_AOK;
if (argc < 2 || argc > 3)
usage(argv[0]);
code_file = fopen(argv[1], "r");
if (!code_file) {
fprintf(stderr, "Can't open code file '%s'\n", argv[1]);
exit(1);
}
if (!load_mem(s->m, code_file, 1)) {
printf("Exiting\n");
return 1;
}
savem = copy_mem(s->m);
// edit by leo
// printf("error happen after here!\n");
// edit end
if (argc > 2)
max_steps = atoi(argv[2]);
for (step = 0; step < max_steps && e == STAT_AOK; step++)
e = step_state(s, stdout);
printf("Stopped in %d steps at PC = 0x%x. Status '%s', CC %s\n",
step, s->pc, stat_name(e), cc_name(s->cc));
printf("Changes to registers:\n");
diff_reg(saver, s->r, stdout);
printf("\nChanges to memory:\n");
diff_mem(savem, s->m, stdout);
free_state(s);
free_reg(saver);
free_mem(savem);
return 0;
}
//ToDo Every X us
void __attribute__((__interrupt__, no_auto_psv)) _T3Interrupt(void)
{
//Latch encoder timestamp on Rising edge.
#ifdef USE_TIMESTAMP_DC
SetTimestampLatch;
ClrTimestampLatch;
#endif
#if defined USE_ENCODER_VERTX
step_vertx();
#endif
ToggleHeartbeatLED();
step_adc_spi();
// set_pwm(0,1000);
/*P1DC1=500;
P1DC2=500;
P1DC3=500;
set_bldc_dir(1);*/
#ifdef USE_CURRENT
//step_current();
#endif
#ifdef USE_CONTROL
//step_control();
#endif
step_state();
set_bldc_mode(ec_cmd.command[0].bldc_mode);
if (ec_cmd.command[0].config & ACTUATOR_EC_CONFIG_HAS_BRAKE)
{
TRISBbits.TRISB1=0; //RB1 OUTPUT PIN22 //Brake enable
//SetBrakeOff;
//step_brake(1);
} else {
TRISBbits.TRISB1=1; //RB1 OUTPUT PIN22 //Brake disable
}
step_brake(ec_cmd.command[0].config&M3ACT_CONFIG_BRAKE_OFF);
//TRISBbits.TRISB1=0; //RB1 OUTPUT PIN22
//SetBrakeOn;
asm("clrwdt");
_T3IF = 0;
// irq_cnt++;
}
int main(int argc, char *argv[])
{
FILE *code_file;
int max_steps = 10000;
state_ptr s = new_state(MEM_SIZE);
mem_t saver = copy_reg(s->r);
mem_t savem;
int step = 0;
exc_t e = EXC_NONE;
if (argc < 2 || argc > 3)
usage(argv[0]);
code_file = fopen(argv[1], "r");
if (!code_file) {
fprintf(stderr, "Can't open code file '%s'\n", argv[1]);
exit(1);
}
if (!load_mem(s->m, code_file, 1)) {
printf("Exiting\n");
return 1;
}
savem = copy_mem(s->m);
if (argc > 2)
max_steps = atoi(argv[2]);
for (step = 0; step < max_steps && e == EXC_NONE; step++)
e = step_state(s, stdout);
printf("Stopped in %d steps at PC = 0x%x. Exception '%s', CC %s\n",
step, s->pc, exc_name(e), cc_name(s->cc));
printf("Changes to registers:\n");
diff_reg(saver, s->r, stdout);
printf("\nChanges to memory:\n");
diff_mem(savem, s->m, stdout);
free_state(s);
free_reg(saver);
free_mem(savem);
return 0;
}
/*
* run_tty_sim - Run the simulator in TTY mode
*/
static void run_tty_sim()
{
int icount = 0;
exc_t status = EXC_NONE;
cc_t result_cc = 0;
int byte_cnt = 0;
mem_t mem0, reg0;
state_ptr isa_state = NULL;
/* In TTY mode, the default object file comes from stdin */
if (!object_file) {
object_file = stdin;
}
/* Initializations */
if (verbosity >= 2)
sim_set_dumpfile(stdout);
sim_init();
/* Emit simulator name */
printf("%s\n", simname);
byte_cnt = load_mem(mem, object_file, 1);
if (byte_cnt == 0) {
fprintf(stderr, "No lines of code found\n");
exit(1);
} else if (verbosity >= 2) {
printf("%d bytes of code read\n", byte_cnt);
}
fclose(object_file);
if (do_check) {
isa_state = new_state(0);
free_mem(isa_state->r);
free_mem(isa_state->m);
isa_state->m = copy_mem(mem);
isa_state->r = copy_mem(reg);
isa_state->cc = cc;
}
mem0 = copy_mem(mem);
reg0 = copy_mem(reg);
icount = sim_run(instr_limit, &status, &result_cc);
if (verbosity > 0) {
printf("%d instructions executed\n", icount);
printf("Exception status = %s\n", exc_name(status));
printf("Condition Codes: %s\n", cc_name(result_cc));
printf("Changed Register State:\n");
diff_reg(reg0, reg, stdout);
printf("Changed Memory State:\n");
diff_mem(mem0, mem, stdout);
}
if (do_check) {
exc_t e = EXC_NONE;
int step;
bool_t match = TRUE;
for (step = 0; step < instr_limit && e == EXC_NONE; step++) {
e = step_state(isa_state, stdout);
}
if (diff_reg(isa_state->r, reg, NULL)) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Register != Pipeline Register File\n");
diff_reg(isa_state->r, reg, stdout);
}
}
if (diff_mem(isa_state->m, mem, NULL)) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Memory != Pipeline Memory\n");
diff_mem(isa_state->m, mem, stdout);
}
}
if (isa_state->cc != result_cc) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Cond. Codes (%s) != Pipeline Cond. Codes (%s)\n",
cc_name(isa_state->cc), cc_name(result_cc));
}
}
if (match) {
printf("ISA Check Succeeds\n");
} else {
printf("ISA Check Fails\n");
}
}
}
/*
* run_tty_sim - Run the simulator in TTY mode
*/
static void run_tty_sim()
{
int icount = 0;
byte_t run_status = STAT_AOK;
cc_t result_cc = 0;
int byte_cnt = 0;
mem_t mem0, reg0;
state_ptr isa_state = NULL;
/* In TTY mode, the default object file comes from stdin */
if (!object_file) {
object_file = stdin;
}
if (verbosity >= 2)
sim_set_dumpfile(stdout);
sim_init();
/* Emit simulator name */
if (verbosity >= 2)
printf("%s\n", simname);
byte_cnt = load_mem(mem, object_file, 1, START_PLACE);
if (byte_cnt == 0) {
fprintf(stderr, "No lines of code found\n");
exit(1);
} else if (verbosity >= 2) {
printf("%d bytes of code read\n", byte_cnt);
}
fclose(object_file);
if (do_check) {
isa_state = new_state(0);
free_reg(isa_state->r);
free_mem(isa_state->m);
isa_state->m = copy_mem(mem);
isa_state->r = copy_reg(reg);
isa_state->cc = cc;
}
if(verbosity > 0){
mem0 = copy_mem(mem);
reg0 = copy_reg(reg);
}
icount = sim_run_pipe(instr_limit, 5*instr_limit, &run_status, &result_cc);
if (verbosity > 0) {
printf("%d instructions executed\n", icount);
printf("Status = %s\n", stat_name(run_status));
printf("Condition Codes: %s\n", cc_name(result_cc));
printf("Changed Register State:\n");
diff_reg(reg0, reg, stdout);
printf("Changed Memory State:\n");
diff_mem(mem0, mem, stdout);
}
if (do_check) {
exit(0);
byte_t e = STAT_AOK;
int step;
bool_t match = TRUE;
for (step = 0; step < instr_limit && e == STAT_AOK; step++) {
e = step_state(isa_state, stdout);
}
if (diff_reg(isa_state->r, reg, NULL)) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Register != Pipeline Register File\n");
diff_reg(isa_state->r, reg, stdout);
}
}
if (diff_mem(isa_state->m, mem, NULL)) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Memory != Pipeline Memory\n");
diff_mem(isa_state->m, mem, stdout);
}
}
if (isa_state->cc != result_cc) {
match = FALSE;
if (verbosity > 0) {
printf("ISA Cond. Codes (%s) != Pipeline Cond. Codes (%s)\n",
cc_name(isa_state->cc), cc_name(result_cc));
}
}
if (match) {
printf("ISA Check Succeeds\n");
} else {
printf("ISA Check Fails\n");
}
}
/* Emit CPI statistics */
{
double cpi = instructions > 0 ? (double) cycles/instructions : 1.0;
printf("%d CPI: %d cycles/%d instructions = %.2f\n", PSIM_ID,
cycles, instructions, cpi);
}
}
