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 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);
}
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 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 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 pid(int encoder_read, int motor_number, int expected_encoder_read, int previous_error, int total_error, int speed, float p, float i, float d, int time_interval){
int actual_error = 0;
if(encoder_read != expected_encoder_read)
actual_error = expected_encoder_read - encoder_read;
int error_difference = previous_error - actual_error;
float proportional = p * actual_error;
float integral = i * total_error;
float derivative = (d * error_difference)/time_interval;
float adjusted_speed = floor(proportional + derivative + integral + speed);
//LCD_ClearAndPrint("Previous error:%d, Actual error:%d\nP:%f\nI:%f\nD:%f\nSpeed:%d\nAdjusted_speed:%d", previous_error, actual_error, proportional, integral, derivative, speed, adjusted_speed);
MOTOR_SetSpeed(motor_number, adjusted_speed);
return actual_error;
}
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 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 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 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 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 stop_movement(){
MOTOR_SetSpeed(MOTOR_RIGHT, 0);
MOTOR_SetSpeed(MOTOR_LEFT, 0);
}
void set_motors_speed(int motor_speed){
MOTOR_SetSpeed(MOTOR_LEFT, motor_speed);
MOTOR_SetSpeed(MOTOR_RIGHT, motor_speed);
}
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;
}
void stop(){
MOTOR_SetSpeed(MOTOR_RIGHT, 0);
MOTOR_SetSpeed(MOTOR_LEFT, 0);
}
