Étape 9: Code
Vous aurez besoin de la bibliothèque de lutin de la suivre le lien :
Aussi essayer et mettre à jour vers le nouveau firmware sur le Pixy et son logiciel de contrôle PixyMon
Remarque :
Lorsque le chargement des programmes, il est préférable de déconnecter le Pixy du connecteur ISP puis se reconnecter une fois fait.
ArchReactor Pixybot < br > / / écrit par : Apollo W. bois / / 2015 / / / / Code de marque de révision VI / / / / ce code est une intigration du système Pixy vision en une plate-forme mobile de soulever que de l’espace machine à / / connu sous le réacteur arc. Articles du code incluent les suivants les objets suivis, la détection d’objet capteur IR, la boucle de régulation PID A, / / un avertissement de batterie faible/cuttoff. et une pause les servos de commande donc vous pouvez former le Pixy. / / Parties de ce code sont dérivés de code Adafruit Industries Pixy Pet. Certaines parties de ce code sont tirées de l’exemple du dôme de Pixy CMUcam5 code. < p > #include < SPI.h >< br > #include < Pixy.h > #include < Servo.h >< / p > #define centre_x 160L #define centre_y 100L #define RCS_MIN_POS 0L #define RCS_MAX_POS 1000 L #define RCS_CENTER_POS ((RCS_MAX_POS-RCS_MIN_POS)/2) / / nombre d’échantillons analogiques à prendre par lecture #define NUM_SAMPLES 10 / / constantes ne changeront pas. Ils sont utilisés ici pour / / set NIP : int somme = 0 ; somme des échantillons pris unsigned char sample_count = 0 ; actuel échantillon numéro const int buttonPin = 2 ; le nombre de la tige poussoir const int ledPin = 13 ; le nombre de broches de la LED pin int ledgreen = 5 ; le nombre de broches de la LED pin int ledred = 7 ; le NIP de la goupille de LED / / variables vont changer : buttonState int = 0 ; variable pour la lecture de l’État touche Servo leftServo ; Définir la rightServo Servos Servo ; Servo gripperServo ; int analogInput = A0 ; unsigned int brute ; double vcc = 0 ; double tension ; calculer la tension const int irSenseleft = A1 ; Connecter le capteur à broche analogique A0 int distanceleft = 0 ; const int irSenseright = A2 ; Connecter le capteur à broche analogique A0 int distanceright = 0 ; Class ServoLoop {public : ServoLoop (pgain int32_t, int32_t dgain); void update (erreur int32_t); int32_t m_pos ; int32_t m_prevError ; int32_t m_pgain int32_t m_dgain;} ; ServoLoop panLoop (200, 200) ; ServoLoop tiltLoop (150, 200) ; ServoLoop::ServoLoop (pgain int32_t, int32_t dgain) {m_pos = RCS_CENTER_POS ; m_pgain = pgain ; m_dgain = dgain ; m_prevError = 0x80000000L;} void ServoLoop::update (int32_t erreur) {long int vel ; char buf [32]; si (m_prevError! = 0 x 80000000) {vel = (erreur * m_pgain + (erreur - m_prevError) * m_dgain) >> 10 ; //sprintf (buf, « %ld\n », vel); / / Serial.print(buf) ; m_pos += vel ; si (m_pos > RCS_MAX_POS) m_pos = RCS_MAX_POS ; ElseIf (m_pos > 3; if (taille > = 500) ; Grip() ; Si (millis() - cale > random(3000, 7000)) / / Drop() ; } ElseIf (millis() - lastBlockTime > 3000) {leftServo.writeMicroseconds(1510) ; rightServo.writeMicroseconds(1510) ; ScanForBlocks() ; } Si (millis ()-minuterie > 2999) / / si au moins se sont écoulés 3000mS {timer=millis(); / / reset timer readVcc() ; Voltage() ; } Si (millis ()-timerA > 49) //Wait 50 ms entre chaque lecture pour IR / / fonction de l’heure de la feuille de données entre chaque lecture / / est-38ms +/-10ms. 50 ms d’attente assure chacun / / lecture est auprès d’un échantillon différent {timerA=millis(); / / reset timer Pausebutton() ; irRead();} si (tension > = 6) {digitalWrite (ledgreen, HIGH);} ElseIf ((tension > = 5) & & (tension < 6)) {digitalWrite (ledgreen, basse); digitalWrite (ledred, HIGH);} ElseIf (tension < 5) {leftServo.writeMicroseconds(1510); / / cale moteurs en raison de la faible poer state rightServo.writeMicroseconds(1510) ; digitalWrite (ledgreen, LOW); digitalWrite (ledred ÉLEVÉ) ; LED pour prévenir du retard de batterie faible (500) ; digitalWrite (ledred, basse) ; retard (500) ; }} int oldX, oldY, oldSignature ; ---/ / Blocs de piste via le Pixy pan/tilt mech / / (basée en partie sur l’exemple de dôme Pixy CMUcam5) / /---int TrackBlock (int blockCount) {int trackedBlock = 0; long maxSize = 0; uint32_t tenir = 0 ; Serial.Print ("blocs =") ; Serial.println(blockCount) ; pour (int i = 0; i < blockCount; i ++) {si ((oldSignature == 0) || (pixy.Blocks[i].signature == oldSignature)) {long newSize = pixy.blocks[i].height * pixy.blocks[i].width ; if (newSize > maxSize) {trackedBlock = i; maxSize = newSize;}} } panError int32_t = centre_x - pixy.blocks[trackedBlock].x ; int32_t tiltError = pixy.blocks[trackedBlock].y - centre_y ; panLoop.update(panError) ; tiltLoop.update(tiltError) ; pixy.setServos (panLoop.m_pos, tiltLoop.m_pos) ; oldX = pixy.blocks[trackedBlock].x ; oldY = pixy.blocks[trackedBlock].y ; oldSignature = pixy.blocks[trackedBlock].signature ; Return trackedBlock ; } void FollowBlock (int trackedBlock) {int32_t followError = pixy.blocks[0] .x-centre_x; / / comment loin hors du centre nous sommes maintenant à la recherche? La taille est la zone de l’objet. Nous gardons une course moyenne des 8 derniers. taille += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height ; Taille = Taille >> 3 ; Vitesse d’avancement diminue à mesure que nous approchons de l’objet (la taille est plus grande) int forwardSpeed = contraindre (400 - (taille/400), -100, 400) ; Prime de direction est proportionnelle à l’erreur fois la vitesse d’avancement int32_t différentielle = (followError + (followError * forwardSpeed)) >> 6 ; Ajuster les vitesses gauche et à droite par le différentiel de la direction. int leftSpeed = contraindre (forwardSpeed + différentiel, -400, 400) ; int rightSpeed = contraindre (forwardSpeed - différentielle, -400, 400) ; leftSpeed = map(leftSpeed,-400,400,1650,1350) ; Carte de sortie servo valeurs rightSpeed = map(rightSpeed,-400,400,1350,1650) ; Carte de valeurs de sortie servo / / mise à jour de servos leftServo.writeMicroseconds(leftSpeed) ; rightServo.writeMicroseconds(rightSpeed) ; } int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150 ; uint32_t lastMove = 0 ; ---/ / Random recherche pour blocs / / / / ce code tourne lentement dans un cercle / / jusqu'à un bloc de détection / /---void ScanForBlocks() {si (millis() - lastMove > 10) {lastMove = millis() ; //panLoop.m_pos += scanIncrement ; //if ((panLoop.m_pos > = RCS_MAX_POS) || (panLoop.m_pos < = RCS_MIN_POS)) {tiltLoop.m_pos =(RCS_MAX_POS * 0.6) ; //scanIncrement = - scanIncrement ; leftServo.writeMicroseconds(1460) //if (scanIncrement < 0); rightServo.writeMicroseconds(1460) ; retard (20), pixy.setServos (panLoop.m_pos, tiltLoop.m_pos);}} Prendre des lectures multiples et leur moyenne afin de réduire les fausses lectures void irRead() {int averagingleft = 0; / / contient la valeur moyenne des lectures int averagingright = 0; / / contient la valeur de lectures moyennes / / obtenir un échantillon de 5 lectures du capteur //for (int j’ai = 0; j’ai < 3; i ++) {distanceleft = analogRead(irSenseleft) ; averagingleft = averagingleft + distanceleft ; distanceright = analogRead(irSenseright) ; averagingright = averagingright + distanceright;} distanceleft = averagingleft / 5; / / moyenne des lectures / / return(distanceleft); / / Return value Serial.println (distanceleft DEC) ; DistanceRight = averagingright / 5 ; Moyenne des lectures / / return(distanceright) ; Valeur de retour Serial.println (distanceright, DEC) ; Si (distanceleft > 85) {leftServo.writeMicroseconds(1510) ; //allstop rightServo.writeMicroseconds(1510) ; delay(200) ; leftServo.writeMicroseconds(1660) ; rightServo.writeMicroseconds(1460) //backup ; delay(500) ; leftServo.writeMicroseconds(1460) ; //spin rightServo.writeMicroseconds(1460) ; delay(300);} else if (distanceright > 85) {//allstop leftServo.writeMicroseconds(1510) ; rightServo.writeMicroseconds(1510) ; retard (200);}} void Voltage() {vcc = readVcc () / 1000.0 ; brut = analogRead(analogInput) ; tension = ((raw / 1023.0) * vcc) * 2 ; Serial.Print ("tension de la batterie =") ; Serial.println (tension, DEC) ; Serial.Print ("tension VCC =") ; Serial.println (vcc, DEC) ; } readVcc() long {longtemps en résulter; / / lecture 1.1V référence contre AVcc ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1) ; delay(2); / / attendre Vref de régler ADCSRA | = _BV(ADSC); / / convertir tandis que (bit_is_set(ADCSRA,ADSC)) ; résultat = ADCL ; résultat | = ADCH << 8; résultat = 1106400 L / résulter; / / AVcc rétrocalcul en mV retournent le résultat;} void Pausebutton() {sample_count = 0; somme = 0; buttonState = digitalRead(buttonPin); / / vérifier si le bouton est appuyé. / / si c’est le buttonState est élevée : si (buttonState == HIGH) {/ / allumer la LED: leftServo.writeMicroseconds(1510) ; rightServo.writeMicroseconds(1510) ; delay(18000);}}
