static int __init zrcar_wheel_module_init(void)
{
wheel_major=register_chrdev(0, DEVICE_NAME, &wheel_fops);
if (wheel_major < 0){
printk("failed to register device.\n");
return -1;
}
wheel_class = class_create(THIS_MODULE, "wheel_class");
if (IS_ERR(wheel_class)){
printk("failed to create device class.\n");
unregister_chrdev(wheel_major, DEVICE_NAME);
return -1;
}
wheel_device = device_create(wheel_class, NULL, MKDEV(wheel_major, 0), NULL, "zrcar_wheel_dev");
if (IS_ERR(wheel_device)){
printk("failed to create device .\n");
unregister_chrdev(wheel_major, DEVICE_NAME);
return -1;
}
wheel_l_baseaddr = (unsigned long)ioremap(WHEEL_L_BASEADDR, 16);
wheel_r_baseaddr = (unsigned long)ioremap(WHEEL_R_BASEADDR, 16);
wheel_init();
printk("my wheel driver initial successfully!\n");
return 0;
}
int main(void)
{
sched_init(); /* initialize the scheduler */
led_init(); /* initialize led */
button_init(); /* initialize button */
adc_init(); /* initialize ADC (battery voltage measurement) */
serial_init(); /* initialize serial communication */
wheel_init(); /* initialize encoders, PWM output, PID etc. */
pid_interval = 50; /* default PID update interval 50ms */
pid_rate = 1000/pid_interval; /* [Hz] always remember after setting pid_interval */
pfbst_interval = 20; /* send $PFBST at 20 ms interval */
nmea_wd_timeout = 1; /* set PFBCT watchdog timeout to 100ms */
nmea_wd = NMEA_WD_TOUT+1; /* make sure we begin in watchdog timeout state */
voltage_min = VOLTAGE_MIN_DEFAULT;
battery_low_warning = false;
state_update();
sei(); /* enable interrupts */
nmea_init(); /* initialize nmea protocol handler */
for (;;) /* go into an endless loop */
{
/* motor_update(); */
if (t1ms != 0) /* if the interrupt has timed out after 10ms */
{
t1ms --;
sched_update(); /* run the scheduler */
}
else
{
nmea_rx_update();
}
}
return 0; /* just for the principle as we never get here */
}
int main(){
//initialisation des variable de base
int cont=1;
char str[100];
char chact;
// alocation de la possition de l'evalbot
evalbot = malloc(sizeof(t_possition));
evalbot->x =0;
evalbot->y = 0;
evalbot->angle =0;
evalbot->state =WAIT;
//init
motor_init();
mess_init();
bump_init();
wheel_init();
//on met l'evalbot imobile
motor_STOP();
while(cont){
//get un message
chact = mess_get();
//fait l'action
actionInt(chact);
// detecte si l'evalbot n' a pas de PB
if(evalbot->state != PB){
//regarde l'ordre
switch(chact){
// envoie une info
case 'i':
sprintf(str,"!POS:x:%4i y:%4i a:%4i",
evalbot->x,
evalbot->y,
evalbot->angle);
mess_setString(str,24);
break;
// met pause
case 'p':
motor_STOP();
break;
}
}
/*
tout les autres comportement sont geré
par les interuptions
*/
}
}
void odometry_base_init(
odometry_differential_base_t *robot,
const struct robot_base_pose_2d_s init_pose,
const float right_wheel_radius,
const float left_wheel_radius,
const int right_wheel_direction,
const int left_wheel_direction,
const float wheelbase,
const uint32_t time_now)
{
robot->pose.x = init_pose.x;
robot->pose.y = init_pose.y;
robot->pose.theta = init_pose.theta;
robot->velocity.x = 0.0f;
robot->velocity.y = 0.0f;
robot->velocity.omega = 0.0f;
int right_wheel_dir, left_wheel_dir;
if(right_wheel_direction >= 0.0f) {
right_wheel_dir = 1.0f;
} else {
right_wheel_dir = -1.0f;
}
if(left_wheel_direction >= 0.0f) {
left_wheel_dir = 1.0f;
} else {
left_wheel_dir = -1.0f;
}
wheel_init(&robot->right_wheel, right_wheel_radius * right_wheel_dir);
wheel_init(&robot->left_wheel, left_wheel_radius * left_wheel_dir);
robot->wheelbase = wheelbase;
robot->time_last_estim = time_now;
}
int wheel_test(){
wheel_init();
return 1;
}
