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