Pixybot färg spårning robotar (9 / 10 steg)

Steg 9: kod

Du behöver TOPPLUVA biblioteket från Följ länken:

TOPPLUVA bibliotek

Försök också och uppdatera till den senaste firmware på TOPPLUVA och dess kontrollerande programvara PixyMon

Obs:

När lastning program är det bäst att koppla ur TOPPLUVA från ISP-anslutningen sedan återkoppla den en gång gjort.

ArchReactor Pixybot < br > / / Skrivet av: Apollo W. virke
2015
//
Koden översynen mark VI
//
Denna kod är en intigration av TOPPLUVA Visionsystem till en mobil plattform att höja awerness av maker utrymme
vet som Arch reaktorn.
Delar av koden inkluderar efter spårade objekt, IR sensor objekt upptäcka, A PID kontrollerar kretsar,
Ett svagt batteri varning/cuttoff. och pausar enheten servon så kan du träna i TOPPLUVA.
//
Delar av denna kod kommer från Adafruit industrier TOPPLUVA Pet kod.
Delar av denna kod kommer från TOPPLUVA CMUcam5 pantilt exempelkod.
//

< 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 1000L
#define RCS_CENTER_POS ((RCS_MAX_POS-RCS_MIN_POS)/2)

antal analoga prover att ta per läsning
#define NUM_SAMPLES 10

konstanter ändras inte. De används här till
Ange pin-koder:
int summa = 0; summan av prover
unsigned char sample_count = 0; aktuella antalet stickprov
CONST int buttonPin = 2; numrera av tryckknapp PIN-koden
CONST int ledPin = 13. pin-kod för LED stift
int ledgreen = 5; pin-kod för LED stift
int ledred = 7. pin-kod för LED stift
variabler som ändras:
int buttonState = 0; variabel för att läsa tryckknapp status
Servo leftServo; Definiera servon
Servo rightServo;
Servo gripperServo;
int analogInput = A0;
unsigned int raw;
dubbel vcc = 0;
dubbel spänning; beräknade spänningen
CONST int irSenseleft = A1; Anslut sensor till analoga stift A0
int distanceleft = 0;
CONST int irSenseright = A2; Anslut sensor till analoga stift A0
int distanceright = 0;
klass ServoLoop

{
offentlig:
ServoLoop (int32_t pgain, int32_t dgain);

void uppdatering (int32_t fel);

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 (int32_t pgain, int32_t dgain)
{
m_pos = RCS_CENTER_POS;
m_pgain = pgain;
m_dgain = dgain;
m_prevError = 0x80000000L;
}

void ServoLoop::update (int32_t fel)
{
långa int vel;
char buf [32];
om (m_prevError! = 0x80000000)
{
Vel = (fel * m_pgain + (fel - m_prevError) * m_dgain) >> 10;
sprintf (buf, "%ld\n", vel);
Serial.Print(BUF);
m_pos += vel;
om (m_pos > RCS_MAX_POS)
m_pos = RCS_MAX_POS;
annars om (m_pos > 3.
om (storlek > = 500);
Grip();
om (millis() - håll > random(3000, 7000))
Drop();
}
annars om (millis() - lastBlockTime > 3000)
{
leftServo.writeMicroseconds(1510);
rightServo.writeMicroseconds(1510);
ScanForBlocks();
}
om (millis ()-timer > 2999) / / om minst 3000mS har gått
{
timer=Millis(); nollställa timern
readVcc();
Voltage();
}
om (millis ()-timerA > 49) //Wait 50 ms mellan varje läsa för IR
Enligt datablad tid mellan varje Läs
är-38ms +/-10 MS. Väntar på 50 ms försäkrar varje
Läs är från ett annat prov
{
timerA=millis(); nollställa timern
PauseButton();
irRead();
}
om (spänning > = 6) {
digitalWrite (ledgreen, hög);
}
annars om ((spänning > = 5) & & (spänning < 6))
{
digitalWrite (ledgreen, låg);
digitalWrite (ledred, hög);
}
annars om (spänning < 5)
{
leftServo.writeMicroseconds(1510); Håll motorer på grund av låg poer statligt
rightServo.writeMicroseconds(1510);
digitalWrite (ledgreen, låg);
digitalWrite (ledred, hög); LEDDE till att varna för lågt batteri
fördröjning (500).
digitalWrite (ledred, låg);
fördröjning (500).
}

}

int oldX, oldY, oldSignature;

//---------------------------------------
Spåra block via TOPPLUVA pan/tilt mech
(bygger delvis på TOPPLUVA CMUcam5 pantilt exempel)
//---------------------------------------
int TrackBlock (int blockCount)
{
int trackedBlock = 0;
långa maxSize = 0;
uint32_t håll = 0;

Serial.Print ("block =");
Serial.println(blockCount);

för (int jag = 0; jag < blockCount; i ++)
{
om ((oldSignature == 0) || (pixY.Blocks[i].Signature == oldSignature))
{
lång newSize = pixy.blocks[i].height * pixy.blocks[i].width;
om (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;
återvända trackedBlock;
}

void FollowBlock(int trackedBlock)
{
int32_t followError = pixy.blocks[0] .x-X_CENTER; Hur långt off-center är vi ser nu?

Storleken är området av objektet.
Vi hålla en löpande genomsnittliga senaste 8.
storlek += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
storlek-= storlek >> 3.

Fart framåt minskar när vi närmar oss objektet (storleken är större)
int forwardSpeed = begränsa (400 - (storlek/400), -100, 400);

Styrning differential är proportionell mot fel tider fart framåt
int32_t differential = (followError + (followError * forwardSpeed)) >> 6;

Justera vänster och höger hastigheterna av manöverorganet differential.
int leftSpeed = begränsa (forwardSpeed + differential,-400, 400);
int rightSpeed = begränsa (forwardSpeed - Differens,-400, 400);

leftSpeed = map(leftSpeed,-400,400,1650,1350); Karta till servo produktionsvärden
rightSpeed = map(rightSpeed,-400,400,1350,1650); Karta till servo produktionsvärden

Uppdatera servon
leftServo.writeMicroseconds(leftSpeed);
rightServo.writeMicroseconds(rightSpeed);
}

int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
uint32_t lastMove = 0;
//---------------------------------------
Slumpmässig sökning för block
//
Denna kod roterar långsamt i en cirkel
tills ett block detekteras
//---------------------------------------
void ScanForBlocks()
{
om (millis() - lastMove > 10)
{
lastMove = millis();
panLoop.m_pos += scanIncrement;
om ((panLoop.m_pos > = RCS_MAX_POS) || (panLoop.m_pos < = RCS_MIN_POS))
//{
tiltLoop.m_pos =(RCS_MAX_POS * 0.6);
scanIncrement = - scanIncrement;
om (scanIncrement < 0)

leftServo.writeMicroseconds(1460);
rightServo.writeMicroseconds(1460);
fördröjning (20);

pixy.setServos (panLoop.m_pos, tiltLoop.m_pos);
}
}

Ta flera avläsningar, och dem utjämnas till i genomsnitt minska felaktiga avläsningar
void irRead()
{
int averagingleft = 0; Håller värde till genomsnittliga avläsningar
int averagingright = 0; Håller värde till genomsnittliga avläsningar

Får ett urval av 5 avläsningar från sensorn
för (int jag = 0; jag < 3; i ++)
{
distanceleft = analogRead(irSenseleft);
averagingleft = averagingleft + distanceleft;
distanceright = analogRead(irSenseright);
averagingright = averagingright + distanceright;
}
distanceleft = averagingleft / 5; I genomsnitt ut avläsningar
Return(distanceleft); Returvärdet
Serial.println (distanceleft, DEC);
distanceright = averagingright / 5; I genomsnitt ut avläsningar
Return(distanceright); Returvärdet
Serial.println (distanceright, DEC);
om (distanceleft > 85)
{
leftServo.writeMicroseconds(1510); allstop
rightServo.writeMicroseconds(1510);
Delay(200);
leftServo.writeMicroseconds(1660); säkerhetskopiering
rightServo.writeMicroseconds(1460);
Delay(500);
leftServo.writeMicroseconds(1460); Spin
rightServo.writeMicroseconds(1460);
Delay(300);

}
annars om (distanceright > 85)
{
allstop
leftServo.writeMicroseconds(1510);
rightServo.writeMicroseconds(1510);
fördröjning (200);
}
}

void Voltage()
{
VCC = readVcc () / 1000.0;
rå = analogRead(analogInput);
spänning = ((raw / 1023.0) * vcc) * 2;
Serial.Print ("batterispänning =");
Serial.println (spänning, DEC);
Serial.Print ("VCC spänning =");
Serial.println (vcc, DEC);
}

lång readVcc() {
långa resultatet;
Läs 1.1v referens mot AVcc
ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
Delay(2); Vänta på Vref sedimentera
ADCSRA | = _BV(ADSC); Konvertera
samtidigt (bit_is_set(ADCSRA,ADSC));
resultat = ADCL;
resultatet | = ADCH << 8;
resultat = 1106400L / leda; Back-beräkna AVcc i mV
returnera resultat;
}

void Pausebutton()
{
sample_count = 0;
summa = 0;
buttonState = digitalRead(buttonPin);

Kontrollera om tryckknappen trycks.
om det är, är buttonState hög:
om (buttonState == hög) {
Aktivera LED:
leftServo.writeMicroseconds(1510);
rightServo.writeMicroseconds(1510);
Delay(18000);
}
}