Zoidbot TG – Fjärrstyrning med Bluetooth

Denna kod låter dig fjärrstyra din robot med en mobiltelefon (Android) via bluetooth.

Beskrivning av koden

Importera bibliotek

Motorerna på roboten är så kallade stegmotorer. Normala motorer roterar när de får ström, men stegmotorer behöver strömpulser och rör sig bara ett litet steg vid varje puls. Fördelen med detta är att motorerna går att styra mycket exakt. Motorerna på Zoidbot TG har 400 steg per varv så man behöver skicka 400 pulser för att få dem att rotera ett helt varv. För att slippa skriva kod som skickar pulser i rätt hastighet och som accelererar och decelererar motorerna mjukt så använder vi oss av ett kod-bibliotek som gör detta åt oss. Biblioteket heter ”ContinuousStepper” och behöver installeras innan det kan användas. Biblioteket installeras enkelt med ett par klick genom bibliotek-hanteraren i Arduinos utvecklingsmiljö (Arduino IDE).

#include <ContinuousStepper.h>

Definiera konstanta värden för anslutningar

För att använda de digitala anslutningarna på Arduino-kortet anger man numret för dem. Siffror är dock inte särskilt beskrivande och om man använder en anslutning på många ställen i koden och behöver ändra dem måste man gå igenom hela koden och ändra siffran på alla ställen. Vi skapar därför istället konstanta värden som vi ger läsbara och beskrivande namn. Vi kan sedan använda dessa namn i vår kod istället för siffrorna.

#define LEFT_MOTOR_STEP_PIN 2
#define LEFT_MOTOR_DIR_PIN 1
#define LEFT_MOTOR_ENABLE_PIN 0
#define RIGHT_MOTOR_STEP_PIN 5
#define RIGHT_MOTOR_DIR_PIN 4
#define RIGHT_MOTOR_ENABLE_PIN 3

Definiera konstanta värden för hastighet etc

Max hastighet och accelaration för roboten beror på motorerna, drivspänning, vikt och många andra parameterar. För att lätt kunna ändra dessa värden på ett enda ställe så skapar vi konstanter i början av koden. Värdena är steg per sekund för hastighet och steg per kvadrat-sekund för accelerationen. Högre värden ökar hastighet/accelaration, men minskar motorernas kraft.

#define ACCELERATION 800
#define FORWARD_SPEED 600
#define BACK_SPEED 600
#define TURN_SPEED 600

Skapa variabler för motorerna

Kod-biblioteket som styr motorerna använder ett så kallat objekt-orienterat gränssnitt. Vad detta betyder behöver du inte veta för att använda det, men det innebär att vi måste skapa två variabler (objekt) som representerar motorerna. Nedan ser du hur man gör det. Efter detta så har vi två variabler left_motor och right_motor som vi kan använda för att köra motorerna.

ContinuousStepper<StepperDriver> left_motor;
ContinuousStepper<StepperDriver> right_motor;

Funktion för att köra roboten

Vi skapar fyra funktioner för att köra roboten framåt, bakåt, höger och vänster. De fyra funktionerna gör i princip samma sak. Först aktiveras motorerna; om de redan är aktiverade så händer inget. Sedan sätter vi roteringshastigheten på de två motorerna. Positiva tal betyder rotering framåt och negativa betyder rotering bakåt. För att svänga så kör vi framåt med ena hjulet och bakåt med det andra. Om du tycker att roboten svänger för snävt eller för snabbt så kan du ändra så att ena motorn står stilla istället för att köra bakåt. Det gör du genom att använda 0 istället för TURN_SPEED. Självklart kan vilken siffra som helst användas här, men för att slippa ändra på många ställen så skapade vi ju våra konstanta värden överst i koden. Så om du vill ändra hastigheten på roboten gör du det där, och inte i dessa funktioner. Sist så har vi funktionen stop() som, föga förvånande, stannar motorerna.

void forward() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(FORWARD_SPEED);
  right_motor.spin(FORWARD_SPEED);
}

void backward() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(-BACK_SPEED);
  right_motor.spin(-BACK_SPEED);
}

void turn_left() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(-TURN_SPEED);
  right_motor.spin(TURN_SPEED);
}

void turn_right() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(TURN_SPEED);
  right_motor.spin(-TURN_SPEED);
}

void stop() {
  left_motor.stop();
  right_motor.stop();
}

Setup

Setup-funktionen är speciell. Denna måste alltid finnas i ett Arduino-projekt och denna funktion körs en gång vid uppstart. Det är alltså den absolut första koden som körs och det är här som man ställer in inställningar och gör saker som bara behöver göras en gång vid start. Det första vi gör är att ställa in motor-variablerna. Först måste vi berätta för dem vilka anslutningar på kortet som motor-modulerna är inkopplade till via funktionen begin(). Dessa anslutningar har vi ju ställt in som konstanta värden först i koden så det är dessa namn vi använder istället för siffror direkt. De anslutningar vi behöver är steg (STEP) och riktning (DIR). Vi behöver ange anslutning för aktivering av motorerna (ENABLE) också, men pga hur biblioteket är gjort anger man denna separat med funktionen setEnablePin().

