Skip to content

Instantly share code, notes, and snippets.

@HorlogeSkynet

HorlogeSkynet/ITERRobot2014.c

Last active May 25, 2019 13:44
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save HorlogeSkynet/ec6e2fa10d7a92960524f8b82bb4a265 to your computer and use it in GitHub Desktop.
Save HorlogeSkynet/ec6e2fa10d7a92960524f8b82bb4a265 to your computer and use it in GitHub Desktop.
Download ZIP
Our code for the ITER Robot 2014 edition
Raw
ITERRobot2014.c
#include "LSA-lib.nxc" //http://www.mindsensors.com/index.php?module=documents&JAS_DocumentManager_op=viewDocument&JAS_Document_id=219
#define LeftMotor OUT_A
#define RightMotor OUT_C
#define BothMotors OUT_AC
#define Plateforme OUT_B
#define Capteur_Couleur IN_1
#define Capteur_Ligne IN_2
#define ADDR 0x14
#define INPUT_BLACKCOLOR 1
#define INPUT_BLUECOLOR 2
#define INPUT_GREENCOLOR 3
#define INPUT_YELLOWCOLOR 4
#define INPUT_REDCOLOR 5
#define INPUT_WHITECOLOR 6
#define Blanc 90
#define Noir 10
#define nbLentilles 8
#define nbCourses 3
#define nbPieces 5
#define puissance_moteur 30
#define correctionFine 1.5
#define correctionMoyenne 1.8
#define correctionForte 2.1
#define tempsAngleDroit 1250
#define tempsDemiTour 4500
#define puissance_AngleDroit 35
#define puissance_plateforme 30
bool prendreVirage(bool aller, int numeroCourse, int nbAnglesGauche, int nbAnglesDroit int nbAllerRetour)
{
if(aller) //Aller: On ne prend pas tous les Virages a Gauche !
{
switch(numeroCourse)
{
case 1:
if(nbAnglesGauche == nbAllerRetour)
{
return true;
}
else
{
return false;
}
break;
case 2:
if((nbAnglesGauche == 1 && nbAllerRetour == 1) || (nbAnglesGauche == 3 && nbAllerRetour == 2) || (nbAnglesGauche == 5 && nbAllerRetour == 3))
{
return true;
}
else
{
return false;
}
break;
case 12:
case 13:
case 14:
case 15:
if(nbAnglesGauche == (numeroCourse - 10))
{
return true;
}
else
{
return false;
}
break;
default:
TextOut(0, 0, "Probleme #2.", 0);
for(int i = 0; i < 3; i++)
{
SetSensorColorRed(Capteur_Couleur);
Wait(50);
SetSensorColorNone(Capteur_Couleur);
Wait(50);
}
return false;
break;
}
}
else //Retour: On prend tous les Virages a Gauche !
{
return true;
}
}
void suivre_ligne(bool aller, int numeroCourse, int nbAllerRetour)
{
SetSensorLowspeed(Capteur_Ligne);
LSA_WakeUp(Capteur_Ligne, ADDR);
int ValeurLue[nbLentilles] = {0};
bool ligneNoireRencontree = false;
int nbAnglesGauche = 0, nbAnglesDroit = 0;
while(1)
{
SetSensorColorNone(Capteur_Couleur);
if(LSA_ReadRaw_Calibrated(Capteur_Ligne, ADDR, ValeurLue))
{
//Ligne Noire
if((ValeurLue[0] < Noir) && (ValeurLue[1] < Noir) && (ValeurLue[2] < Noir) && (ValeurLue[3] < Noir) && (ValeurLue[4] < Noir) && (ValeurLue[5] < Noir) && (ValeurLue[6] < Noir) && (ValeurLue[7] < Noir))
{
SetSensorColorRed(Capteur_Couleur);
if(!ligneNoireRencontree) //premiere Ligne Noire rencontree, AngleDroit sur la Droite
{
OnFwd(BothMotors, puissance_moteur);
Wait(100);
OnRev(RightMotor, puissance_AngleDroit);
OnFwd(LeftMotor, puissance_AngleDroit);
Wait(tempsAngleDroit);
ligneNoireRencontree = true;
}
else //Ligne Noire deja rencontree, on s arrete car fin momentanee du parcours
{
break;
}
}
//Ligne Blanche
else if((ValeurLue[0] > Blanc) && (ValeurLue[1] > Blanc) && (ValeurLue[2] > Blanc) && (ValeurLue[3] > Blanc) && (ValeurLue[4] > Blanc) && (ValeurLue[5] > Blanc) && (ValeurLue[6] > Blanc) && (ValeurLue[7] > Blanc))
{
OnRev(BothMotors, puissance_moteur * correctionFine);
Wait(350);
}
//Ligne Droite
else if((ValeurLue[3] < Noir) && (ValeurLue[4] < Noir) && (ValeurLue[0] > Blanc) && (ValeurLue[1] > Blanc) && (ValeurLue[2] > Blanc) && (ValeurLue[5] > Blanc) && (ValeurLue[6] > Blanc) && (ValeurLue[7] > Blanc))
{
OnFwd(BothMotors, puissance_moteur * correctionMoyenne);
}
//Angle Droit...
else if((ValeurLue[0] < Noir) && (ValeurLue[1] < Noir) && (ValeurLue[2] < Noir) && ((ValeurLue[3] < Noir) || (ValeurLue[4] < Noir)) /*&& (ValeurLue[5] > Blanc)*/ && (ValeurLue[6] > Blanc) && (ValeurLue[7] > Blanc)) //... a Gauche
{
SetSensorColorBlue(Capteur_Couleur);
nbAnglesGauche++;
if(prendreVirage(aller, numeroCourse, nbAnglesGauche, nbAnglesDroit, nbAllerRetour))
{
OnFwd(BothMotors, puissance_AngleDroit);
Wait(100);
OnRev(LeftMotor, puissance_AngleDroit);
OnFwd(RightMotor, puissance_AngleDroit);
Wait(tempsAngleDroit);
}
}
else if(((ValeurLue[3] < Noir) || (ValeurLue[4] < Noir)) && (ValeurLue[5] < Noir) && (ValeurLue[6] < Noir) && (ValeurLue[7] < Noir) && (ValeurLue[0] > Blanc) && (ValeurLue[1] > Blanc) /*&& (ValeurLue[2] > Blanc)*/) //... a Droite
{
SetSensorColorGreen(Capteur_Couleur);
nbAnglesDroit++;
}
//Correction Fine
else if((ValeurLue[2] < Noir) && (ValeurLue[0] > Blanc) && (ValeurLue[1] > Blanc) /*&& (ValeurLue[3] > Blanc)*/ && (ValeurLue[4] > Blanc) && (ValeurLue[5] > Blanc) && (ValeurLue[6] > Blanc) && (ValeurLue[7] > Blanc)) //... sur la Gauche
{
OnFwd(RightMotor, puissance_moteur * correctionMoyenne);
OnFwd(LeftMotor, puissance_moteur);
}
else if((ValeurLue[5] < Noir) && (ValeurLue[0] > Blanc) && (ValeurLue[1] > Blanc) && (ValeurLue[2] > Blanc) && (ValeurLue[3] > Blanc) /*&& (ValeurLue[4] > Blanc)*/ && (ValeurLue[6] > Blanc) && (ValeurLue[7] > Blanc)) //... sur la Droite
{
OnFwd(LeftMotor, puissance_moteur * correctionFine);
OnFwd(RightMotor, puissance_moteur);
}
//Correction Moyenne
else if((ValeurLue[1] < Noir) && (ValeurLue[0] > Blanc) /*&& (ValeurLue[2] > Blanc)*/ && (ValeurLue[3] > Blanc) && (ValeurLue[4] > Blanc) && (ValeurLue[5]) && (ValeurLue[6] > Blanc) && (ValeurLue[7] > Blanc)) //... sur la Gauche
{
OnFwd(RightMotor, puissance_moteur * correctionMoyenne);
OnFwd(LeftMotor, puissance_moteur);
}
else if((ValeurLue[6] < Noir) && (ValeurLue[0] > Blanc) && (ValeurLue[1] > Blanc) && (ValeurLue[2] > Blanc) && (ValeurLue[3] > Blanc) && (ValeurLue[4] > Blanc) /*&& (ValeurLue[5] > Blanc)*/ && (ValeurLue[7] > Blanc)) //... sur la Droite
{
OnFwd(RightMotor, puissance_moteur);
OnFwd(LeftMotor, puissance_moteur * correctionMoyenne);
}
//Correction Forte
else if(((ValeurLue[6] < Noir) || (ValeurLue[7] < Noir)) && (ValeurLue[0] > Blanc) && (ValeurLue[1] > Blanc) && (ValeurLue[2] > Blanc) && (ValeurLue[3] > Blanc) && (ValeurLue[4] > Blanc) && (ValeurLue[5] > Blanc)) //... sur la Gauche
{
OnFwd(LeftMotor, puissance_moteur * correctionForte);
OnFwd(RightMotor, puissance_moteur);
}
else if(((ValeurLue[0] < Noir) || (ValeurLue[1] < Noir)) && (ValeurLue[2] > Blanc) && (ValeurLue[3] > Blanc) && (ValeurLue[4] > Blanc) && (ValeurLue[5] > Blanc) && (ValeurLue[6] > Blanc) && (ValeurLue[7] > Blanc)) //... sur la Droite
{
OnFwd(LeftMotor, puissance_moteur);
OnFwd(RightMotor, puissance_moteur * correctionForte);
}
else
{
OnFwd(BothMotors, puissance_moteur);
Wait(100);
}
}
else
{
TextOut(0, LCD_LINE1, "Probleme #1.", 0);
for(int i = 0; i < 3; i++)
{
SetSensorColorRed(Capteur_Couleur);
Wait(50);
SetSensorColorNone(Capteur_Couleur);
Wait(50);
}
ClearScreen();
}
}
Float(BothMotors);
Wait(1000);
LSA_Sleep(Capteur_Ligne, ADDR);
}
bool TestPresencePiece()
{
SetSensorColorFull(Capteur_Couleur);
if(Sensor(Capteur_Couleur) == INPUT_REDCOLOR)
{
return true;
}
else
{
return false;
}
}
void saisie_depot(bool chargement)
{
if(chargement)
{
bool piecePresente = false;
while(!piecePresente)
{
SetSensorColorNone(Capteur_Couleur);
OnRev(Plateforme, puissance_plateforme); //Position Basse
Wait(1000);
Off(Plateforme);
Wait(500);
RotateMotor(Plateforme, puissance_moteur, 80); //Un peu au-dessus de l horizontale
Off(Plateforme);
Wait(250);
Float(BothMotors);
Wait(500);
if(TestPresencePiece())
{
piecePresente = true;
SetSensorColorGreen(Capteur_Couleur);
Wait(500);
OnRev(BothMotors, puissance_moteur + 10); //Reculer
Wait(750);
RotateMotor(Plateforme, puissance_moteur, 10);
Off(Plateforme);
}
else
{
OnRev(BothMotors, puissance_moteur + 10); //Reculer
Wait(750);
Float(BothMotors);
OnRev(Plateforme, puissance_moteur); //Position Basse
Wait(1500);
OnFwd(BothMotors, puissance_moteur + 10); //Avancer
Wait(750);
Float(BothMotors);
}
Wait(250);
}
}
else
{
RotateMotor(Plateforme, puissance_moteur, -10); //Position horizontale
Off(Plateforme);
Wait(250);
Wait(500);
OnRev(Plateforme, puissance_moteur); //Position Basse
Wait(1500);
OnRev(BothMotors, puissance_moteur + 10); //Reculer
Wait(1000);
Float(BothMotors);
Wait(500);
}
}
void demi_tour()
{
SetSensorColorBlue(Capteur_Couleur);
OnRev(BothMotors, puissance_moteur);
Wait(1000);
OnFwd(LeftMotor, puissance_AngleDroit);
OnRev(RightMotor, puissance_AngleDroit);
Wait(tempsDemiTour);
Float(BothMotors);
}
void plateforme_position_transport()
{
OnRev(Plateforme, puissance_moteur);
Wait(1500);
Float(Plateforme);
}
int choixCourse()
{
TextOut(0, LCD_LINE2, "Course:", 0);
int numeroCourse = 1;
while(!ButtonState(BTNCENTER))
{
ClearLine(LCD_LINE3);
NumOut(50, LCD_LINE3, numeroCourse, 0);
if(ButtonState(BTNRIGHT))
{
while(ButtonState(BTNRIGHT));
numeroCourse++;
if(numeroCourse > nbCourses)
{
numeroCourse = nbCourses;
}
}
else if(ButtonState(BTNLEFT))
{
while(ButtonState(BTNLEFT));
numeroCourse--;
if(numeroCourse < 1)
{
numeroCourse = 1;
}
}
}
Wait(500);
if(numeroCourse == 3)
{
ClearLine(LCD_LINE2);
TextOut(0, LCD_LINE2, "Piece:", 0);
int pieceAPrendre = 1;
while(!ButtonState(BTNCENTER))
{
ClearLine(LCD_LINE3);
NumOut(50, LCD_LINE3, pieceAPrendre, 0);
if(ButtonState(BTNRIGHT))
{
while(ButtonState(BTNRIGHT));
pieceAPrendre++;
if(pieceAPrendre > nbPieces)
{
pieceAPrendre = nbPieces;
}
}
else if(ButtonState(BTNLEFT))
{
while(ButtonState(BTNLEFT));
pieceAPrendre--;
if(pieceAPrendre < 1)
{
pieceAPrendre = 1;
}
}
}
numeroCourse = pieceAPrendre + 10;
}
return numeroCourse;
}
void concours()
{
int numeroCourse = choixCourse();
for(int nbAllerRetour = 1; nbAllerRetour < 5; nbAllerRetour++)
{
ClearScreen();
plateforme_position_transport();
suivre_ligne(true, numeroCourse, nbAllerRetour); //Aller
saisie_depot(true); //Saisie
RotateMotor(Plateforme, puissance_moteur, 10);
Off(Plateforme); //Verrouillage de la Plateforme
demi_tour(); //Demi-Tour
RotateMotor(Plateforme, puissance_moteur, 10);
Off(Plateforme); //Verrouillage de la Plateforme
suivre_ligne(false, numeroCourse, nbAllerRetour); //Retour
saisie_depot(false); //Depot
if(numeroCourse > 10) //On s arrete apres un AllerRetour simple (si Course 3)
{
break;
}
demi_tour(); //Demi-Tour
}
}
task main()
{
concours();
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment