Verander elke DC-motor in een custom 360° servo – Stapsgewijze handleiding

In deze tutorial leren we hoe je elke DC-motor kunt omzetten in een zelfstandige custom servomotor met veel functies. In tegenstelling tot gewone servo's die een beperkte beweging van 180 of 270 graden hebben, heeft deze een onbeperkt bereik van 360 graden, en daarbovenop hebben we de mogelijkheid om het bereik van de rotatie aan te passen naar elke waarde die we nodig hebben.

Dat is best handig, vind ik, en bovenop kunnen we zelfs het middenpunt van de servo aanpassen. Dus we kunnen zowel het middenpunt als het bereik van de rotatie tegelijkertijd aanpassen.

Je kunt de volgende video bekijken of de geschreven tutorial hieronder lezen.

Een andere functie is dat we de gevoeligheid of hoe snel de servo reageert op onze invoer kunnen aanpassen. Wat de invoer betreft, kunnen we drie verschillende invoermodi hebben.

We kunnen de servo besturen met een analoog spanning-ingang of met een potentiometer, we kunnen de servo besturen met een RC-zender, evenals de servo besturen via een seriële poort door hoekwaarden in te voeren via de seriële monitor op onze PC of laptop.

We kunnen dat ook tegelijkertijd doen, de servo besturen door waarden in te voeren via de seriële monitor en handmatig de servo bewegen met de RC-zender. De servomotor weet te allen tijde zijn huidige positie en kan die op de seriële monitor zien.

Bovenop deze lijst met servomotorfuncties, is de continue rotatiemodus. Dat klopt. We kunnen de servomotorpositie besturen en bijhouden zelfs in deze continue rotatiemodus. We kunnen de servomotoras instellen om naar elke positie te gaan met een oneindig aantal omwentelingen.

Al dit is mogelijk dankzij de 12-bit encoder die deze servomotor gebruikt, de AS5600 magnetische draaiende positie-sensor en de geïmplementeerde PID-regeling voor het aansturen van de DC-motor.

Ik maakte deze custom servomotorcontrollerbord die zijn eigen microcontroller en alles bevat om elke DC-motor makkelijk om te zetten in een zelfstandige servomotor.

We hoeven het bord alleen in het midden van de uitvoeras te positioneren (inclusief een specifieke magneet op de as), elke DC-motor tot 3,5A nominal stroom aansluiten, het hele systeem voeden met 12V en dat is het, we krijgen een servomotor van een gewone DC-motor met al deze functies.

Nu zal ik je door het hele proces van het maken van deze zelfgebouwde servomotor leiden, zodat jij er ook een kunt maken. Ik zal het werkingsprincipe van een servomotor uitleggen, een gesloten-lus regelaar, een PID-regelaar, hoe ik de custom PCB ervoor ontwierp en tandwielkast, alsook de code erachter uitleggen.

Werkingsprincipe servomotor

Om het werkingsprincipe van een servomotor uit te leggen, laten we een typische RC-servomotor uit elkaar halen en kijken wat erin zit.

We merken dat het een kleine DC-motor heeft, een controllerbord, een potentiometer en een drie-draden verbinding, twee draden voor de voeding en één voor het invoersignaal. Ook zijn er enkele tandwielen voor het verminderen van de snelheid en het verhogen van het koppel van de DC-motor.

Dit is een typische opstelling voor de meeste RC- of hobby-servomotoren. De potentiometer zit op de uitvoeras van de DC-motor, en fungeert als positie-sensor, het vertelt de controller de huidige positie van de servomotoras. Het controllerbord regelt de DC-motor op basis van het invoersignaal (de gewenste positie) en de werkelijke positie die we als terugkoppeling krijgen van de potentiometer. Dit vertegenwoordigt een gesloten-lus regelsysteem.

Het invoersignaal, of de gewenste positie, wordt vergeleken met de werkelijke positie van de motor die we krijgen van de positie-terugkoppelingssensor. Het verschil dat ontstaat, de fout genoemd, wordt dan verwerkt in de controller die de motor opdraagt te bewegen tot hij die gewenste positie bereikt.

Hoe een custom servomotor te maken

Dus, als we onze eigen servomotor willen bouwen met grotere DC-motoren dan die typische RC-servo's gebruiken, kunnen we hetzelfde gesloten-lus regelsysteem implementeren.

We hebben gewoon een positie-sensor nodig die op een of andere manier aan de uitvoeras zit, en een microcontroller voor het aansturen van de DC-motor.

Nu wat de positie-sensor betreft, is de simpelste oplossing om een simpele potentiometer te gebruiken, net zoals die we in de RC-servo's zagen. Het probleem met deze typen potentiometers is dat ze een beperkt bereik van rotatie van slechts 270 graden hebben, wat direct het bereik van de servomotor beperkt. Er zijn ook andere typen potentiometers die meerdere omwentelingen kunnen maken en beter bereik en resolutie bieden, maar nog steeds hebben ze beperkte rotatie.

Als we de servomotor onbeperkte rotatie willen laten hebben, dan moeten we een encoder gebruiken. Encoders zijn elektromechanische apparaten die de hoekpositie van de as met onbeperkte rotatie kunnen bijhouden. Er zijn veel typen encoders, zoals incrementele of absolute, of afhankelijk van hun sensortechnologie optisch, magnetisch of capacitief. Natuurlijk heeft elk zijn eigen voor- en nadelen.

AS5600 Encoder – Magnetische draaiende positie-sensor

Ik koos voor een magnetische encoder, of de AS5600 magnetische draaiende positie-sensor, omdat het een heel compacte en makkelijk te implementeren encoder is die hoge precisie of resolutie biedt. Kijk eens hoe klein deze microchip is.

Het heeft een ingebouwde Hall-effect sensor die veranderingen in de richting van het magnetische veld kan detecteren. Dus we hoeven alleen een magneet aan de uitvoeras van de motor te bevestigen en het dicht bij de microchip te positioneren op een afstand van 0,5 tot 3mm.

Nu, zodra de motoras en de magneet gaan draaien, zal de Hall-effect sensor die veranderingen in de magnetische veldrichting vastleggen. Met behulp van de ingebouwde 12-bit A/D-converter, kan de AS5600-sensor 4096 posities per omwenteling of 360 graden rotatie uitvoeren.

Dat betekent dat het veranderingen in de hoekpositie van maar 0,0878 graden kan detecteren. Dat is indrukwekkend, en met het feit dat het heel betaalbaar en makkelijk te verkrijgen is, is het de juiste keuze voor een zelfgebouwde servomotor.

Oké, wat hebben we nog nodig? Een microcontroller en een driver voor de DC-motor. Ik koos de DRV8871 DC-motordriver die tot 3,5 ampère stroom kan aan, en de Atmega328-microcontroller.

Ik koos de surface-mount versie ervan, omdat die veel compacter is dan de DIP-versie, en mijn doel was om zo klein mogelijke custom PCB te maken waarop ik alles kan onderbrengen, zodat de servo als zelfstandig apparaat kan werken.

Schakelschema custom servomotor

Hier is het complete schakelschema van deze zelfgebouwde servomotor.

Je kunt de benodigde componenten voor dit project via de onderstaande links verkrijgen:

• AS5600 magnetische encoder ………………….. Amazon / AliExpress
• DRV8871 DC-motordriver ……………….…. Amazon / AliExpress
• Atmega328p-AU …………………………………. Amazon / AliExpress
• 16Mhz kristaloscillator …………………….. Amazon / AliExpress
• AMS1117 5V spanningsregelaar ………..…. Amazon / AliExpress
• 3386P vierkante potentiometer ………………. Amazon / AliExpress
• Condensatoren 0805 kit …………………………….…. Amazon / AliExpress
• 12V DC-motor – ~ 50RPM …………….……. Amazon / AliExpress

Disclaimer: Dit zijn affiliatelinks. Als Amazon Associate verdien ik aan kwalificatieve aankopen.

Dus, we hebben de Atmega328-microcontroller samen met het aanbevolen bare minimum circuit, wat een 16Mhz oscillator, een paar condensatoren en een weerstand omvat.

Voor het voeden van de microcontroller en de andere componenten die 5V vereisen, gebruiken we de AMS1117-spanningsregelaar, die de 12V voeding naar 5V brengt.

Hier is de AS5600-positie-sensor met zijn aanbevolen circuit, wat twee condensatoren en twee pull-up weerstanden voor de I2C-communicatie omvat.

De DRV8871 DC-motordriver heeft slechts één weerstand voor stroombegrenzing, en twee decoupling-condensatoren. Dan hebben we twee potentiometers aangesloten op de analoge ingangen van de microcontroller, één voor het instellen van het rotatiebereik en de ander voor het instellen van de gevoeligheid van de servo. De drukknop wordt gebruikt voor het instellen van het middenpunt van de servo, en de twee-weg DIP-schakelaar voor het selecteren van de werkmodi van de servo. Er is een pinheader voor de invoeren van de servo, ofwel analoog spanning-ingang of een digitaal PWM-signaal van een RC-ontvanger, samen met een 5V- en een massapin. Er is ook een pinheader voor het programmeren van de microcontroller via het SPI-protocol en de seriële poort.

Hier is een samengevat overzicht van dit circuit en zijn werkstroom. De invoer, of de gewenste hoekpositie, wordt via deze twee pinnen ontvangen en kan ofwel een analoog spanning zijn van een potentiometer, of een digitaal PWM-signaal van een RC-ontvanger. De invoer gaat in de microcontroller waar het vergeleken wordt met de werkelijke hoekpositie die gedetecteerd wordt door de encoder, of de AS5600-positie-sensor. Deze sensor communiceert met de microcontroller via het I2C-protocol.

Dan doet de microcontroller de wiskunde, het berekent de fout en stuurt ervolgens een PWM-signaal naar de DRV8871-driver die de DC-motor aandrijft tot hij de gewenste positie bereikt.

Het hele circuit wordt gevoed door 12V, en de AS1117-spanningsregelaar levert 5V voor de microcontroller en de andere componenten op de juiste manier.

PCB-ontwerp

Volgens het schakelschema probeerde ik de PCB zo klein mogelijk te ontwerpen, en het kwam uit op 40x40mm.

Ik positioneerde de encoder aan de onderkant en precies in het middelpunt van de PCB, zodat het gemakkelijk gemonteerd en uitgelijnd kan worden met de uitvoeras van de servo.

Alle andere componenten zitten aan de andere kant, zodat ze niet interfereren met de encoder en de uitvoeras.

Ik bestelde de PCB bij PCBWay. Hier kunnen we simpelweg het Gerber-bestand uploaden, de eigenschappen van onze PCB kiezen, en het tegen een redelijke prijs bestellen.

Ik ontwierp de PCB met 4 lagen, de middelste zijn voor GND, wat de prijs iets verhoogt. Ik veranderde geen van de standaardeigenschappen, behalve de PCB-kleur die ik wit koos, en ik vinkte aan dat ik akkoord ga met wijziging van de oppervlakteafwerking naar Immersion gold als van toepassing zonder extra kosten.

Je kunt het Gerber-bestand vinden en downloaden via de PCBWay project-deelcommunity, waar je de PCB ook direct kunt bestellen.

Toch, na enkele dagen kwam de PCB aan. De kwaliteit van de PCB is geweldig, alles is precies hetzelfde als in het ontwerp, en ik kreeg het in immersion gold oppervlakteafwerking.

Oké, nu kunnen we doorgaan met het solderen van de componenten erop. Ik begon met de kleinere componenten zoals deze indicator LED, en de condensatoren en weerstanden.

Dit is eigenlijk mijn eerste keer dat ik dergelijke kleine SMD-componenten soldeer, en ik was er echt, echt slecht in.

Het meest uitdagend was het solderen van de Atmega328-microcontroller, want de pootjes zijn echt heel klein en heel dicht bij elkaar, maar ik wist het ergens te krijgen.

De AS5600-encoder microchip was makkelijk te solderen aan de achterkant van de PCB, evenals de grotere, doorboorde componenten, zoals de DIP-schakelaar, de potentiometers, de klemblokken en de pinheaders.

Oké, hier is de eindelijke uitstraling van het controllerbord, wat na alles, best netjes uitkwam, vind ik.

Nu is het tijd om een geschikte tandwielkast te maken voor de DC-motor en dit controllerbord.

Custom servo 3D-model

Ik ontwierp de tandwielkast voor deze custom servomotor met Onshape. Het ontwerp van de tandwielkast hangt af van de DC-motor natuurlijk. Zoals gezegd, kunnen we elke grootte DC-motor in combinatie met het controllerbord dat we net maakten gebruiken.

Hier gebruik ik een DC-motor met 37mm diameter en een ingebouwde tandwielkast die 50 RPM levert. 50 RPM is een goede snelheid voor een servomotor, maar ik wilde iets lager gaan, om beter koppel te krijgen, dus ik maakte een tandwielkast met 3 keer reductie. Ik gebruikte haarlinttandwielen (herringbone gears) daarvoor, want die zijn efficiënt en makkelijk te maken met een 3D-printer.

Natuurlijk hebben we hier de vrijheid om dit tandwielkastontwerp hoe we willen te maken, want het hangt af van de DC-motor die we willen gebruiken en welke uitgaande snelheden we willen krijgen.

Ik positioneerde het controllerbord aan de achterkant van deze tandwielkast en lijnde het perfect uit in het midden van de uitvoeras.

Als we de DC-motoras direct als uitgang willen gebruiken, kunnen we gewoon een 1:1 tandwielset gebruiken, zodat we de positie van de as correct kunnen bijhouden. Of we kunnen ook een riemensysteem gebruiken in zo'n geval. Zoals gezegd, we hebben oneindige mogelijkheden voor het maken van de tandwielkast.

Download 3D-model en STL-bestanden

Je kunt het 3D-model van deze zelfgebouwde servomotor direct in je webbrowser bekijken en verkennen met Onshape. (Je hebt een Onshape-account nodig, je kunt gratis een account aanmaken voor thuisgebruik)

Natuurlijk kun je ook het 3D-model downloaden, evenals de STL-bestanden voor 3D-printen van de onderdelen hier:

STEP-bestand:

STL-bestanden voor 3D-printen:

Monteren van de custom servo

Hier zijn de 3D-geprinte onderdelen voor deze build, dus we kunnen beginnen met het monteren van de servomotor.

Samen met ze hebben we enkele M3-bouten en gezwinde-inserts, en enkele lagers nodig.

Eerst bevestigde ik de DC-motor aan de basisplaat met enkele M3-bouten van 8mm lengte.

Dan kunnen we de twee tandwielen op hun plaats installeren. Het kleinere tandwiel gaat direct op de DC-motoras, en het grotere tandwiel zal de uitgang van de servo zijn. Hoewel de uitvoeras uit twee delen bestaat.

Ik installeerde gezwinde-inserts aan beide kanten van dit uitvoerasdeel, aan de ene kant voor het verbinden van het tandwiel ermee, en aan de andere kant voor het bevestigen van dingen aan de uitgang van de servo.

Ik installeerde ook gezwinde-inserts in het kleinere tandwiel, die gebruikt zullen worden voor het bevestigen ervan aan de DC-motoras. Nu kunnen we de gepaarde tandwielen in hun positie schuiven. Omdat dit haarlinttandwielen zijn, moeten we ze allebei tegelijkertijd in plaats schuiven, anders kunnen we ze niet paren als we ze er één voor één in duwen.

Met een zeskantschroef bevestigde ik het kleine tandwiel aan de DC-motoras. Ik gaf 12V op de DC-motor om te checken of de tandwielset goed werkt.

We kunnen de tandwielkast-montage afronden door het zijpaneel, het kogellager voor de uitvoeras en de bovenste deksel te plaatsen.

Ik installeerde enkele M3-gezwinde-inserts op de achterplaat, zodat we de hele montage met enkele 20mm lange M3-bouten kunnen bevestigen. Ik testte de tandwielkast opnieuw, het werkt geweldig. We kunnen zien hoe de uitvoeras aan de achterkant draait en hier moeten we de magneet installeren die de AS5600-encoder zal bijhouden.

We bevestigen het controllerbord aan de tandwielkast met enkele M2-bouten en moeren. De AS5600-positie-sensor is nu perfect uitgelijnd met de magneet, en dus wanneer de uitvoeras zal draaien, zal het de verandering in het magnetische veld correct meten.

Let hier op dat de magnetiseringsrichting van de permanente magneet heel belangrijk is. Afhankelijk van of hij assiaal of diametraal gemagnetiseerd is, moeten we de magneet ofwel loodrecht of parallel aan die AS5600 IC positioneren.

Ik moest de richting van mijn magneet veranderen, omdat hij niet de juiste magnetisatie had, zodat de AS5600-encoder hem kon meten.

Vervolgens soldeerde ik twee draden aan de DC-motor en verbond de motor met de controller via de klemblok. Wat de voeding betreft, verbond ik twee draden aan de voedingsklemblok, die aan de andere kant een DC-voedingsconnector hebben voor het aansluiten van een 12V-voeding. En dat is het, onze zelfgebouwde servomotor is klaar.

Programmeren van de controller

Wat er nog over is, is om deze servo tot leven te wekken, of de controller te programmeren. Daartoe moeten we eerst een bootloader branden in de ATmega328p-microcontroller. Zonder een bootloader kan de microcontroller de taal of de code die we ernaar sturen niet begrijpen.

Bootloader branden

Om de bootloader in de ATmega328p te branden, hebben we een Arduino-bord nodig, in mijn geval gebruik ik een Arduino Nano-bord.

We zullen de SPI-communicatie gebruiken, dus we moeten de geschikte SPI-pinnen op het Arduino-bord en ons controllerbord verbinden.

Nu, met de Arduino IDE, moeten we het ArduinoISP-voorbeeldsketch openen en uploaden naar het Arduino Nano-bord. Met deze code is de Arduino Nu in staat om de bootloader in de ATmega328-microcontroller te branden.

Vervolgens, in het Tools-menu, als Programmer moeten we Arduino as ISP selecteren en dan op Burn Bootloader klikken.

Tijdens het branden van de bootloader, moeten we merken dat de Arduino NANO-lichtjes veel knipperen, en dat resulteert in een succesvolle bootloader-branding.

Code uploaden

Zodra we daarmee klaar zijn, nu kunnen we de code programmeren of uploaden naar het controllerbord met behulp van een USB naar UART-interface module.

Het controllerbord heeft speciale pinnen voor het gemakkelijk verbinden ervan, net zoals getoond in dit schakelschema.

Nu kunnen we de code voor deze zelfgebouwde servo die ik maakte openen en uploaden naar de controller. Voordat we dat doen, moeten we eerst de bibliotheken voor de AS5600-sensor en de PID-regeling installeren. Dat kunnen we gemakkelijk doen via de Arduino IDE Library Manager. Zodra we op de upload-knop drukken, wordt de code naar onze ATmega328-controller geschreven via de USB-naar-UART-interface-module.

En dat is het, onze zelfgebouwde servomotor is klaar. Nu kunnen we een potentiometer aan verbinden om het te testen. Even een opmerking dat de analoge invoer naar de "S"-pin op het controllerbord gaat, in plaats van de "A"-pin.

Bij het ontwerpen van de PCB had ik deze twee pinnen verkeerd aan de ATmega328 aangesloten. Dan kunnen we de analoge invoermodus selecteren via de DIP-schakelaar en de servo voeden.

En hier is het, we kunnen de servomotorpositie besturen met behulp van de analoge invoer van de potentiometer. We hebben onze DC-motor met succes omgezet in een servomotor.

Broncode

Nu kijken we naar de code van deze zelfgebouwde servomotor.

/* 
* Zelfgebouwde Servomotor - Arduino-code
* door Dejan, www.HowToMechatronics.com
* 
* Bibliotheken:
* AS5600 encoder: https://github.com/RobTillaart/AS5600
* PID-regelaar: https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h 
*/
#include "AS5600.h"
#include "Wire.h"
#include 
AS5600 as5600; // standaard Wire
double Pk1 = 2; // snelheid waarmee hij er komt
double Ik1 = 0;
double Dk1 = 0.025;
// Variabelen definiëren die we zullen koppelen
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, Pk1, Ik1, Dk1, DIRECT);
#define motor_IN1 5
#define motor_IN2 6
#define ch1 2
#define centerSet 7
#define inputSwitch 3
#define modeSwitch 4
int ch1Value;
int encoderValue, inputValue, pwmValue;
String inString = ""; // string om invoer te houden
int centerAngle = 2047; // 180 graden
int angleDifference = 0;
int angleValue = 0;
int leftLimit = 30;
int rightLimit = 4067;
int rangeAdjustment = 0;
float sensitivityAdjustment = 0;
float angle = 0;
int quadrantNumber = 2;
int previousQuadrantNumber = 3;
int numberOfTurns = 0;
float totalAngle = 0;
int error = 0;
char incomingByte = 0;
int intInput = 0;
void setup() {
  Serial.begin(115200);
  Serial.println(__FILE__);
  Serial.print("AS5600_LIB_VERSION: ");
  Serial.println(AS5600_LIB_VERSION);
  Wire.begin();
  pinMode(motor_IN1, OUTPUT);
  pinMode(motor_IN2, OUTPUT);
  // Activeer Arduino interne pull-up weerstanden
  pinMode(centerSet, INPUT_PULLUP);
  pinMode(inputSwitch, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);
  myPID.SetMode(AUTOMATIC);
  // PID-instelling
  myPID.SetOutputLimits(-255, 255);
  myPID.SetSampleTime(20);
}
void loop() {
  // Lees encoder-waarde - huidige positie
  encoderValue = as5600.readAngle();
  // Continue rotatiemodus
  if (digitalRead(modeSwitch) == 0) {
    // Voer gewenste hoek in voor servo om naar toe te gaan via seriële monitor
    while (Serial.available() > 0) {
      int inChar = Serial.read();
      if (isDigit(inChar)) {
        // Converteer binnenkomende byte naar char en voeg toe aan string:
        inString += (char)inChar;
      }
      // Als je een newline krijgt, print de string, dan de waarde van de string:
      if (inChar == '\n') {
        Setpoint = inString.toInt(); // Setpoint - gewenste hoek
        // Wis string voor nieuwe invoer:
        inString = "";
      }
    }
    if (digitalRead(inputSwitch) == 0) {
      // Potentiometer als invoer
      inputValue = analogRead(A0);
      if (inputValue < 400) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue < 300) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue < 200) {
        Setpoint = Setpoint - 0.3;
      }
      if (inputValue > 600) {
        Setpoint = Setpoint + 0.3;
      }
      if (inputValue > 700) {
        Setpoint = Setpoint + 0.3;
      }
      if (inputValue > 800) {
        Setpoint = Setpoint + 0.3;
      }
    } else if (digitalRead(inputSwitch) == 1) {
      inputValue = pulseIn(ch1, HIGH, 30000); // RC-ontvanger als invoer
      if (inputValue < 1450) {
        Setpoint--;
      }
      if (inputValue < 1350) {
        Setpoint--;
      }
      if (inputValue < 1250) {
        Setpoint--;
      }
      if (inputValue < 1150) {
        Setpoint--;
      }
      if (inputValue > 1550) {
        Setpoint++;
      }
      if (inputValue > 1650) {
        Setpoint++;
      }
      if (inputValue > 1750) {
        Setpoint++;
      }
      if (inputValue > 1850) {
        Setpoint++;
      }
    }
    // Converteer encoder RAW-waarden naar hoekwaarde
    angle = encoderValue * 0.087890625;
    // Kwadrant 1
    if (angle >= 0 && angle <= 90) {
      quadrantNumber = 1;
    }
    // Kwadrant 2
    if (angle >= 90 && angle <= 180) {
      quadrantNumber = 2;
    }
    // Kwadrant 3
    if (angle >= 180 && angle <= 270) {
      quadrantNumber = 3;
    }
    // Kwadrant 4
    if (angle >= 270 && angle <= 360) {
      quadrantNumber = 4;
    }
    if (quadrantNumber != previousQuadrantNumber) {
      // Overgang van 4e naar 1e kwadrant
      if (quadrantNumber == 1 && previousQuadrantNumber == 4) {
        numberOfTurns++;
      }
      // Overgang van 1e naar 4e kwadrant
      if (quadrantNumber == 4 && previousQuadrantNumber == 1) {
        numberOfTurns--;
      }
      previousQuadrantNumber = quadrantNumber;
    }
    if (totalAngle >= 0) {
      totalAngle = (numberOfTurns * 360) + angle;
    } else {
      totalAngle = (numberOfTurns * 360) + angle;
    }
    // Stel Input-waarde voor PID vast
    Input = totalAngle;
  }
  // Beperkte rotatiemodus
  else if (digitalRead(modeSwitch) == 1) {
    rangeAdjustment = analogRead(A1);
    leftLimit = 0 + 30 + rangeAdjustment;
    rightLimit = 4097 - 30 - rangeAdjustment;
    if (digitalRead(inputSwitch) == 0) {
      // Analog invoer - Potentiometer
      // Haal waarde van potentiometer
      inputValue = analogRead(A0);
      if (inputValue < 15) {
        inputValue = 15;
      }
      if (inputValue > 1008) {
        inputValue = 1008;
      }
      Setpoint = map(inputValue, 15, 1008, -255, 255);
    } else if (digitalRead(inputSwitch) == 1) {
      // Digitale invoer - RC-zender
      inputValue = pulseIn(ch1, HIGH, 30000); // Lees RC-ontvanger als invoer
      Setpoint = map(inputValue, 1000, 2000, -255, 255);
    }
    // Stel middenhoek in
    if (digitalRead(centerSet) == LOW) {
      centerAngle = encoderValue;
      angleDifference = 2047 - encoderValue;
      delay(1000);
    }
    // Pas hoekwaarde aan volgens middenpunt (angleDifference)
    if (centerAngle < 2047) {
      angleValue = encoderValue + angleDifference;
      if (encoderValue < 4097 && encoderValue > (4096 - angleDifference)) {
        angleValue = encoderValue - 4097 + angleDifference;
      }
    }
    if (centerAngle > 2047) {
      angleValue = encoderValue + angleDifference;
      if (encoderValue >= 0 && encoderValue < abs(angleDifference)) {
        angleValue = encoderValue + 4097 + angleDifference;
      }
    } else if (centerAngle == 2047) {
      angleValue = encoderValue;
    }
    // Stel Input-waarde voor PID vast
    Input = map(angleValue, leftLimit, rightLimit, -255, 255);
  }
  // Gevoeligheid aanpassen
  Pk1 = analogRead(A2) * 0.002;
  myPID.SetTunings(Pk1, Ik1, Dk1);
  // Voer PID-proces uit om Output-waarde te krijgen
  myPID.Compute();
  // Beweeg rechts
  if (Output > 1) {
    pwmValue = Output;
    if (pwmValue < 30 && pwmValue > 5) {
      pwmValue = pwmValue + 30;
    }
    if (pwmValue <= 5) {
      pwmValue = 0;
    }
    digitalWrite(motor_IN1, LOW);
    analogWrite(motor_IN2, pwmValue);
  }
  // Beweeg links
  else if (Output < 1) {
    pwmValue = abs(Output);
    if (pwmValue < 30 && pwmValue > 5) {
      pwmValue = pwmValue + 30;
    }
    if (pwmValue <= 5) {
      pwmValue = 0;
    }
    analogWrite(motor_IN1, pwmValue);
    digitalWrite(motor_IN2, LOW);
  }
  // Beweeg niet
  else if (Output > -1 && Output < 1) {
    pwmValue = 0;
    digitalWrite(motor_IN1, LOW);
    digitalWrite(motor_IN2, LOW);
  }
  //Serial.print(Setpoint);
  //Serial.print("\t");
  //Serial.println(totalAngle);
}

