Color de Pixybot Robots de seguimiento (9 / 10 paso)

Paso 9: código

Necesita la biblioteca duende desde el siguiente enlace:

Biblioteca de Pixy

También probar y actualizar con el firmware más reciente en el duende y su software de control PixyMon

Nota:

Cuando carga programas lo mejor es desconectar el duende del ISP y vuelva a hacerlo una vez.

ArchReactor Pixybot < br > / / escrito por: Apolo W. maderas / / 2015 / / / / código de marca de revisión VI / / / este código es un intigration del sistema de visión de duende en una plataforma móvil para levantar awerness del espacio maker / saben como Reactor de arco. Secciones del código incluyen los siguientes objetos seguidos, IR sensor de detección del objeto, lazo de control PID A, / / una advertencia de batería baja/diversos. y haciendo una pausa los servos de accionamiento por lo que se puede entrenar el duende. / / Partes de este código se derivan de código Adafruit industrias Pixy Pet. Porciones de este código se derivan en el código de ejemplo de pantilt CMUcam5 Pixy. < p > #include < SPI.h >< br > #include < Pixy.h > #include < Servo.h >< /p > #define X_CENTER 160L #define Y_CENTER 100L #define RCS_MIN_POS 0L #define RCS_MAX_POS 1000 L #define RCS_CENTER_POS ((RCS_MAX_POS-RCS_MIN_POS)/2) / / número de muestras analógicas tomar por lectura #define NUM_SAMPLES 10 / / constantes no va a cambiar. Se utilizan aquí para / / conjunto de números de pin: int suma = 0; suma de las muestras tomadas unsigned char sample_count = 0; actual muestra número const buttonPin int = 2; el número de lo pin pulsador const int ledPin = 13; el número de pines de lo LED pin int LEDUFO = 5; el número de pines de lo LED pin int LEDPlataforma = 7; el número de pin del perno del LED / cambiará las variables: int buttonState = 0; variable para leer el estado del pulsador Servo leftServo; Definir el rightServo de Servos Servo; Servo gripperServo; int analogInput = A0; unsigned int crudo; doble vcc = 0; voltaje doble; calcula voltaje const int irSenseleft = A1; Conecte el sensor al pin analógico A0 int distanceleft = 0; const int irSenseright = A2; Conecte el sensor al pin analógico A0 int distanceright = 0; clase ServoLoop {público: ServoLoop (pgain int32_t, int32_t dgain); void update (error int32_t) m_pos int32_t; 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 (error int32_t) {larga int vel; char buf [32]; si (m_prevError! = 0x80000000) {vel = (error * m_pgain + (error - 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; else if (m_pos > 3; si (tamaño > = 500); Grip(); Si (millis() - hold > random(3000, 7000)) / / Drop(); } else if (millis() - lastBlockTime > 3000) {leftServo.writeMicroseconds(1510); rightServo.writeMicroseconds(1510); ScanForBlocks(); } Si (millis ()-temporizador > 2999) / / si por lo menos transcurrieron 3000mS {timer=millis(); / / reset contador de tiempo readVcc(); Voltage(); } Si (millis ()-timerA > 49) //Wait 50 ms entre cada lectura para IR / / según hoja de datos tiempo entre cada lectura / es-38ms +/-10ms. Esperar 50 ms cada uno asegura / leer es de una muestra de diferentes {timerA=millis(); / rearme temporizador Pausebutton(); irRead();} si (tensión > = 6) {digitalWrite (LEDUFO, HIGH);} else if ((tensión > = 5) & & (voltaje < 6)) {digitalWrite (LEDUFO, LOW); digitalWrite (LEDPlataforma, HIGH);} else if (voltaje < 5) {leftServo.writeMicroseconds(1510); / / Hold motores por poer bajo estado rightServo.writeMicroseconds(1510); digitalWrite (LEDUFO, LOW); digitalWrite (LEDPlataforma ALTO); LED para avisar del retraso de batería baja (500); digitalWrite (LEDPlataforma, bajo); Delay (500); }} int oldX, oldY, oldSignature; ---/ / Bloques de pista por el duende de pan/tilt mech / / (basado en parte en CMUcam5 de Pixy pantilt ejemplo) / /---int TrackBlock (int blockCount) {int trackedBlock = 0; largo maxSize = 0; uint32_t mantenga = 0; Serial.Print ("bloques ="); Serial.println(blockCount); para (int i = 0; i < blockCount; i ++) {si ((oldSignature == 0) || (Pixy.blocks[i].Signature == oldSignature)) {largo newSize = pixy.blocks[i].height * pixy.blocks[i].width; si (newSize > maxSize) {trackedBlock = i; maxSize = newSize;}} } int32_t panError = X_CENTER - pixy.blocks[trackedBlock].x; int32_t tiltError = pixy.blocks[trackedBlock].y - Y_CENTER; 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; volver trackedBlock; } void FollowBlock (int trackedBlock) {int32_t followError = pixy.blocks[0] 2.x-X_CENTER; / / cómo lejano fuera del centro estamos mirando ahora

