void repeatSongSelection() { AUDIO_PlayFile(VOIX_REPEAT); LCD_ClearAndPrint("Bonne chance!\n"); LCD_Printf("1\n"); THREAD_MSleep(1000); LCD_Printf("2\n"); THREAD_MSleep(1000); LCD_Printf("3\n"); THREAD_MSleep(1000); LCD_Printf("GO!!!\n"); //Random number to select the song to play int songSelect = rand()%4 + 1; switch (songSelect) { case 1: repeat(PATH_CLAIRE_FONTAINE); break; case 2: repeat(PATH_HYMNE_A_LA_JOIE); break; case 3: repeat(PATH_STAR_WARS); break; case 4: repeat(PATH_RENNE_NEZ_ROUGE); break; default: break; } }
void fear() { DIGITALIO_Write(14, 1); int i = 0; for (i = 0; i < 5; i++) { MOTOR_SetSpeed(7,-70); MOTOR_SetSpeed(8,0); DIGITALIO_Write(14, 0); THREAD_MSleep(300); MOTOR_SetSpeed(8,-75); MOTOR_SetSpeed(7,0); DIGITALIO_Write(14, 1); THREAD_MSleep(300); } for (i = 0; i < 5; i++) { MOTOR_SetSpeed(7,70); MOTOR_SetSpeed(8,0); DIGITALIO_Write(14, 0); THREAD_MSleep(300); MOTOR_SetSpeed(8,75); MOTOR_SetSpeed(7,0); DIGITALIO_Write(14, 1); THREAD_MSleep(300); } MOTOR_SetSpeed(7,0); MOTOR_SetSpeed(8,0); DIGITALIO_Write(14, 0); }
RFID_Tag read_rfid(){ int bits[RFID_BIT_COUNT] = {0, 0, 0}, read_bit = 0; enable_rfid(); THREAD_MSleep(100); while(1) { for(int i = 0; i < 4; i++){ THREAD_MSleep(50); read_bit = DIGITALIO_Read(9); if(read_bit != bits[0]){ bits[0] = read_bit; } THREAD_MSleep(50); read_bit = DIGITALIO_Read(10); if(read_bit != bits[1]){ bits[1] = read_bit; } THREAD_MSleep(50); read_bit = DIGITALIO_Read(11); if(read_bit != bits[2]){ bits[2] = read_bit; } } if (bits[0] != 0 || bits[1] != 0 || bits[2] != 0) break; } THREAD_MSleep(200); disable_rfid(); return RFID_Tag(bits); }
void turn(float motor_speed, float angle, char side) { float turn = 3.85*64*(angle/360); int angle_turned = 0; if (side == 'L') { MOTOR_SetSpeed(MOTOR_RIGHT, motor_speed); while (1) { reset_encoders(); if (angle_turned >= turn) { MOTOR_SetSpeed(MOTOR_LEFT, 0); break; } THREAD_MSleep(30); angle_turned += ENCODER_Read(ENCODER_RIGHT); } } if (side == 'R') { MOTOR_SetSpeed(MOTOR_LEFT, motor_speed); while (1) { reset_encoders(); if (angle_turned >= turn) { MOTOR_SetSpeed(MOTOR_LEFT, 0); break; } THREAD_MSleep(30); angle_turned += ENCODER_Read(ENCODER_LEFT); } } }
void happy() { DIGITALIO_Write(14, 1); MOTOR_SetSpeed(7,100); THREAD_MSleep(4000); MOTOR_SetSpeed(7,-100); THREAD_MSleep(4000); MOTOR_SetSpeed(7,0); DIGITALIO_Write(14, 0); }
void angry() { DIGITALIO_Write(14, 0); DIGITALIO_Write(15, 1); MOTOR_SetSpeed(7,-50); MOTOR_SetSpeed(8,-50); THREAD_MSleep(3000); MOTOR_SetSpeed(7,84); MOTOR_SetSpeed(8 ,90); THREAD_MSleep(1500); MOTOR_SetSpeed(7,0); MOTOR_SetSpeed(8,0); DIGITALIO_Write(15, 0); DIGITALIO_Write(14, 0); }
void defaite() { // Il manque a faire le fichier jaiperdu AUDIO_SetVolume(90); /*AUDIO_PlayFile("jaiperdu.mp3");*/ tourner(180, GAUCHE); THREAD_MSleep(3000); tourner(180, DROITE); }
void miam() { int i = 0; DIGITALIO_Write(14, 1); MOTOR_SetSpeed(7,100); THREAD_MSleep(700); MOTOR_SetSpeed(7,-100); THREAD_MSleep(700); MOTOR_SetSpeed(7,0); MOTOR_SetSpeed(8,100); THREAD_MSleep(700); MOTOR_SetSpeed(8,-100); THREAD_MSleep(700); MOTOR_SetSpeed(7,0); MOTOR_SetSpeed(8,0); DIGITALIO_Write(14, 1); }
void mainCapteur() { while (1) { lireCapteurLigne(); capt_bumper = DIGITALIO_Read(12); couleur = lireCouleur(); capt_boutonEssai = lireBoutonPhysiqueEssai(); THREAD_MSleep(20); } }
void freePlay() { AllLED(VERT); AUDIO_PlayFile(VOIX_FREE); LCD_ClearAndPrint("Je t'ecoute!\n"); LCD_ClearAndPrint("Appuie sur un des trois boutons pour revenir au menu.\n"); THREAD_MSleep(3000); PianoStream stream; stream.size = PIANO_SIZE; stream.streamID = -1; stream.currentNote = -1; Note notes[PIANO_SIZE] = {{false, true, VAL_DO1}, {false, true, VAL_RE}, {false, true, VAL_MI}, {false, true, VAL_FA}, {false, true, VAL_SOL}, {false, true, VAL_LA}, {false, true, VAL_SI}, {false, true, VAL_DO2}}; stream.notes = notes; AUDIO_SetVolume(50); //Infinite loop until the buttons call break while(1) { CheckPressedKeys(&stream); PlayAndStopNotes(&stream); THREAD_MSleep(10); if(ButtonStatus(1) == 1 || ButtonStatus(2) == 1 || ButtonStatus(3) == 1) break; } //Assure the piano has stopped playing sounds ResetStream(&stream); PlayAndStopNotes(&stream); }
void sad() { DIGITALIO_Write(14, 1); MOTOR_SetSpeed(7,100); THREAD_MSleep(2500); DIGITALIO_Write(14, 0); MOTOR_SetSpeed(7,45); MOTOR_SetSpeed(8,45); THREAD_MSleep(2000); MOTOR_SetSpeed(7, -100); THREAD_MSleep(1500); MOTOR_SetSpeed(7,45); MOTOR_SetSpeed(8,45); THREAD_MSleep(4000); MOTOR_SetSpeed(7,0); MOTOR_SetSpeed(8,0); DIGITALIO_Write(14, 0); }
void encodeurGaucheTest() { MOTOR_SetSpeed(7,30); THREAD_MSleep(100); short int encodeurGauche = 0; while(encodeurGauche <= 64) { encodeurGauche += ENCODER_Read(ENCODER_LEFT); LCD_Printf("G: %i \n",encodeurGauche); } MOTOR_SetSpeed(7,0); }
void encodeurDroitTest() { MOTOR_SetSpeed(8,30); THREAD_MSleep(100); short int encodeurDroit = 0; while(encodeurDroit <= 64) { encodeurDroit+=ENCODER_Read(ENCODER_RIGHT); LCD_Printf("D: %i \n",encodeurDroit); } MOTOR_SetSpeed(8,0); }
void vitesseTEST(robot &unRobot,short int TRANSITIONS) { MOTOR_SetSpeed(MOTOR_RIGHT, 50); MOTOR_SetSpeed(MOTOR_LEFT, 50); THREAD_MSleep(500); //Distance parcourue en nombre de transitions effectués unRobot.encodeurDroit = ENCODER_Read(ENCODER_RIGHT); unRobot.encodeurGauche = ENCODER_Read(ENCODER_LEFT); //Affichage des encodeurs et des vitesses LCD_Printf("D: %i v: %i, G: %i v: %i \n",unRobot.encodeurDroit,50,unRobot.encodeurGauche,50); }
void move_simple(float distance, int motor_speed){ float distance_remaining = distance; bool move_is_done = false; set_motors_speed(motor_speed); while(!move_is_done){ THREAD_MSleep(__PID_TIME_INTERVAL__); int encoder_read_right = ENCODER_Read(ENCODER_RIGHT); distance_remaining = distance_remaining - (encoder_read_right * __DISTANCE_PER_ENCODER__); if(distance_remaining < 0){ move_is_done = true; } } }
void Contourne_Obstacle (int direction) { bool obstacleMouvant=0; do{ float distanceInitiale=capteur.distance.distance1; THREAD_MSleep(550);//temps d'attente float variation=capteur.distance.distance1-distanceInitiale; if (absf(variation)>variationMouvement) obstacleMouvant=1; else obstacleMouvant=0; }while(obstacleMouvant||capteur.distance.distance1<distanceCritique); //Si l'obstacle n'est plus là, on continue if (capteur.distance.distance1>distanceCritique){ MOVE_Resume(); return; } //L'obstacle est fixe, on l'évite! MOVE_Tourner(45); THREAD_MSleep(550); if (capteur.distance.distance1>distanceCritique){ MOVE_Avance(20); MOVE_Tourner(-45); MOVE_Resume(); return; } else{ MOVE_Tourner(-90); MOVE_Avance(20); MOVE_Tourner(45); MOVE_Resume(); return; } }
void testInfrarouge() { LCD_ClearAndPrint("Test Infrarouge\n"); int sortie = 0; sortie = IR_Detect(IR_FRONT); switch(sortie) { case 0: LCD_Printf("RIEN\n");break; case 1: LCD_Printf("DROITE\n");break; case 2: LCD_Printf("GAUCHE\n");break; case 3: LCD_Printf("DROITE ET GAUCHE\n");break; } THREAD_MSleep(100); }
void set_expected_encoder_reads(int motor_speed){ int average_encoder_read_right = 0, average_encoder_read_left = 0, total_encoder_read_right = 0, total_encoder_read_left = 0; int calculation_period = 3000; prompt_bumpers(); reset_encoders(); set_motors_speed(motor_speed); LCD_ClearAndPrint("Testing and setting encoder expected reads for %d ms...", calculation_period); THREAD_MSleep(calculation_period); total_encoder_read_right += ENCODER_Read(ENCODER_RIGHT); total_encoder_read_left += ENCODER_Read(ENCODER_LEFT); stop(); EXPECTED_ENCODER_READ_LEFT = (int)floor(total_encoder_read_left/(calculation_period/__PID_TIME_INTERVAL__)); EXPECTED_ENCODER_READ_RIGHT = (int)floor(total_encoder_read_right/(calculation_period/__PID_TIME_INTERVAL__)); LCD_ClearAndPrint("Done!\nExpected encoder reads are:\nRight: %d\nLeft: %d\nPress any bumper to continue!",EXPECTED_ENCODER_READ_RIGHT,EXPECTED_ENCODER_READ_LEFT); prompt_bumpers(); reset_encoders(); }
void parcoursTest(robot &unRobot, short int TRANSITIONS) { /* reinitialiser(unRobot); encodeurDroitTest(); reinitialiser(unRobot); encodeurGaucheTest(); */ //boucle permettant de garder le programme en execution pour etre capable d'arreter son execution en pesant le bouton 'user' while(1) { MOTOR_SetSpeed(MOTOR_RIGHT, 40); MOTOR_SetSpeed(MOTOR_LEFT, 40); THREAD_MSleep(1000); } //vitesseTEST(unRobot,TRANSITIONS); }
int choixNbPastilles() { int choixFait = 0; int compteur = 2; capt_boutonNbPastilles = 0; /*Afficher(en gros) ,avec la matrice: Chiffre associé au nombre de pastilles maximales prises en compte par le robot-jouet pour la partie donnée*/ while (choixFait != 1) { capt_boutonNbPastilles = lireBoutonPhysiqueNbPastilles(); compteur += capt_boutonNbPastilles; if (compteur > 4) { compteur = 2; } /*Afficher(en gros) ,avec la matrice: Chiffre associé au nombre de pastilles maximales prises en compte par le robot-jouet pour la partie donnée*/ choixFait = lireBoutonPhysiqueEssai(); THREAD_MSleep(200); } return compteur; }
void move(float motor_speed, int expected_encoder_read, int move_dist_max) { int previous_error_right = 0; int previous_error_left = 0; double total_error_right = 0; double total_error_left = 0; double current_time = 0; int move_dist_count = 0; MOTOR_SetSpeed(MOTOR_RIGHT, motor_speed); MOTOR_SetSpeed(MOTOR_LEFT, motor_speed+1); while(1) { reset_encoders(); THREAD_MSleep(PID_ADJUST_TIME); int actual_right_encoder_read = ENCODER_Read(ENCODER_RIGHT); int actual_left_encoder_read = ENCODER_Read(ENCODER_LEFT); int actual_error_right = expected_encoder_read - actual_right_encoder_read; int actual_error_left = expected_encoder_read - actual_left_encoder_read; total_error_right += actual_error_right; total_error_left += actual_error_left; double right_P_factor = P_CONSTANT_RIGHT * actual_error_right; double left_P_factor = P_CONSTANT_LEFT * actual_error_left; double right_I_factor = total_error_right * I_CONSTANT_RIGHT; double left_I_factor = total_error_left * I_CONSTANT_LEFT; double right_D_factor = (previous_error_right - actual_error_right) * D_CONSTANT_RIGHT; double left_D_factor = (previous_error_left - actual_error_left) * D_CONSTANT_LEFT; int adjusted_speed_right = motor_speed + right_P_factor + right_I_factor + right_D_factor; int adjusted_speed_left = motor_speed + left_P_factor + left_I_factor + left_D_factor; MOTOR_SetSpeed(MOTOR_RIGHT, adjusted_speed_right); MOTOR_SetSpeed(MOTOR_LEFT, adjusted_speed_left); previous_error_right = actual_error_right; previous_error_left = actual_error_left; move_dist_count += expected_encoder_read; if (move_dist_count >= move_dist_max) { MOTOR_SetSpeed(MOTOR_RIGHT, 0); MOTOR_SetSpeed(MOTOR_LEFT, 0); total_error_right = 0; total_error_left = 0; break; } } }
void turn(float degree, char side, int speed){ float distance_remaining = get_circle_arc_length(degree, __DISTANCE_BETWEEN_WHEELS__/2); int motor, reverse_motor, encoder; if(side=='L'){ motor = MOTOR_RIGHT; reverse_motor = MOTOR_LEFT; encoder = ENCODER_RIGHT; }else if(side=='R'){ motor = MOTOR_LEFT; reverse_motor = MOTOR_RIGHT; encoder = ENCODER_LEFT; } MOTOR_SetSpeed(motor, speed); MOTOR_SetSpeed(reverse_motor, -speed); while(distance_remaining >= 0){ THREAD_MSleep(250); int encoder_read = ENCODER_Read(encoder); distance_remaining = distance_remaining - (encoder_read * __TURN_DISTANCE_PER_ENCODER__); } stop_and_wait(10); reset_encoders(); }
void move(float distance, int motor_speed){ float distance_remaining = distance; int encoder_error_right = 0, encoder_error_left = 0, total_error_right = 0, total_error_left = 0; bool move_is_done = false; set_motors_speed(motor_speed); while(!move_is_done){ if(read_bumpers()){ stop_and_wait(250); }else{ THREAD_MSleep(__PID_TIME_INTERVAL__); int encoder_read_right = ENCODER_Read(ENCODER_RIGHT); int encoder_read_left = ENCODER_Read(ENCODER_LEFT); total_error_right += encoder_error_right; total_error_left += encoder_error_left; encoder_error_right = pid(encoder_read_right, MOTOR_RIGHT, EXPECTED_ENCODER_READ_RIGHT, encoder_error_right, total_error_right, motor_speed, __PID_PROPORTIONAL_GAIN__, __PID_INTEGRAL_GAIN__, __PID_DERIVATIVE_GAIN__, __PID_TIME_INTERVAL__); encoder_error_left = pid(encoder_read_left, MOTOR_LEFT, EXPECTED_ENCODER_READ_LEFT, encoder_error_left, total_error_left, motor_speed, __PID_PROPORTIONAL_GAIN__, __PID_INTEGRAL_GAIN__, __PID_DERIVATIVE_GAIN__, __PID_TIME_INTERVAL__); distance_remaining = distance_remaining - (encoder_read_right * __DISTANCE_PER_ENCODER__); if(distance_remaining < 0){ move_is_done = true; } } } }
void stop_and_wait(int time){ stop(); THREAD_MSleep(time); }
int mainCRJ() { int essai, comptePastille, pastille = 0; int vitesse = 50; int direction = 1; int gagner = 0; gestionAvantDeCommencer(); transmettreMot(1, "A TOI"); for (essai = 1; essai < 11 && !gagner; ++essai) { if (essai % 2 == 1) { pastille = 0; } else { pastille = 3; } while (capt_boutonEssai != 1) { THREAD_MSleep(20); } MOTOR_SetSpeed(MOTOR_RIGHT, vitesse * vitesseDroitePRGauche); MOTOR_SetSpeed(MOTOR_LEFT, vitesse); suivreLigne(); for (comptePastille = 0; comptePastille < 4; ++comptePastille) { while (couleur == eBLANC || couleur == eNOIR || couleur == eORANGE) { suivreLigne(); THREAD_MSleep(20); } stockerCouleur(couleur, essai, pastille); if (essai % 2 == 1) { ++pastille; } else { --pastille; } while (couleur != eBLANC) { suivreLigne(); THREAD_MSleep(20); } } while (couleur != eORANGE) { suivreLigne(); THREAD_MSleep(20); } tourner(180, DROITE); //transmettre essai, verif couleurs, etc int verifNbrCouleurABonnePlace(essai); } /* gestionAvantDeCommencer(); int essai, pastille = 0; int direction = 1; int gagner = 0; while (essai != 10 && gagner != 1) { while (capt_boutonEssai == 1) { suivreLigne(); if (direction == 1) { while (pastille != nbPastilles) { int couleurCaptee = lireCouleur(); if (couleurCaptee != blanc) { stockerCouleur(couleurCaptee, essai, pastille); pastille++; do { int couleur = lireCouleur(); THREAD_MSleep(200); } while (couleur != blanc); } } direction *= (-1); MOTOR_SetSpeed(MOTOR_LEFT, 0); MOTOR_SetSpeed(MOTOR_RIGHT, 0); tourner(180, DROITE); } else { pastille = (nbPastilles - 1); while (pastille != (-1)) { int couleurCaptee = lireCouleur(); if (couleurCaptee != blanc) { stockerCouleur(couleurCaptee, essai, pastille); pastille--; do { int couleur = lireCouleur(); THREAD_MSleep(200); } while (couleur != blanc); } } direction *= (-1); MOTOR_SetSpeed(MOTOR_LEFT, 0); MOTOR_SetSpeed(MOTOR_RIGHT, 0); } THREAD_MSleep(200); verifNbrCouleurOK(essai); verifNbrCouleurABonnePlace(essai); storeDansStructure(essai); envoieStringStructure(essai); debugAffichage(direction, essai); //Ajouter : Fonction d'affichage sur la matrice de LED int i = 0; int j = 0; for (i = 0; i < nbPastilles; i++) { if (tableau_a_verifier[i][essai] == vert) j++; } if (j == nbPastilles) gagner = 1; essai++; } THREAD_MSleep(200); } if (gagner == 1) victoire(); else defaite(); THREAD_MSleep(50); // Sleep for 50ms */ return 0; }