Codeoverzicht

We starten de lus met het lezen van de encoder-waarde of de huidige positie van de as.

// Lees encoder-waarde - huidige positie
encoderValue = as5600.readAngle();

Dan, als we in Continue rotatiemodus zitten, accepteren we waarden van de seriële monitor en gebruiken ze als setpoint of gewenste hoek voor de PID-controller.

// Voer gewenste hoek in voor servo om naar toe te gaan via seriële monitor
while (Serial.available() > 0) {
  int inChar = Serial.read();
  if (isDigit(inChar)) {
    // Converteer binnenkomende byte naar char en voeg toe aan string:
    inString += (char)inChar;
  }
  // Als je een newline krijgt, print de string, dan de waarde van de string:
  if (inChar == '\n') {
    Setpoint = inString.toInt(); // Setpoint - gewenste hoek
    // Wis string voor nieuwe invoer:
    inString = "";
  }
}

Als de invoermodus ingesteld is op potentiometer, lezen we de analoge invoer, en corrigeren de setpoint-waarde afhankelijk van hoe ver we hem draaien.

if (digitalRead(inputSwitch) == 0) {
  // Potentiometer als invoer
  inputValue = analogRead(A0);
  if (inputValue < 400) {
    Setpoint = Setpoint - 0.3;
  }
  if (inputValue < 300) {
    Setpoint = Setpoint - 0.3;
  }
  if (inputValue < 200) {
    Setpoint = Setpoint - 0.3;
  }
  if (inputValue > 600) {
    Setpoint = Setpoint + 0.3;
  }
  if (inputValue > 700) {
    Setpoint = Setpoint + 0.3;
  }
  if (inputValue > 800) {
    Setpoint = Setpoint + 0.3;
  }
}

We doen dezelfde setpoint-correctie als de invoer de RC-ontvanger is.

Hier converteren we de RAW-encoderwaarden naar hoekwaarden, en met deze if-statements houden we bij in welk kwadrant de huidige positie van de as zit.

// Converteer encoder RAW-waarden naar hoekwaarde
angle = encoderValue * 0.087890625;
// Kwadrant 1
if (angle >= 0 && angle <= 90) {
  quadrantNumber = 1;
}
// Kwadrant 2
if (angle >= 90 && angle <= 180) {
  quadrantNumber = 2;
}
// Kwadrant 3
if (angle >= 180 && angle <= 270) {
  quadrantNumber = 3;
}
// Kwadrant 4
if (angle >= 270 && angle <= 360) {
  quadrantNumber = 4;
}

Met deze informatie kunnen we bijhouden hoe de as draait en wanneer hij een volle draai maakt. De totale hoek is de Input-waarde voor de PID-controller.

