Beispiel #1
0
/* for elevator auto mode */
static void elevator_cb(void *dummy)
{
	static uint8_t prev = 0;
	uint8_t val;

	val = !sensor_get(0);

	switch (elevator_state) {
	case OFF:
		break;
	case WAIT_SENSOR:
		if (val && !prev) {
			delay = elevator_delay_up;
			elevator_state = SENSOR_OK;
		}
		break;
	case SENSOR_OK:
		if (delay-- == 0) {
			cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
			elevator_state = WAIT_DOWN;
			delay = elevator_delay_down;
		}
		break;
	case WAIT_DOWN:
		if (delay-- == 0) {
			cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
			elevator_state = WAIT_SENSOR;
		}
		break;
	default:
		break;
	}
	prev = val;
}
Beispiel #2
0
void robot_cs_set_xy_consigns( robot_cs_t* rcs,
											  	  		int32_t x,
												  	  	int32_t y)
{
  uint8_t flags;

  IRQ_LOCK(flags);
  cs_set_consign(&csm_x,x);
  cs_set_consign(&csm_y,y);
  IRQ_UNLOCK(flags);
}
Beispiel #3
0
void motor_cs_update(void* dummy, int32_t m1, int32_t m2, int32_t m3)
{
  uint8_t flags;

	// set consigns for motors CS
	cs_set_consign(&csm_motor1, m1);
	cs_set_consign(&csm_motor2, m2);
	cs_set_consign(&csm_motor3, m3);

	// update motors CS
	cs_manage(&csm_motor1);
	cs_manage(&csm_motor2);
	cs_manage(&csm_motor3);

 	return;
}
Beispiel #4
0
void robot_cs_set_a_consign( robot_cs_t* rcs,
		    											int32_t angle)
{                                
  uint8_t flags;

  IRQ_LOCK(flags);
  cs_set_consign(&csm_angle,angle);
  IRQ_UNLOCK(flags);
}
Beispiel #5
0
void rsh_update(struct robot_system_holonomic *rs) {
    if(rs->pos == NULL)
        return;

    /** @todo add real physical parameters. */
    double theta_r = holonomic_position_get_a_rad_double(rs->pos);
    int i;

    for(i = 0; i < 3; i++) {
        /** omega = omega_r + omega_t = speed of the wheel i */
        
        /** < the part for translation */
        const double omega_t = - rs->speed / rs->pos->geometry.wheel_radius[i] * 
                     cos(theta_r - rs->direction + rs->pos->geometry.beta[i] - M_PI_2);
        /** < the part for rotation */
        const double omega_r = - rs->rotation_speed * rs->pos->geometry.wheel_distance[i] / rs->pos->geometry.wheel_radius[i];
        
        /** @bug @todo le facteur 10 : on ne le met pas et on a des vitesse 10 x trop haute */
        cs_set_consign(rs->motors[i],((omega_t + omega_r) / M_2_PI * (double)rs->pos->geometry.encoder_resolution / (double)rs->pos->update_frequency)/10);
        //DEBUG(E_ROBOT_SYSTEM, "wheel %d : omega_t=%.1f omega_r=%.1f", i, omega_t, omega_r);
    }
}
Beispiel #6
0
void beacon_calibre_pos(void)
{
	sensorboard.flags &= ~DO_CS;

	/* init beacon pos */
	pwm_ng_set(BEACON_PWM, 100);

	/* find rising edge of the mirror*/
	wait_ms(100);
	while (sensor_get(BEACON_POS_SENSOR));
	wait_ms(100);
	while (!sensor_get(BEACON_POS_SENSOR));

	pwm_ng_set(BEACON_PWM, 0);


	beacon_reset_pos();
	pid_reset(&sensorboard.beacon.pid);
	encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);

	cs_set_consign(&sensorboard.beacon.cs, 0);

	sensorboard.flags |= DO_CS;
}
Beispiel #7
0
void roller_reverse(void)
{
	cs_set_consign(&ballboard.roller.cs, ROLLER_REVERSE);
}
Beispiel #8
0
void roller_on(void)
{
	cs_set_consign(&ballboard.roller.cs, ROLLER_ON);
}
Beispiel #9
0
void roller_off(void)
{
	cs_set_consign(&ballboard.roller.cs, ROLLER_OFF);
}
Beispiel #10
0
void elevator_up(void)
{
	elevator_state = 0;
	cs_set_consign(&mainboard.elevator.cs, elevator_pos_up);
}
Beispiel #11
0
void fessor_auto(void)
{
	fessor_state = WAIT_SENSOR;
	cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
}
Beispiel #12
0
void fessor_down(void)
{
	fessor_state = 0;
	cs_set_consign(&mainboard.fessor.cs, fessor_pos_down);
}
Beispiel #13
0
int main(void)
{


  
  // init motors and PWMs
  brushless_init();


  // enable power bridges
  sbi(DDRG, 1);
  sbi(PORTG, 1);
  
  
  // init uart
  uart_init();
  fdevopen((void *)uart0_send,NULL,0);

  
  
  //printf_P(PSTR("\nbonjour\n"));
  
  
  
  /** replaces the scheduler. This is directly derived from the interrupt which runs the brushless motors, for convenience */
  brushless_0_register_periodic_event(asserv_rapide_manage); // 10 ms
  
  
  
  /** speed PID stuff */
  
  // PID
  pid_init(&speed_pid);
  
  pid_set_gains(&speed_pid,     180, 70, 40); // sur alim
  
  pid_set_maximums(&speed_pid,  0, 80, PWM_MAX*4/5);
  pid_set_out_shift(&speed_pid, 0);
  
  // derivation (This could alternatively be skipped if we use the brushless peed info directly)
  biquad_init(&speed_derivation);
  biquad_set_numerator_coeffs(&speed_derivation, 1,-1,0); // this is a discrete derivation : O(n) = I(n) - I(n-1)
  // no need to initialize denominator coeffs to 0, init has done it
  
  // control system speed
  cs_init(&speed);
  
  cs_set_correct_filter(&speed, pid_do_filter, &speed_pid);
  cs_set_process_in(&speed, brushless_set_torque, (void *)0 );
  cs_set_process_out(&speed,brushless_get_pos , (void *)0 );
  cs_set_feedback_filter(&speed, biquad_do_filter, &speed_derivation);
  cs_set_consign(&speed, 0);
  
  
  
  /** ramp generator parameters */
  
  quadramp_derivate_init(&position_quadr_derv);
  
  quadramp_derivate_set_gain_anticipation(&position_quadr_derv, 256 *3);// some anticipation : 3.0 (this is a fixed point value *1/256)

  quadramp_derivate_set_goal_window(&position_quadr_derv, 5);           // algorithm shut down window

  quadramp_derivate_set_2nd_order_vars(&position_quadr_derv,   1 ,  1); // max acceleration : 1
  quadramp_derivate_set_1st_order_vars(&position_quadr_derv,  12,  12); // max speed is 12

  quadramp_derivate_set_divisor(&position_quadr_derv, 2);               // divisor, for precision


  // control system position 
  cs_init(&position);
  cs_set_correct_filter(&position, quadramp_derivate_do_filter, &position_quadr_derv); 
  cs_set_process_in(&position, (void *)cs_set_consign, &speed );
  cs_set_process_out(&position,brushless_get_pos , (void *)0 );
  cs_set_consign(&position, 0);


  /** begin */
  
  brushless_set_speed((void *)0 , BRUSHLESS_MAX_SPEED); // init speed

  sei();




  // some simple trajectories (enable one )

  while(1)
  {
  wait_ms(3500);
  cs_set_consign(&position, 400);
  wait_ms(500);
  cs_set_consign(&position, 0);
  }


  /*
  while(1)
  {
  wait_ms(2500);
  cs_set_consign(&position, 2000);
  wait_ms(2500);
  cs_set_consign(&position, 0);
  }



  // test of speed pid only, deactivate the position.
  while(1)
  {
  wait_ms(300);
  cs_set_consign(&speed, 10);
  wait_ms(300);
  cs_set_consign(&speed, -10);
  } */




  while(1);


  return 0;
}
Beispiel #14
0
void fork_mid2(void)
{
	cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID2);
}
Beispiel #15
0
void fork_deploy(void)
{
	cs_set_consign(&ballboard.forkrot.cs, FORKROT_DEPLOYED);
}
Beispiel #16
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);
}
Beispiel #17
0
void elevator_down(void)
{
	elevator_state = 0;
	cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
}
Beispiel #18
0
void elevator_auto(void)
{
	elevator_state = WAIT_SENSOR;
	cs_set_consign(&mainboard.elevator.cs, elevator_pos_down);
}
Beispiel #19
0
void fork_pack(void)
{
	cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED);
}
Beispiel #20
0
void beacon_start(void)
{
	beacon_reset_pos();
	sensorboard.beacon.on = 1;
	cs_set_consign(&sensorboard.beacon.cs, 600);
}
Beispiel #21
0
void fork_eject(void)
{
	cs_set_consign(&ballboard.forkrot.cs, FORKROT_EJECT);
}
Beispiel #22
0
void fessor_up(void)
{
	fessor_state = 0;
	cs_set_consign(&mainboard.fessor.cs, fessor_pos_up);
}