Hela koden
#define MOTOR_1_SPEED_PIN 10
#define MOTOR_2_SPEED_PIN 11
#define MOTOR_1_DIR_PIN 12
#define MOTOR_2_DIR_PIN 13
#define LEFT_BUMPER_PIN 7
#define CENTER_BUMPER_PIN 8
#define RIGHT_BUMPER_PIN 9
#define FORWARD_SPEED 200
#define BACKWARD_SPEED 150
#define TURN_SPEED 120
#define BACKUP_TIME 1000
#define TURN_TIME 1000
void forward() {
digitalWrite(MOTOR_1_DIR_PIN, HIGH);
digitalWrite(MOTOR_2_DIR_PIN, HIGH);
analogWrite(MOTOR_1_SPEED_PIN, FORWARD_SPEED);
analogWrite(MOTOR_2_SPEED_PIN, FORWARD_SPEED);
}
void backward() {
digitalWrite(MOTOR_1_DIR_PIN, LOW);
digitalWrite(MOTOR_2_DIR_PIN, LOW);
analogWrite(MOTOR_1_SPEED_PIN, BACKWARD_SPEED);
analogWrite(MOTOR_2_SPEED_PIN, BACKWARD_SPEED);
}
void turn_left() {
digitalWrite(MOTOR_1_DIR_PIN, LOW);
digitalWrite(MOTOR_2_DIR_PIN, HIGH);
analogWrite(MOTOR_1_SPEED_PIN, TURN_SPEED);
analogWrite(MOTOR_2_SPEED_PIN, TURN_SPEED);
}
void turn_right() {
digitalWrite(MOTOR_1_DIR_PIN, HIGH);
digitalWrite(MOTOR_2_DIR_PIN, LOW);
analogWrite(MOTOR_1_SPEED_PIN, TURN_SPEED);
analogWrite(MOTOR_2_SPEED_PIN, TURN_SPEED);
}
void stop() {
analogWrite(MOTOR_1_SPEED_PIN, 0);
analogWrite(MOTOR_2_SPEED_PIN, 0);
}
void setup() {
pinMode(MOTOR_1_SPEED_PIN, OUTPUT);
pinMode(MOTOR_2_SPEED_PIN, OUTPUT);
pinMode(MOTOR_1_DIR_PIN, OUTPUT);
pinMode(MOTOR_2_DIR_PIN, OUTPUT);
pinMode(LEFT_BUMPER_PIN, INPUT_PULLUP);
pinMode(CENTER_BUMPER_PIN, INPUT_PULLUP);
pinMode(RIGHT_BUMPER_PIN, INPUT_PULLUP);
}
void loop() {
// Kolla om vänstra delen av stöt-sensorn rör vid något
if (digitalRead(LEFT_BUMPER_PIN) == LOW) {
// Backa lite och snurra åt höger
backward()
delay(BACKUP_TIME);
turn_right();
delay(TURN_TIME);
}
// Kolla om högra delen av stöt-sensorn rör vid något
else if (digitalRead(RIGHT_BUMPER_PIN) == LOW) {
// Backa lite och snurra åt vänster
backward()
delay(BACKUP_TIME);
turn_left();
delay(TUNR_TIME);
}
// Kolla om mitten på stöt-sensorn rör vid något
else if (digitalRead(CENTER_BUMPER_PIN) == LOW) {
// Backa lite och snurra runt slumpmässigt åt höger eller vänster
backward();
delay(BACKUP_TIME);
if (random(100) < 50) {
turn_right();
}
else {
turn_left();
}
delay(TURN_TIME);
}
// Om ingen del av stöt-sensorn är aktiverad fortsätter vi framåt
else {
forward();
}
}Beskrivning av koden
Först namnger vi olika värden som vi kommer behöva i resten av koden. Vi gör detta dels för att vi ska lätt kunna se vad värdena är till för och dels för att vi ska kunna ändra dem senare på ett enda ställe utan att behöva gå igenom hela koden och ändra på alla ställen som värdena används. Värdena vi behöver är vilka anslutningar som används av motor-kontrollern och stöt-sensorn samt hastigheter och tider för olika riktningar. Hastigheten för motorerna på roboten ska vara ett heltal mellan 0 och 255 där 0 är stillastående och 255 är full fart. Tiderna anges i millisekunder så att t.ex. 1000 motsvarar en sekund, och 500 motsvarar en halv sekund.
#define MOTOR_1_SPEED_PIN 10
#define MOTOR_2_SPEED_PIN 11
#define MOTOR_1_DIR_PIN 12
#define MOTOR_2_DIR_PIN 13
#define LEFT_BUMPER_PIN 7
#define CENTER_BUMPER_PIN 8
#define RIGHT_BUMPER_PIN 9
#define FORWARD_SPEED 200
#define BACKWARD_SPEED 150
#define TURN_SPEED 120
#define BACKUP_TIME 1000
#define TURN_TIME 1000Sedan skapar vi ett antal funktioner för att få roboten att köra och svänga i olika riktningar. De rörelserna vi har i vår kod är forward(), backward(), turn_left(), turn_right() och stop(), men det är självklart fritt fram för dig att lägga till dina egna. Funktionerna ställer in riktning och hastighet på båda motorerna genom att sätta riktnings-pinnen till hög eller låg och aktivera PWM (puls-bredds-modulering) på hastighets-pinnen. Riktningen på motorerna beror på hur de är anslutna till motor-kontrollern. Om det visar sig att en motorer snurrar åt fel håll så byter du plats på sladdarna på motorn.
void forward() {
digitalWrite(MOTOR_1_DIR_PIN, HIGH);
digitalWrite(MOTOR_2_DIR_PIN, HIGH);
analogWrite(MOTOR_1_SPEED_PIN, FORWARD_SPEED);
analogWrite(MOTOR_2_SPEED_PIN, FORWARD_SPEED);
}Innan vi kan börja köra måste vi berätta för Arduino-kortet vilka anslutningar som ska vara utgångar och vilka som ska vara ingångar. De fyra anslutningarna som styr motorerna ska vara utgångar och de tre anslutningarna som stöt-sensorn är kopplad till ska vara ingångar. Vi använder ”INPUT_PULLUP” istället för bara ”INPUT”. Detta har att göra med att stöt-sensorn inte ger ut en ordentlig signal när den inte är aktiverad och för att undvika att Arduino-kortet ska plocka upp radiosignaler och andra störningar och tro att det är signaler från sensor aktiverar vi det s.k. pull-up-motståndet som garnaterar att ingången alltid har en giltig insignal.
void setup() {
pinMode(MOTOR_1_SPEED_PIN, OUTPUT);
pinMode(MOTOR_2_SPEED_PIN, OUTPUT);
pinMode(MOTOR_1_DIR_PIN, OUTPUT);
pinMode(MOTOR_2_DIR_PIN, OUTPUT);
pinMode(LEFT_BUMPER_PIN, INPUT_PULLUP);
pinMode(CENTER_BUMPER_PIN, INPUT_PULLUP);
pinMode(RIGHT_BUMPER_PIN, INPUT_PULLUP);
}I huvud-loopen dvs funktionen loop() läser vi av stöt-sensorn om och om igen. Så länge sensorn inte är aktiverad så kör vi framåt, men så fort sensorn aktiveras så backar vi och vänder runt för att undvika hinder. Vilket håll vi vänder på beror på vilken del av sensorn som stött emot hinder. Om hindret är på vänster sida så svänger vi åt höger
if (digitalRead(LEFT_BUMPER_PIN) == LOW) {
// Backa lite och snurra åt höger
backward()
delay(BACKUP_TIME);
turn_right();
delay(TURN_TIME);
}Annars om hindret är på höger sida vänder vi åt vänster
else if (digitalRead(RIGHT_BUMPER_PIN) == LOW) {
// Backa lite och snurra åt vänster
backward()
delay(BACKUP_TIME);
turn_left();
delay(TUNR_TIME);
}Och om hindret är rakt fram så vänder vi höger eller vänster slumpmässigt. Vi väljer håll genom att skapa ett slumptal mellan 0 och 100. Om talet är mindre än 50 så vänder vi åt höger, annars vänder vi åt vänster. Detta ger oss 50% chans att svänga vänster och 50% att svänga höger.
else if (digitalRead(CENTER_BUMPER_PIN) == LOW) {
// Backa lite och snurra runt slumpmässigt åt höger eller vänster
backward();
delay(BACKUP_TIME);
if (random(100) < 50) {
turn_right();
}
else {
turn_left();
}
delay(TURN_TIME);
}Om sensorn inte aktiverats så kör vi framåt
else {
forward();
}