if (quadrantNumber != previousQuadrantNumber) {
  // Overgang van 4e naar 1e kwadrant
  if (quadrantNumber == 1 && previousQuadrantNumber == 4) {
    numberOfTurns++;
  }
  // Overgang van 1e naar 4e kwadrant
  if (quadrantNumber == 4 && previousQuadrantNumber == 1) {
    numberOfTurns--;
  }
  previousQuadrantNumber = quadrantNumber;
}
if (totalAngle >= 0) {
  totalAngle = (numberOfTurns * 360) + angle;
} else {
  totalAngle = (numberOfTurns * 360) + angle;
}
// Stel Input-waarde voor PID vast
Input = totalAngle;

Aan de andere kant, als we in beperkte rotatiemodus zitten, lezen we eerst de potentiometerwaarde die gebruikt wordt voor het instellen van het rotatiebereik en passen de linker- en rechterlimiet van de rotatie aan.

rangeAdjustment = analogRead(A1);
leftLimit = 0 + 30 + rangeAdjustment;
rightLimit = 4097 - 30 - rangeAdjustment;

Als de invoermodus de potentiometer is, gebruiken we de waarde ervan als setpoint-waarde voor de PID-controller.

if (digitalRead(inputSwitch) == 0) {
  // Analog invoer - Potentiometer
  // Haal waarde van potentiometer
  inputValue = analogRead(A0);
  if (inputValue < 15) {
    inputValue = 15;
  }
  if (inputValue > 1008) {
    inputValue = 1008;
  }
  Setpoint = map(inputValue, 15, 1008, -255, 255);
}

Als de invoermodus de RC-ontvanger is, lezen we de binnenkomende PWM-waarde van de ontvanger en gebruiken die waarde als setpoint.

else if (digitalRead(inputSwitch) == 1) {
  // Digitale invoer - RC-zender
  inputValue = pulseIn(ch1, HIGH, 30000); // Lees RC-ontvanger als invoer
  Setpoint = map(inputValue, 1000, 2000, -255, 255);
}

Voor het instellen van een ander middenpunt, controleren we of we op de drukknop hebben gedrukt en vangen die positie op als nieuw middenpunt.

// Stel middenhoek in
if (digitalRead(centerSet) == LOW) {
  centerAngle = encoderValue;
  angleDifference = 2047 - encoderValue;
  delay(1000);
}

Volgens dat, moeten we dan de werkelijke metingen van de encoder aanpassen en ze verschuiven met het hoekverschil tussen het nieuwe en het oude middenpunt. We gebruiken die waarde als invoerwaarde voor de PID-controller.

if (centerAngle > 2047) {
  angleValue = encoderValue + angleDifference;
  if (encoderValue >= 0 && encoderValue < abs(angleDifference)) {
    angleValue = encoderValue + 4097 + angleDifference;
  }
} else if (centerAngle == 2047) {
  angleValue = encoderValue;
}
// Stel Input-waarde voor PID vast
Input = map(angleValue, leftLimit, rightLimit, -255, 255);

Met de analoge invoer van de andere potentiometer passen we de proportionele winst van de PID-controller aan, en ten slotte voeren we het PID-proces uit om een uitgangswaarde te krijgen.

// Gevoeligheid aanpassen
Pk1 = analogRead(A2) * 0.002;
myPID.SetTunings(Pk1, Ik1, Dk1);
// Voer PID-proces uit om Output-waarde te krijgen
myPID.Compute();

We gebruiken die uitgangswaarde voor het aansturen van de DC-motoren met PWM-signaal, links of rechts, of in stilstand, afhankelijk van de uitgangswaarde van de PID-controller, of de fout tussen de gewenste en de werkelijke positie die de encoder leest.

// Beweeg rechts
if (Output > 1) {
  pwmValue = Output;
  if (pwmValue < 30 && pwmValue > 5) {
    pwmValue = pwmValue + 30;
  }
  if (pwmValue <= 5) {
    pwmValue = 0;
  }
  digitalWrite(motor_IN1, LOW);
  analogWrite(motor_IN2, pwmValue);
}

Dat was het dan voor deze video. Let op dat de code niet goed geoptimaliseerd is en er ruimte voor verbetering is.

Ook, als je dit project probeert na te bouwen, moet je klaar zijn om te troubleshooten. Er zijn veel dingen die mis kunnen gaan, vooral bij het solderen van die kleine SMD-componenten.

Ik kreeg deze servo niet vanaf de eerste keer werkend. Initieel had ik enkele verkeerde verbindingen op de PCB, toen bestelde ik de PCB opnieuw met nieuwe updates, maar had nog een paar pogingen nodig voordat het werkte.

Deze sectie van het artikel is nog in aanbouw, bekijk het later nog eens….