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