ABLF (9 / 9 steg)

Steg 9: PD rad efterföljare

Något mer sofistikerade program. Fortfarande ett pågående arbete, men denna fungerar. KP = 1, Kd = 2

/ * ABLB
* ActoBitty linje Bot
*
* bas robot Actobotics ActoBitty:
* https://www.servocity.com/html/actobitty_2_wheel_robot_kit.html#.VthCuvkrI-U
*
* microcontroller ombord DFRobot Romeo 2 (har inbyggd motor controller):
* http://www.dfrobot.com/wiki/index.php/Romeo_V2-All_in_one_Controller _(R3)_(SKU:DFR0225)
*
* montera panel för styrelsen:
* http://www.thingiverse.com/thing:1377159
*
* linje efterföljare matris från Sparkfun:
* https://github.com/sparkfun/Line_Follower_Array
* https://learn.sparkfun.com/tutorials/sparkfun-line-follower-array-hookup-guide
* Placera som spänner från-127 (långt L) till 127 (långt R)
*
* PID snabbgenomgång
* http://letsmakerobots.com/node/39972
*
*/

#include "Wire.h" / / för I2C
#include "sensorbar.h" / / behov SparkFun bibliotek

Kommentera bort en av de fyra linjerna att matcha din SX1509 adress
stift väljer. SX1509 breakout som standard [0:0] (0x3E).
CONST uint8_t SX1509_ADDRESS = 0x3E; SX1509 I2C adress (00)
CONST byte SX1509_ADDRESS = 0x3F; SX1509 I2C adress (01)
CONST byte SX1509_ADDRESS = 0x70; SX1509 I2C adress (10)
CONST byte SX1509_ADDRESS = 0x71; SX1509 I2C adress (11)

SensorBar mySensorBar(SX1509_ADDRESS);

kommer att försöka undvika flytande punkt math
CONST byte Kp = 1;
CONST byte Kd = 2;

CONST byte MAXOMDR = 128; sakta ner saker för att testa ändamål
int Lspeed = MAXOMDR; int sedan får överstiga 255 i beräkningar, men i slutändan kommer att begränsas
int Rspeed = MAXOMDR;

CONST int ButtonPin = 0;
int buttonVal = 0;
booleska goFlag = false;
int fel = 0;
int lastError = 0;

Romeo standard pins
int Lmotor = 5; M1 Varvtalsreglering
int Rmotor = 6; M2 Varvtalsreglering
int Ldir = 4; M1 Riktning kontroll
int Rdir = 7. M1 Riktning kontroll

void setup() {
Standard: IR bara aktiveras under läsningar.
mySensorBar.setBarStrobe();
Andra alternativet: kommandot att köra hela tiden
mySensorBar.clearBarStrobe();

Standard: mörkt på ljus
mySensorBar.clearInvertBits();
Andra alternativet: ljus linje på mörka
mySensorBar.setInvertBits();

Glöm inte att ringa .begin() för att få baren redo. Detta konfigurerar HW.
uint8_t returnStatus = mySensorBar.begin();
/*
IF(returnStatus)
{
Serial.println ("sx1509 IC kommunikation OK");
}
annat
{
Serial.println ("sx1509 IC kommunikation misslyckades!");
}
Serial.println();
*/

} / / end setup()

void loop() {

buttonVal = analogRead(ButtonPin);

om (buttonVal < 30) {/ / knapp 1
halt();
goFlag = false;
}
annars om (buttonVal < 175) {/ / knappen 2
Kommandot att köra hela tiden - tillåter kalibrering
mySensorBar.clearBarStrobe();
int jag = mySensorBar.getPosition();
}
annars om (buttonVal < 360) {/ / knapp 3
Standard: IR bara aktiveras under läsningar.
mySensorBar.setBarStrobe();
}

annars om (buttonVal < 540) {/ / knappen 4
/ / för framtida bruk
// }

annars om (buttonVal < 800) {/ / knapp 5 - kör linje efterföljare program
goFlag = sant;
Delay(3000); 3 SEK fördröjning att backa
inkludera visuell indikator senare
}

om (goFlag) {
fel = mySensorBar.getPosition(); ståndpunkten ger avstånd från mittlinjen, dvs felet

vill vara så snabb som möjligt, så kommer bara sakta ner nödvändiga hjul för korrigering
i stället försöker påskynda en långsam andra
om (fel < 0) {/ / robot har hamnat rätt, långsamt ner L hjul
Rspeed = MAXOMDR;
Lspeed = MAXSPEED + (Kp * fel) + (Kd * (fel - lastError)); plus eftersom felet är negativa, kommer att resultera i negativa värden proportionerliga sikt
Lspeed = begränsa (Lspeed, 0, MAXOMDR);

}
annars om (fel > 0) {/ / robot har glidit L; sakta ner R hjul
Rspeed = MAXOMDR - (Kp * fel)-(Kd * (fel - lastError));
Rspeed = begränsa (Rspeed, 0, MAXOMDR);
Lspeed = MAXOMDR;
}
annat {/ / position är noll; full på båda
Rspeed = MAXOMDR;
Lspeed = MAXOMDR;
}

FWD(Lspeed,rspeed);

} / / end om (goFlag)
} / / end loop()

void halt(void) / / stopp
{
digitalWrite(Lmotor,LOW);
digitalWrite(Rmotor,LOW);
}

void fwd (byte l, byte r) / / flytta fram
{
analogWrite (Lmotor, l); PWM varvtalsreglering
digitalWrite(Ldir,LOW); LÅG för fwd
analogWrite (Rmotor, r);
digitalWrite(Rdir,LOW);
}

inte behöver dessa funktioner för grundläggande linje efterföljare
void rev (byte l, byte r) / / back
//{
analogWrite (Lmotor, l);
digitalWrite(Ldir,HIGH);
analogWrite (Rmotor, r);
digitalWrite(Rdir,HIGH);
//}
void spinR (byte l, byte r)
//{
analogWrite (Lmotor, l);
digitalWrite(Ldir,LOW); L fwd, R rev att snurra R (medsols)
analogWrite (Rmotor, r);
digitalWrite(Rdir,HIGH);
//}
void spinL (byte l, byte r)
//{
analogWrite (Lmotor, l);
digitalWrite(Ldir,HIGH); R fwd, L rev att snurra L (moturs)
analogWrite (Rmotor, r);
digitalWrite(Rdir,LOW);
//}