? Tamaño es el área del objeto. Mantener un funcionamiento promedio de los últimos 8. tamaño += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height; tamaño = >> 3; Velocidad de avance disminuye a medida que nos acercamos el objeto (el tamaño es más grande) int forwardSpeed = restringir (400 - (tamaño/400), -100, 400); Dirección diferencial es proporcional al error veces la velocidad de avance int32_t diferencial = (followError + (followError * forwardSpeed)) >> 6; Regulación de las velocidades de izquierda y derecha por el manejo diferencial. int leftSpeed = restringir (forwardSpeed + diferencial, -400, 400); int rightSpeed = restringir (forwardSpeed - diferencial, -400, 400); leftSpeed = map(leftSpeed,-400,400,1650,1350); Mapa para servos valores rightSpeed = map(rightSpeed,-400,400,1350,1650); Mapa de valores de salida del servo / / actualización de servos leftServo.writeMicroseconds(leftSpeed); rightServo.writeMicroseconds(rightSpeed); } int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150; lastMove de uint32_t = 0; ---/ / Random buscar bloques / / / / este código gira lentamente en un círculo / / hasta que se detecta un bloque / /---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; //if (scanIncrement < 0) leftServo.writeMicroseconds(1460); rightServo.writeMicroseconds(1460); delay (20), pixy.setServos (panLoop.m_pos, tiltLoop.m_pos);}} Tomar múltiples lecturas y les promedio hacia fuera para reducir irRead() vacío lecturas falsas {int averagingleft = 0; / / tiene valor en un promedio de lecturas int averagingright = 0; / / tiene valor a las lecturas promedio / / obtener un muestreo de 5 lecturas de sensor //for (int i = 0; me < 3; i ++) {distanceleft = analogRead(irSenseleft); averagingleft = averagingleft + distanceleft; distanceright = analogRead(irSenseright); averagingright = averagingright + distanceright;} distanceleft = averagingleft / 5; / / media a lecturas / / return(distanceleft); / retorno valor Serial.println (distanceleft DEC); distanceright = averagingright / 5; Promedio a lecturas / / return(distanceright); Valor devuelto Serial.println (distanceright, DEC); Si (distanceleft > 85) {leftServo.writeMicroseconds(1510); //allstop rightServo.writeMicroseconds(1510); delay(200); leftServo.writeMicroseconds(1660); //backup rightServo.writeMicroseconds(1460); delay(500); leftServo.writeMicroseconds(1460); //spin rightServo.writeMicroseconds(1460); delay(300);} else if (distanceright > 85) {//allstop leftServo.writeMicroseconds(1510); rightServo.writeMicroseconds(1510); delay (200);}} void Voltage() {vcc = readVcc () / 1000.0, crudo = analogRead(analogInput); voltaje = ((raw / 1023.0) * vcc) * 2; Serial.Print ("voltaje de la batería ="); Serial.println (voltaje, DEC); Serial.Print ("voltaje VCC ="); Serial.println (vcc, DEC); } readVcc() largo {mucho resultado; / / leer 1.1V referencia contra AVcc ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1); delay(2); / / espere Vref resolver ADCSRA | = _BV(ADSC); / / convertir mientras (bit_is_set(ADCSRA,ADSC)); resultado = ADCL; resultado | = ADCH << 8; resultado = 1106400 L / resultado; / / espalda-calcular AVcc en mV return resultado;} void Pausebutton() {sample_count = 0; en definitiva = 0; buttonState = digitalRead(buttonPin); / / comprobar si el pulsador está presionado. / Si es el buttonState es alta: Si (buttonState == HIGH) {/ / enciende el LED: leftServo.writeMicroseconds(1510); rightServo.writeMicroseconds(1510); delay(18000);}}