Example #1
0
void strat_autopos(int16_t x, int16_t y, int16_t a, int16_t epaisseurRobot)
{

    robot.is_aligning = 1;

    // Pour se recaler, on met le robot en regulation angulaire, on reduit la vitesse et l'acceleration
    // On diminue la sensibilite on augmente la constante de temps de detection du bloquage

    bd_set_thresholds(&robot.distance_bd,  5000, 2);
    trajectory_set_speed(&robot.traj, 100, 100);
    robot.mode = BOARD_MODE_DISTANCE_ONLY;

    // On recule jusqu'a� qu'on ait touche un mur
    trajectory_d_rel(&robot.traj, (double) -2000);
    wait_traj_end(END_BLOCKING);

    trajectory_hardstop(&robot.traj);
    bd_reset(&robot.distance_bd);
    bd_reset(&robot.angle_bd);
    robot.mode = BOARD_MODE_ANGLE_DISTANCE;

    position_set(&robot.pos, epaisseurRobot, 0, COLOR_A(0.));
    /* On se mets a la bonne position en x. */
    trajectory_d_rel(&robot.traj, 80);
    wait_traj_end(END_TRAJ);

    /* On se tourne face a la paroi en Y. */
    trajectory_only_a_abs(&robot.traj, COLOR_A(90));
    wait_traj_end(END_TRAJ);

    /* On recule jusqu'a avoir touche le bord. */

    trajectory_d_rel(&robot.traj, (double) -2000);
    wait_traj_end(END_BLOCKING);

    bd_reset(&robot.distance_bd);
    bd_reset(&robot.angle_bd);

    /* On reregle la position. */
    robot.pos.pos_d.y = COLOR_Y(epaisseurRobot);
    robot.pos.pos_s16.y = COLOR_Y(epaisseurRobot);


    trajectory_d_rel(&robot.traj, 80);
    wait_traj_end(END_TRAJ);

    trajectory_goto_forward_xy_abs(&robot.traj, x, COLOR_Y(y));
    wait_traj_end(END_TRAJ);

    /* Pour finir on s'occuppe de l'angle. */
    trajectory_a_abs(&robot.traj, COLOR_A((double)a));
    wait_traj_end(END_TRAJ);

    /* On remet le robot dans son etat initial. */
    robot.mode = BOARD_MODE_ANGLE_DISTANCE;
    robot.is_aligning = 0;

    /* On se met en place a la position demandee. */
    trajectory_set_speed(&robot.traj, speed_mm2imp(&robot.traj, 300), speed_rd2imp(&robot.traj, 2.5));
}
Example #2
0
void strat_set_speed(enum speed_e speed)
{
    int speed_d, speed_a;
    int acc_d, acc_a;

    switch (speed) {

        case SLOW:
            speed_d = 400;
            acc_d = 500;
            acc_a = 10;
            speed_a = 10;
            break;

        case FUCKING_SLOW:
            speed_d = 400;
            acc_d = 50;
            acc_a = 1;
            speed_a = 10;
            break;

        case FAST:
        default:
            speed_d = 800;
            acc_d = 1000;
            acc_a = 20;
            speed_a = 20;
            break;
    }


    trajectory_set_acc(&robot.traj,
            acc_mm2imp(&robot.traj, acc_d),
            acc_rd2imp(&robot.traj, acc_a));

    trajectory_set_speed(&robot.traj,
            speed_mm2imp(&robot.traj, speed_d),
            speed_rd2imp(&robot.traj, speed_a));
}
Example #3
0
void microb_cs_init(void)
{
	/* ROBOT_SYSTEM */
	rs_init(&mainboard.rs);
	rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
	rs_set_right_pwm(&mainboard.rs,  pwm_set_and_save, RIGHT_PWM);
	/* increase gain to decrease dist, increase left and it will turn more left */
	rs_set_left_ext_encoder(&mainboard.rs, encoders_microb_get_value, 
				LEFT_ENCODER, IMP_COEF * -1.0000);
	rs_set_right_ext_encoder(&mainboard.rs, encoders_microb_get_value, 
				 RIGHT_ENCODER, IMP_COEF * 1.0000);
	/* rs will use external encoders */
	rs_set_flags(&mainboard.rs, RS_USE_EXT);

	/* POSITION MANAGER */
	position_init(&mainboard.pos);
	position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
	position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
	//position_set_centrifugal_coef(&mainboard.pos, 0.000016);
	position_use_ext(&mainboard.pos);

	/* TRAJECTORY MANAGER */
	trajectory_init(&mainboard.traj);
	trajectory_set_cs(&mainboard.traj, &mainboard.distance.cs,
			  &mainboard.angle.cs);
	trajectory_set_robot_params(&mainboard.traj, &mainboard.rs, &mainboard.pos);
	trajectory_set_speed(&mainboard.traj, 1500, 1500); /* d, a */
	/* distance window, angle window, angle start */
	trajectory_set_windows(&mainboard.traj, 200., 5.0, 30.);

	/* ---- CS angle */
	/* PID */
	pid_init(&mainboard.angle.pid);
	pid_set_gains(&mainboard.angle.pid, 500, 10, 7000);
	pid_set_maximums(&mainboard.angle.pid, 0, 20000, 4095);
	pid_set_out_shift(&mainboard.angle.pid, 10);
	pid_set_derivate_filter(&mainboard.angle.pid, 4);

	/* QUADRAMP */
	quadramp_init(&mainboard.angle.qr);
	quadramp_set_1st_order_vars(&mainboard.angle.qr, 2000, 2000); /* set speed */
	quadramp_set_2nd_order_vars(&mainboard.angle.qr, 13, 13); /* set accel */

	/* CS */
	cs_init(&mainboard.angle.cs);
	cs_set_consign_filter(&mainboard.angle.cs, quadramp_do_filter, &mainboard.angle.qr);
	cs_set_correct_filter(&mainboard.angle.cs, pid_do_filter, &mainboard.angle.pid);
	cs_set_process_in(&mainboard.angle.cs, rs_set_angle, &mainboard.rs);
	cs_set_process_out(&mainboard.angle.cs, rs_get_angle, &mainboard.rs);
	cs_set_consign(&mainboard.angle.cs, 0);

	/* Blocking detection */
	bd_init(&mainboard.angle.bd);
	bd_set_speed_threshold(&mainboard.angle.bd, 80);
	bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 1000000, 50);

	/* ---- CS distance */
	/* PID */
	pid_init(&mainboard.distance.pid);
	pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
	pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
	pid_set_out_shift(&mainboard.distance.pid, 10);
	pid_set_derivate_filter(&mainboard.distance.pid, 6);

	/* QUADRAMP */
	quadramp_init(&mainboard.distance.qr);
	quadramp_set_1st_order_vars(&mainboard.distance.qr, 2000, 2000); /* set speed */
	quadramp_set_2nd_order_vars(&mainboard.distance.qr, 17, 17); /* set accel */

	/* CS */
	cs_init(&mainboard.distance.cs);
	cs_set_consign_filter(&mainboard.distance.cs, quadramp_do_filter, &mainboard.distance.qr);
	cs_set_correct_filter(&mainboard.distance.cs, pid_do_filter, &mainboard.distance.pid);
	cs_set_process_in(&mainboard.distance.cs, rs_set_distance, &mainboard.rs);
	cs_set_process_out(&mainboard.distance.cs, rs_get_distance, &mainboard.rs);
	cs_set_consign(&mainboard.distance.cs, 0);

	/* Blocking detection */
	bd_init(&mainboard.distance.bd);
	bd_set_speed_threshold(&mainboard.distance.bd, 60);
	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);

	/* ---- CS fessor */
	
	fessor_autopos();
	/* PID */
	pid_init(&mainboard.fessor.pid);
	pid_set_gains(&mainboard.fessor.pid, 300, 10, 150);
	pid_set_maximums(&mainboard.fessor.pid, 0, 10000, 4095);
	pid_set_out_shift(&mainboard.fessor.pid, 10);
	pid_set_derivate_filter(&mainboard.fessor.pid, 4);

	/* CS */
	cs_init(&mainboard.fessor.cs);
	cs_set_correct_filter(&mainboard.fessor.cs, pid_do_filter, &mainboard.fessor.pid);
	cs_set_process_in(&mainboard.fessor.cs, fessor_set, NULL);
	cs_set_process_out(&mainboard.fessor.cs, encoders_microb_get_value, FESSOR_ENC);
	fessor_up();



	/* ---- CS elevator */
	
	elevator_autopos();
	/* PID */
	pid_init(&mainboard.elevator.pid);
	pid_set_gains(&mainboard.elevator.pid, 300, 10, 150);
	pid_set_maximums(&mainboard.elevator.pid, 0, 10000, 4095);
	pid_set_out_shift(&mainboard.elevator.pid, 10);
	pid_set_derivate_filter(&mainboard.elevator.pid, 4);

	/* CS */
	cs_init(&mainboard.elevator.cs);
	cs_set_correct_filter(&mainboard.elevator.cs, pid_do_filter, &mainboard.elevator.pid);
	cs_set_process_in(&mainboard.elevator.cs, elevator_set, NULL);
	cs_set_process_out(&mainboard.elevator.cs, encoders_microb_get_value, ELEVATOR_ENC);
	elevator_down();

	/* ---- CS wheel */
	
	/* PID */
	pid_init(&mainboard.wheel.pid);
	pid_set_gains(&mainboard.wheel.pid, 100, 100, 0);
	pid_set_maximums(&mainboard.wheel.pid, 0, 30000, 4095);
	pid_set_out_shift(&mainboard.wheel.pid, 5);
	pid_set_derivate_filter(&mainboard.wheel.pid, 4);

	/* CS */
	cs_init(&mainboard.wheel.cs);
	cs_set_correct_filter(&mainboard.wheel.cs, pid_do_filter, &mainboard.wheel.pid);
	cs_set_process_in(&mainboard.wheel.cs, wheel_set, NULL);
	cs_set_process_out(&mainboard.wheel.cs, wheel_get_value, NULL);
	cs_set_consign(&mainboard.wheel.cs, 1000);

	/* set them on !! */
	mainboard.angle.on = 0;
	mainboard.distance.on = 0;
	mainboard.fessor.on = 1;
	mainboard.elevator.on = 0;
	mainboard.wheel.on = 1;
	mainboard.flags |= DO_CS;

	scheduler_add_periodical_event_priority(do_cs, NULL,
						5000L / SCHEDULER_UNIT,
						CS_PRIO);
}