Därefter ställer vi in accelerationen vi vill använda och därefter ser vi till att motorerna är avstängda. Vi vill inte att mororerna ska vara igång om vi inte kör med roboten. Man kan tycka att om motorernas hastighet är noll så är motorerna avstängda, men stegmotorer är lite speciella. Eftersom de inte snurrar kontinuerligt, utan går bara ett steg i taget så om man aktiverar dem så kommer de att låsa sig fast i den positionen de står i för tillfället tills man ger dem en ny puls. De drar därmed ström även när de står stilla. Men via aktiveringsanslutningen (enable) så kan vi stänga av strömmen helt. Motorerna släpper då taget och drar ingen ström. Det innebär dock att om roboten stannar i en backe så kommer den förmodligen börja rulla, ungefär som om man skulle släppa handbromsen i en vanlig bil.

Det sista vi gör i setup är att aktivera kortets två seriakommunikations-kanaler. Dessa används för att skicka och ta emot data. Siffran 9600 är hastigheten i tecken per sekund. Denna måste matcha hastigheten som den andra enheten som vi pratar med använder. Den kan dessutom inte sättas fritt utan det finns en rad specifika hastigheter som är tillåtna. Vi använder 9600 här för att det är en vanlig hastighet och vad både datorn och bluetooth-modulen använder som standard. Serial är samma sak som USB-kontakten och allt som skickas ut på denna kommer att tas emot av datorn. För att se datan som skickas aktiverar ni serial monitor i Arduino IDE. Se till att kommunikationshastigheten är satt till 9600 baud så den matchar vad vi ställer in roboten att använda. Serial1 är kopplad till anslutningar Rx och Tx på Arduino-kortet och dessa kopplar vi vidare till bluetooth-modulen. Det är genom denna som vi tar emot kommandon från mobiltelefonen via bluetooth.

void setup() {
  left_motor.begin(LEFT_MOTOR_STEP_PIN, LEFT_MOTOR_DIR_PIN);
  right_motor.begin(RIGHT_MOTOR_STEP_PIN, RIGHT_MOTOR_DIR_PIN);
  left_motor.setEnablePin(LEFT_MOTOR_ENABLE_PIN, LOW);
  right_motor.setEnablePin(RIGHT_MOTOR_ENABLE_PIN, LOW);
  left_motor.setAcceleration(ACCELERATION);
  right_motor.setAcceleration(ACCELERATION);
  left_motor.powerOff();
  right_motor.powerOff();
  Serial.begin(9600);
  Serial1.begin(9600);
}

Loop

Ta emot kommando

Efter att setup() har körts så kommer funktionen loop() att köras. Som namnet säger så kommer denna att köras om och om igen så länge Arduino-kortet är igång eller du trycker på reset-knappen.

Först så kollar vi om vi tagit emot någon data från mobiltelefonen via Bluetooth med Serial.available(). Om det finns data så läser vi in ett tecken. Vi kollar sedan vad detta tecknet är med switch/case. Detta är som en lång rad if-satser där vi väljer en kod-snutt av många beroende på värdet på en variabel, i detta fall ch som innehåller tecknet vi fick från bluetooth-modulen. Android-appen skickar ett ”F” om framåtknappn tryckts, ”G” för backåt, ”L” för vänster, ”R” för höger och ”S” för stop. För varje kommando så kör vi motsvarande funktion som vi skapade tidigare i koden. Vi skriver även ut ett meddelande till USB-porten så att vi kan se på datorn vad som händer. Break måste anges sist i varje kodsnutt för att berätta för datorn var koden slutar. Det är lätt att glömma, men gör du det så kommer du få konstiga buggar där roboten inte gör vad du förväntar dig för de olika kommandona. Om vi tar emot något annat än de kommandon vi känner igen så körs default:och här har vi ingen kod vilket innebär att vi helt enkelt ignorera allt som inte är kända kommandon.

