void CWheelHandler::move_until_collision()
{
printf("move_until_collision start\n");
time_t starttime = time(NULL);
time_t currenttime;
int elapsed = 0;
int distance = 0;
int percentage = 0;
int velocity = -40;
int actual_velocity_left = 0;
int actual_velocity_right = 0;
motors_set_speed(ROBOT_WHEEL_RIGHT_PORT, velocity);
motors_set_speed(ROBOT_WHEEL_LEFT_PORT, velocity);
do
{
sleep(1);
distance = sensors_get_us_distance_mm(ROBOT_ULTRASONIC_SENSOR_PORT);
percentage = sensors_get_ir_distance(ROBOT_INFRARED_SENSOR_PORT);
actual_velocity_right = motors_get_motor_speed(ROBOT_WHEEL_RIGHT_PORT);
actual_velocity_left = motors_get_motor_speed(ROBOT_WHEEL_LEFT_PORT);
currenttime = time(NULL);
elapsed = static_cast<int>(difftime(currenttime, starttime));
printf("move_until_collision dist=%d, perc=%d, velocity=%d, left=%d, right=%d, elapsed=%d\n", distance, percentage, velocity, actual_velocity_left, actual_velocity_right, elapsed);
} while ((elapsed < 30) and /*((distance<120) or (distance>180)) and*/ ((abs(actual_velocity_left)>0) or (abs(actual_velocity_right)>0)));
motors_stop(ROBOT_WHEEL_RIGHT_PORT);
motors_stop(ROBOT_WHEEL_LEFT_PORT);
sleep(1);
printf("move_until_collision done\n");
}
void ball_terminate()
{
delete wheelhandler;
delete ballhandler;
motors_stop(ROBOT_WHEEL_RIGHT_PORT);
motors_stop(ROBOT_WHEEL_LEFT_PORT);
motors_stop(ROBOT_BALL_THROW_PORT);
motors_stop(ROBOT_BALL_CATCH_PORT);
}
void PWM1_init () {
/* Initialise le module PWM PWM1 */
P1TCONbits.PTEN = 0; // Désactive PWM 1
P1TCONbits.PTOPS = 0b0000; // No postscale
P1TCONbits.PTCKPS = 0b00; // No prescale
P1TCONbits.PTMOD1 = 0; // Free running mode
P1TCONbits.PTMOD0 = 0; // (PTMODE = 0b00)
//P1TCONbits.PTSIDL = 1; // Arret en pause
/*
La fréquence est codée "en dur" car c'est la bonne fréquence
à utiliser : le pont en H utilisé ne supporte pas de fréquence
plus élevée, et un sifflement désagréable est produit pour
une fréquence plus basse.
*/
P1TPER = MAX_SPEED/2; // Le sifflement a disparu
//P1TPER = 2000 - 1; // 10kHz avec 80MHz clk
P1DC1 = 0; // Duty-cycle PWM1H1 0%
P1DC2 = 0; // Duty-cycle PWM1H2 0%
PWM1CON1bits.PMOD1 = 1; // Sorties PWM1H1 et PWM1L1 indépendantes
PWM1CON1bits.PMOD2 = 1; // Sorties PWM1H2 et PWM1L2 indépendantes
motors_stop();
P1TCONbits.PTEN = 1; // Active PWM 1
}
void CThrower::calibrate()
{
printf("Calibrate throw engine start\n");
motors_set_speed(ROBOT_BALL_THROW_PORT, 20);
SBYTE motor_speeds[4];
int sum_motor_speeds = 0;
for (int i=0; i<4; i++) motor_speeds[i] = 1;
int i = 0;
time_t starttime = time(NULL);
bool timeout = false;
do
{
sleep_ms(100);
timeout = difftime(time(NULL), starttime) > 4.0; // Open arms shouldn't take more than 4 seconds, otherwise mechanical problems
motor_speeds[(i++)%4] = motors_get_motor_speed(ROBOT_BALL_THROW_PORT);
printf("calibrate %d %d %d %d (timeout=%d)\n", motor_speeds[0], motor_speeds[1], motor_speeds[2], motor_speeds[3], timeout);
sum_motor_speeds = abs(motor_speeds[0]) + abs(motor_speeds[1]) + abs(motor_speeds[2]) + abs(motor_speeds[3]);
} while ((not timeout) and (sum_motor_speeds > 0));
motors_reset_angle(ROBOT_BALL_THROW_PORT);
motors_stop(ROBOT_BALL_THROW_PORT);
is_down = false;
is_half_down = false;
is_up = false;
printf("Calibrate throw engine done\n");
}
void fin(void) {
motors_stop();
while (1) {
allumer_del();
pause_ms(500);
eteindre_del();
pause_ms(500);
}
}
void asser_tourner(long consigne){
long delta = 0;
long alpha = 0;
PID_init(&pid_alpha,100,0,0); // nb ki divisé par 10 pour système stable
PID_init(&pid_delta,2000,0,0);
long max_consigne = 400; // Consigne maximale donnée au pid sans saturation (10 cm)
long max_pid = 40000*2;
while(((alpha < consigne && consigne > 0) || ( alpha > consigne && consigne < 0)) && !stop_asser){
/** Actualisation position actuelle (l, r) **/
l = -motors_get_qei(LEFT);//*4; //recupere les données des codeurs, repesentant dd
motors_reset_qei(LEFT); //remise a zero des codeurs
r = -motors_get_qei(RIGHT);//*4+1;
motors_reset_qei(RIGHT);
/** Changement variable (l, r) -> (delta, alpha) **/
delta += (r + l)/2; // diviser par 2 ?
alpha += (r - l)/2; // valeur négatives ?
/** PID asservissement de position **/
if( consigne - alpha>max_consigne ){
PID_run(&pid_alpha, (max_consigne));
}
else{
if(consigne - alpha<-max_consigne){
PID_run(&pid_alpha, -(max_consigne));
}
else{ PID_run(&pid_alpha, (consigne - alpha));}
}
PID_run(&pid_delta, -delta);
commande_alpha = pid_alpha.out;
commande_delta = pid_delta.out;
/** Limitation A sur alpha **/
//*/
if ((commande_alpha-commande_alpha_old)>LIMITE_A_SUR_ALPHA) commande_alpha = commande_alpha_old+LIMITE_A;
if ((commande_alpha-commande_alpha_old)<(-LIMITE_A_SUR_ALPHA)) commande_alpha = commande_alpha_old-LIMITE_A;
commande_alpha_old = commande_alpha;
//*/
/** Changement de variable (commande_delta, commande_alpha) -> (commande_r, commande_l) **/
commande_r = ( commande_delta + commande_alpha );
commande_l = ( commande_delta - commande_alpha );
/** envoi au moteur **/
//motors_set_speed envoi commande_r sous forme de rapport cyclique (en divisant par 200)
motors_set_speed(RIGHT, commande_r*(long)VITESSE/max_pid); // avec
motors_set_speed(LEFT, commande_l*(long)VITESSE/max_pid);
}
if(consigne != 0){
consigne_alpha = 0;
motors_stop();
init_parametre();
send_message(DONE);
}
}
int main()
{
init();
pause_s(1);
motors_stop();
/* Tests */
/*
//consigneDelta = (long)720*RAPPORT_CONVERSION; // 1 tour autour d'une roue
consigneDelta = 216658; // 1 tour autour d'une roue
rapportVitesse = (((long)720*RAPPORT_CONVERSION) << 16)/consigneDelta; // de 0 (droite) à 1
// >> 16 à l'utilisation
// Si > 1 : asser sur alpha//*/
//send_message(rapportVitesse);
/*
consigneAlpha = 0;
consigneDelta = (long)14000*N/D; // 2 cases
rapportVitesse = 0;//*/
//*
consigneDelta = 0;
consigneAlpha = (long)720*RAPPORT_CONVERSION; // 1 tour
rapportVitesse = 3949;//*/ (consigneDelta = 13061)
erreurAlpha += consigneAlpha;
erreurDelta += consigneDelta;
while(1) {
motors_set_speed(LEFT, Vg);
motors_set_speed(RIGHT, Vd);
// if (consigneDelta)
// while(1)
// {
// while (doitAttendre); // Attend l'interruption du Timer
// doitAttendre = TRUE;
// asser_delta();
// if (!doitAttendre) _RB5 = 0; // Timer trop rapide : allume LED debug
// }
// if (consigneAlpha)
// while(1)
// {
// while (doitAttendre); // Attend l'interruption du Timer
// doitAttendre = TRUE;
// asser_alpha();
// if (!doitAttendre) _RB5 = 0; // Timer trop rapide : allume LED debug
// }
}
return 0;
}
void CThrower::shoot()
{
printf("Throw engine SHOOT start\n");
down();
printf("SHOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOT");
motors_set_speed(ROBOT_BALL_THROW_PORT, -100);
sleep_ms(500);
motors_stop(ROBOT_BALL_THROW_PORT);
sleep_ms(100);
is_up = false;
is_down = false;
is_half_down = false;
printf("Throw engine SHOOT done\n");
}
/** Initialise le module PWM PWM1 **/
void PWM1_init () {
//P1TCONbits.PTSIDL = 1; // Arret en pause
P1TPER = MAX_SPEED / 2; // Période du timer
P1DC1 = 0; // Duty-cycle PWM1H1 = 0%
P1DC2 = 0; // Duty-cycle PWM1H2 = 0%
PWM1CON1bits.PMOD1 = 1; // Sorties PWM1H1 et PWM1L1 indépendantes
PWM1CON1bits.PMOD2 = 1; // Sorties PWM1H2 et PWM1L2 indépendantes
motors_stop();
P1TCONbits.PTEN = 1; // Active le Timer des PWMs
P1TCONbits.PTOPS = ASSER_PERIOD - 1; // Mis à 0 si modif de P1TCON
IEC3bits.PWM1IE = 1; // Autorisation de l'interruption
}
bool CCatcher::open()
{
printf("Catch engine OPEN start\n");
bool is_ok = true;
if (not is_open)
{
int try_count = 0;
is_ok = false;
while ((not is_ok) and (try_count++ < 2))
{
motors_set_speed(ROBOT_BALL_CATCH_PORT, direction * 30);
SBYTE motor_speeds[4];
int sum_motor_speeds = 0;
for (int i=0; i<4; i++) motor_speeds[i] = 1;
int i = 0;
int current_angle = motors_get_angle(ROBOT_BALL_CATCH_PORT);
time_t starttime = time(NULL);
bool timeout = false;
do
{
sleep_ms(100);
timeout = difftime(time(NULL), starttime) > 3.0; // Open arms shouldn't take more than 3 seconds, otherwise mechanical problems
motor_speeds[(i++)%4] = motors_get_motor_speed(ROBOT_BALL_CATCH_PORT);
printf("CCatcher.open %d %d %d %d (timeout=%d)\n", motor_speeds[0], motor_speeds[1], motor_speeds[2], motor_speeds[3], timeout);
sum_motor_speeds = abs(motor_speeds[0]) + abs(motor_speeds[1]) + abs(motor_speeds[2]) + abs(motor_speeds[3]);
} while ((not timeout) and (sum_motor_speeds > 0));
int reached_angle = motors_get_angle(ROBOT_BALL_CATCH_PORT);
motors_reset_angle(ROBOT_BALL_CATCH_PORT);
motors_stop(ROBOT_BALL_CATCH_PORT);
// is_ok also true when started with half open arms
int rotation = abs(current_angle - reached_angle);
is_ok = (timeout) ? false : ((is_closed) ? (rotation > 170) : (rotation > 10));
if (not is_ok and (try_count == 1))
{
printf("Trying to open arms with motor in reverse direction!\n");
direction *= -1; // Try once more with motor in another direction
sleep(1);
}
}
}
is_open = true;
is_closed = false;
printf("Catch engine OPEN result %d\n", is_ok);
return is_ok;
}
void asservir()
{
if(consigne_alpha != 0)
{
asser_tourner(consigne_alpha);
pause_ms(200); // peut etre a changer
return;
}
else if(consigne_delta != 0)
{
asser_avancer(consigne_delta);
pause_ms(200); // peut etre a changer
return;
}
else
{
motors_stop();
}
}
void PWM1_init () {
/* Initialise le module PWM PWM1 */
//P1TCONbits.PTSIDL = 1; // Arret en pause
/*
La fréquence est codée "en dur" car c'est la bonne fréquence
à utiliser : le pont en H utilisé ne supporte pas de fréquence
plus élevée, et un sifflement désagréable est produit pour
une fréquence plus basse.
*/
P1TPER = MAX_SPEED/2; // Le sifflement a disparu
P1DC1 = 0; // Duty-cycle PWM1H1 0%
P1DC2 = 0; // Duty-cycle PWM1H2 0%
PWM1CON1bits.PMOD1 = 1; // Sorties PWM1H1 et PWM1L1 indépendantes
PWM1CON1bits.PMOD2 = 1; // Sorties PWM1H2 et PWM1L2 indépendantes
motors_stop();
P1TCONbits.PTEN = 1; // Active le Timer des PWMs
}
void asser_avancer(long consigne) {
long delta = 0;
long alpha = 0;
// kp = 36000 pour limite oscillation
//PID_init(&pid_alpha,16800,170,0); // nb ki divisé par 10 pour système stable
PID_init(&pid_alpha,1000,10,0); // nb ki divisé par 10 pour système stable
//PID_init(&pid_alpha,100,0,0);
PID_init(&pid_delta,100,0,0);
long max_consigne = 13453; // Consigne maximale donnée au pid sans saturation (10 cm)
long max_pid = 1345300;
consigne = consigne*8;
long consigne_temp = 0;//max_pid* V_MAX_DEMARRAGE/VITESSE; pour partir plus vite, ne pas commencer la rampe à vitesse 0
while(((delta < consigne && consigne > 0) || ( delta> consigne && consigne < 0)) && !stop_asser){
// RAMPE
/*
if (consigne > 0) {
if (consigne_temp <= 0) consigne_temp = 0;
if (consigne_temp >= consigne) consigne_temp = consigne;
else consigne_temp += 13453 /250;
} else {
if (consigne_temp > -max_pid * V_MAX_DEMARRAGE/VITESSE) consigne_temp = -max_pid * V_MAX_DEMARRAGE/VITESSE;
if (consigne_temp < consigne_delta) consigne_temp = consigne_delta;
else consigne_temp -= 10;
}
//*/
consigne_temp = consigne;
/** Actualisation position actuelle (l, r) **/
l = -motors_get_qei(LEFT)*8; //recupere les données des codeurs, repesentant dd
r = -motors_get_qei(RIGHT)*8+3;
motors_reset_qei(LEFT);
motors_reset_qei(RIGHT);
/** Changement variable (l, r) -> (delta, alpha) **/
delta += (r + l)/2; // diviser par 2 ?
alpha += (r - l)/2; // valeur négatives ?
/** PID asservissement de position **/
PID_run(&pid_alpha, -alpha);
if( consigne_temp - delta>max_consigne ){
PID_run(&pid_delta, (max_consigne));
}
else{
if(consigne_temp - delta<-max_consigne){
PID_run(&pid_delta, -(max_consigne));
}
else{ PID_run(&pid_delta, (consigne_temp - delta));} // s'arrete brutalement
}//*/
commande_alpha = pid_alpha.out;
commande_delta = pid_delta.out;
/** Limitation A sur delta **/
if ((commande_delta-commande_delta_old ) > LIMITE_A ) commande_delta = commande_delta_old+LIMITE_A;
if ((commande_delta-commande_delta_old )<(-LIMITE_A)) commande_delta = commande_delta_old-LIMITE_A;
commande_delta_old = commande_delta;
/** Changement de variable (commande_delta, commande_alpha) -> (commande_r, commande_l) **/
commande_r = ( commande_delta + commande_alpha );
commande_l = ( commande_delta - commande_alpha );
/** envoi au moteur **/
//motors_set_speed envoi commande_r sous forme de rapport cyclique (en divisant par 200)
motors_set_speed(RIGHT, commande_r*(long)VITESSE/max_pid); // avec
motors_set_speed(LEFT, commande_l*(long)VITESSE/max_pid);
}
if(consigne != 0){
consigne_delta = 0;
motors_stop();
init_parametre();
send_message(DONE);
}
}
