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