void loop() {
  if (Serial1.available() > 0) {
    int ch = Serial1.read();
    switch(ch) {
      case 'F':
        forward();
        Serial.println("Forward");
        break;
      case 'G':
        backward();
        Serial.println("Backward");
        break;
      case 'L':
        turn_left();
        Serial.println("Rotate left");
        break;
      case 'R':
        turn_right();
        Serial.println("Rotate right");
        break;
      case 'S':
        stop();
        Serial.println("Stop");
        break;
      default:
        break;
    }
  }

Driv motorerna

För att skicka pulser till motrorerna så måste vi anropa motorernas loop()-funktion. Detta är inte samma loop() som huvud-programmet utan en del av motor-biblioteket som råkar ha samma namn. Denna funktion måste anropas så ofta och snabbt som möjligt för att den ska kunna skicka pulser i rätt hastighet. Det är därför väldigt viktigt att man inte lägger in några pauser eller kod som tar lång tid att kör i programmet.

Det sista vi gör är att kolla om motorerna fortfarande snurrar. Om de inte gör det så stänger vi av strömmen till dem för att spara batterierna. Man skulle kunna tänka sig att stänga av motorerna i stop()-funktionen eftersom den innebär att vi ska stanna. Men om roboten har fått upp hastigheten så har den en hel del kinetisk energi och om vi stänger av motorerna direkt så kommer den att fortsätta rulla tills friktionen och luftmotståndet får den att stanna och det är inte riktigt vad vi vill. Så när stop() anropas kommer motorer-biblioteket att fortsätta skicka pulser och bromsa in på ett kontrollerat sätt. Vi måste därför vänta tills motorernas hastighet verkligen är noll innan vi stänger av dem.

När motorerna stängs av så rullar hjulen fritt. Om roboten stannar i en backa så finns risken att den kommer att rulla ner av sig självt. Om detta är ett problem så kan du ta bort kodraderna som stänger av motorerna. Roboten kommer då att hålla kvar strömmen och låsa hjulen även när den står stilla. Nackdelen är så klart att batterierna kommer att tömmas även när den står still.

  left_motor.loop();
  right_motor.loop();

  if (! left_motor.isSpinning() && ! right_motor.isSpinning()) {
    left_motor.powerOff();
    right_motor.powerOff();
  }
}

Hela koden

#include <ContinuousStepper.h>

#define LEFT_MOTOR_STEP_PIN 2
#define LEFT_MOTOR_DIR_PIN 1
#define LEFT_MOTOR_ENABLE_PIN 0
#define RIGHT_MOTOR_STEP_PIN 5
#define RIGHT_MOTOR_DIR_PIN 4
#define RIGHT_MOTOR_ENABLE_PIN 3

#define ACCELERATION 800
#define FORWARD_SPEED 600
#define BACK_SPEED 600
#define TURN_SPEED 600


ContinuousStepper<StepperDriver> left_motor;
ContinuousStepper<StepperDriver> right_motor;


void forward() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(FORWARD_SPEED);
  right_motor.spin(FORWARD_SPEED);
}

void backward() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(-BACK_SPEED);
  right_motor.spin(-BACK_SPEED);
}

void turn_left() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(-TURN_SPEED);
  right_motor.spin(TURN_SPEED);
}

void turn_right() {
  left_motor.powerOn();
  right_motor.powerOn();
  left_motor.spin(TURN_SPEED);
  right_motor.spin(-TURN_SPEED);
}

void stop() {
  left_motor.stop();
  right_motor.stop();
}


void setup() {
  left_motor.begin(LEFT_MOTOR_STEP_PIN, LEFT_MOTOR_DIR_PIN);
  right_motor.begin(RIGHT_MOTOR_STEP_PIN, RIGHT_MOTOR_DIR_PIN);
  left_motor.setEnablePin(LEFT_MOTOR_ENABLE_PIN, LOW);
  right_motor.setEnablePin(RIGHT_MOTOR_ENABLE_PIN, LOW);
  left_motor.setAcceleration(ACCELERATION);
  right_motor.setAcceleration(ACCELERATION);
  left_motor.powerOff();
  right_motor.powerOff();
  Serial.begin(9600);
  Serial1.begin(9600);
}

void loop() {
  if (Serial1.available() > 0) {
    int ch = Serial1.read();
    switch(ch) {
      case 'F':
        forward();
        Serial.println("Forward");
        break;
      case 'G':
        backward();
        Serial.println("Backward");
        break;
      case 'L':
        turn_left();
        Serial.println("Rotate left");
        break;
      case 'R':
        turn_right();
        Serial.println("Rotate right");
        break;
      case 'S':
        stop();
        Serial.println("Stop");
        break;
      default:
        break;
    }
  }

  left_motor.loop();
  right_motor.loop();

  if (! left_motor.isSpinning() && ! right_motor.isSpinning()) {
    left_motor.powerOff();
    right_motor.powerOff();
  }
}

Android app

Programmet förväntar sig att få kommandon som enskilda tecken. Det finns flera Android-appar som fungerar med Bluetooth-modulen som vi använder och som skickar kommandon på detta sätt. Den som koden är testad med är ”Arduino Car”. Dessvärre är denna, och i princip alla andra liknande appar, reklamfinansierade. Om reklamen känns irriterande kan man stänga av datatrafik för appen, eller för hela telefonen innan man startar appen.