int main(void) { INS *ins = init_ins(GSENSOR_SPI_BASE); // auto calibration: auto_calibrate_ins(ins, 1000); // manual calibration: // calibrate_ins(ins, 0.0, 0.46, -0.4); double acceleration[DIM]; double speed[DIM]; double distance[DIM]; while(1) { usleep(1000 * 10); // update the INS with new values from the sensor update_ins(ins); // load the current state in our arrays get_latest_state(ins, acceleration, speed, distance); // printf("acceleration: X: %6.2f,\tY: %6.2f,\tZ: %6.2f\n", acceleration[0], acceleration[1], acceleration[2]); printf("speed: X: %6.2f,\tY: %6.2f,\tZ: %6.2f\n", speed[0], speed[1], speed[2]); // printf("distance: X: %6.2f,\tY: %6.2f,\tZ: %6.2f\n", distance[0], distance[1], distance[2]); } return 0; }
int step(int flag, Emulator *emul) { int r = 0; word in = get_word(emul->reg[15], emul->map); Instruction *out = init_ins(); r = disasm(in, out, emul); // retourne l'offset (2 : instruction 16 bits, 4 : 32 bits) if (r == 3) return r; else if (r == 2 || r == 4) { printf("\n"); if(out->run != NULL) { out->run(*out, emul); printf("\033[00;32m"); } else { printf("\033[00;31m"); } if( breakpoint_exist( emul->breaklist, emul->reg[15] ) ) display(*out, DECODED, emul); printf("\033[0m"); } emul->reg[15] += r; return r; }
int main (void) { int pwm_period = 100000; alt_u32 motor_base_addresses[] = { 0x80000000 | A_2_CHANNEL_PWM_0_BASE, 0x80000000 | A_2_CHANNEL_PWM_1_BASE, 0x80000000 | A_2_CHANNEL_PWM_2_BASE, 0x80000000 | A_2_CHANNEL_PWM_3_BASE, 0x80000000 | A_2_CHANNEL_PWM_4_BASE, 0x80000000 | A_2_CHANNEL_PWM_5_BASE, 0x80000000 | A_2_CHANNEL_PWM_6_BASE, 0x80000000 | A_2_CHANNEL_PWM_7_BASE }; // initialize data structures for the engines so that we can control them init_legocar(&car, motor_base_addresses, pwm_period); // initialize Inertial Navigation System // --> the structure is used to exchange data between threads, so it has to be volatile init_ins(&ins, GSENSOR_SPI_BASE); printf("Starting system!\n"); OSInit(); // create the task for the wheel stabilization procedure OSTaskCreateExt(stabilizer_task, NULL, (void *) &stabilizer_task_stk[TASK_STACKSIZE-1], STABILIZER_PRIORITY, STABILIZER_PRIORITY, stabilizer_task_stk, TASK_STACKSIZE, NULL, 0); // create the task for steering the car OSTaskCreateExt(control_task, NULL, (void *) &control_task_stk[TASK_STACKSIZE-1], CONTROL_PRIORITY, CONTROL_PRIORITY, control_task_stk, TASK_STACKSIZE, NULL, 0); /* Third task messes up the system :-( * When this thread is activated additionally to the other two threads, * the main-function will be started over and over again. * If you use this thread alone, there are no problems. * Sorry, but I cannot tell you why! * * Raphael * // create the task for parsing the output of the acceleration sensor OSTaskCreateExt(acc_sensor_task, NULL, (void *) &acc_sensor_task_stk[TASK_STACKSIZE-1], ACC_SENSOR_PRIORITY, ACC_SENSOR_PRIORITY, acc_sensor_task_stk, TASK_STACKSIZE, NULL, 0);*/ // start the operating system => all registered threads will be started OSStart(); return 0; }