ArduBot (7 / 8 étapes)

Étape 7: programme

Nous aurons besoin de définir certains tableaux afin de stocker les coordonnées cartésiennes et polaires se sont réunis pour représenter le plan d’étage de quelque pièce le robot est en. Nous devrons également un autre tableau pour indiquer le délai entre les pings afin de calculer le thêta.

const int pingPin = 7 ;
const int DIRA = 13 ;   définit la direction du moteur une
const int Copieztouslesfichiers = 12 ;   définit la direction du moteur b
const int PW101 = 11 ;   modulation de la largeur du moteur d’impulsion un
const int PWMB = 3 ;    modulation de largeur d’impulsion pour moteur b
int n = 16 ;            nombre de points de données enregistré pour scan () ; -1
int ping [16] ;          stocke les données de PING pour compléter
int theta [16] ;         stocke des données polaires pour données point n
int x [16] ;             magasins cartésiennes x valeur de données point n
int y [16] ;             stocke la valeur y cartésienne pour données point n
int t [16] ;             utilisées pour calculer les thêta ; magasins changent dans le temps de début de balayage de données point n
int spindelay = 3200 ;  temps en ms qu’il faut le robot pour faire tourner une rotation complète
delta int = 3200/16 ;             Delta = spindelay/n ; durée pour une rotation
unsigned longue durée, pouces, cm, dir ;   durée mesure le temps entre les PINGs
minsafe long = 15 ;                         distance de sécurité minimale devant robot
int count ;                                 utilisé pour aider à identifier lorsque le robot est dans un coin
omega int = 2*3.1416/3.2 ;             Vitesse angulaire de rotation robot = 2pi/spindelay

void setup() {}
pinMode (DIRA, OUTPUT); //Sets direction du moteur A
pinMode (DIRB, OUTPUT); //Sets direction du moteur B
pinMode (PW101, OUTPUT); //Turns A sur et en dehors
pinMode (PWMB, OUTPUT); //Turns B sur et en dehors

/ * Si DIRA = LOW, tours moteur A FWD
Si DIRB = LOW, tours moteur B FWD
*/

initialiser la communication série :
Serial.Begin(9600) ;
}

void FWD () {//sets moteurs de transmettre
digitalWrite (DIRA, élevé) ;
digitalWrite (PW101, élevé) ;
digitalWrite (DIRB, élevé) ;
digitalWrite (PWMB, élevé) ;
}

void REV () {//sets moteurs pour inverser
digitalWrite (DIRA, faible) ;
digitalWrite (PW101, élevé) ;
digitalWrite (DIRB, faible) ;
digitalWrite (PWMB, élevé) ;
}

void droite () {//turns droite sur place
digitalWrite (DIRA, élevé) ;
digitalWrite (PW101, élevé) ;
digitalWrite (DIRB, faible) ;
digitalWrite (PWMB, élevé) ;
}

void gauche () {//turns laissé sur place
digitalWrite (DIRA, faible) ;
digitalWrite (PW101, élevé) ;
digitalWrite (DIRB, élevé) ;
digitalWrite (PWMB, élevé) ;
}

void STOP () {}
digitalWrite (PW101, basse) ;
digitalWrite (PWMB, basse) ;
}

unsigned long PING() {
pinMode (pingPin, sortie) ; Faire la Pingpin à la sortie
digitalWrite (pingPin, basse) ; Envoyer une impulsion faible
delayMicroseconds(2) ; attendez deux microsecondes
digitalWrite (pingPin, HIGH) ; Envoyer une impulsion de haute
delayMicroseconds(5) ; attendez 5 secondes micro
digitalWrite (pingPin, basse) ; Envoyer une impulsion faible
pinMode(pingPin,INPUT) ; passer la Pingpin à l’entrée
durée = pulseIn (pingPin, HIGH) ; Ecoute pour echo

/ * Convertir micro secondes en pouces
*/

po = microsecondsToInches(duration) ;
cm = microsecondsToCentimeters(duration) ;
Serial.Print (cm) ;
Serial.Print ("CM") ;
Serial.println () ;

retour cm ;

}

{microsecondsToInches(long microseconds) long
retourner les microsecondes / 74 / 2 ;
}
{microsecondsToCentimeters(long microseconds) long
retourner les microsecondes / 29 / 2 ;
}

unsigned long leftping () {//records distance vers la gauche du robot et le renvoie vers le centre
int gauche ;
(DE GAUCHE) ;
retard (420) ;
STOP () ;
Delay(10) ;

Serial.Print ("gauche:") ;
gauche = PING () ;
retard (420) ;

(DE DROITE) ;
retard (420) ;
STOP () ;
retour gauche ;
}

unsigned long rightping () {//records distance à la droite du robot et le renvoie vers le centre
droit int ;
(DE DROITE) ;
retard (420) ;
STOP () ;
retard (10) ;

Serial.Print ("droit:") ;
droite = PING () ;
retard (420) ;

(DE GAUCHE) ;
retard (420) ;
STOP() ;
retourner à droite ;
}

unsigned long forwardping () {//essentiallhy PING
int moyen ;
STOP () ;
retard (10) ;
Serial.Print ("moyenne:") ;
moyenne = PING () ;
retour moyen ;

}

unsigned long choisi () {}
leftping() ;
forwardping() ;
rightping() ;
if(leftping > rightping)
Si (leftping > forwardping)
(DE GAUCHE) ;
retard (420) ;
STOP() ;
dir = forwardping () ;
Si (rightping > leftping)
Si (rightping > forwardping) {}
(DE DROITE) ;
retard (420) ;
STOP() ;
dir = forwardping () ;
} else
STOP () ;
dir = forwardping () ;

}

Sub scan () {}
Delta = spindelay/n ;
int c ;
int i ;
int t ;

pour (t = 1; t < 17; t ++) {}
Theta [t] = t/8 * 3,1416 ;
}


pour (c = 0; c < 16; c ++) {}
(DE GAUCHE) ;
Delay(Delta) ;
STOP () ;
retard (200) ;

ping [c] = PING () ;

x [c] = ping[c]*sin(theta[c]) ;
y [c] = ping[c]*cos(theta[c]) ;
}

pour (i = 0; j’ai < 16; i ++) {}
Serial.println(theta[i]) ;
}
}

void loop () {}
Scan() ;
Delay(10000) ;
}