Stap 7: Software: PC-kant
Aangezien de SBC als de hersenen voor dit project fungeert, zal de meeste van de interacties met de Phidgets worden verzorgd door het. Wel, tenzij u van plan bent de robot als autonome, een soort van verbinding met een externe PC moet worden gemaakt zodat u het kunt controleren. Uiteindelijk betekent dit dat we zowel een programma dat wordt uitgevoerd op uw computer waarin de invoer van de gebruiker, en een programma zal hebben op de SBC die controles van de motor controllers enz... Communicatie tussen de twee programma's zullen worden behandeld met behulp van de Phidget woordenboek.
De PC verwerkt nemen input van de gebruiker en het vertalen in een set van gewenste motor snelheden, wat informatie en besturingselementen weer te geven, en het bekijken van de webcam feed. Omdat het mecanum wiel moet controleerbaar op 3 aparte assen (x, y, en een rotatie) de meest populaire methode voor het beheren van deze stijl van de robot is een twin joystick stijl met een joystick controle x en y beweging en de andere joystick controle van rotatie. We eindigden met behulp van een enkele joystick die had een extra as ingebouwd, maar in de praktijk om het even wat die u toelaat om het bepalen van de positie in 3 verschillende kanalen zou werken. De joystick die we gebruikten maakt rechtstreeks verbinding met de analoge input poorten op een 1018, dit is leuk omdat het maakt de code van de joystick invoerfilter nemen gemakkelijk.
Invoer
void controller_SensorChange(object sender, Phidgets.Events.SensorChangeEventArgs e) { JoystickDrive((controller.sensors[joyStickXAxisIndex].RawValue - xMid) / 2000.0, (controller.sensors[joyStickYAxisIndex].RawValue - yMid) / 2000.0, -(controller.sensors[joyStickZAxisIndex].RawValue - zMid) / 1500.0); } Deze functie verzamelt gegevens van elk kanaal van de joystick telkens er is een wijziging in een van de kanalen en verzendt vervolgens het uit naar een functie die converteert de joystick positie in een magnitude, richting (x en y vector) en een rotatie (de laatste as van de joystick). De wiskunde gedaan op elke sensor-waarde is het centrum rond 0 in plaats van 500 (of 2000 in het geval van RawValue). Het 3e kanaal voor onze joystick is beperkter bereik dan de andere twee, dus we alleen verdelen door 1500, niet 2000.
Converteren van Input
De functie die deze gegevens in iets bruikbaar converteert ziet er zo uit:
void JoystickDrive(double x, double y, double z) { //Dead Space double xyDeadSpace = 0.1; double zDeadSpace = 0.25; //Check that the position is outside the deadspace double newx = (Math.Abs(x) - xyDeadSpace) * (xyDeadSpace+1); if (newx < 0) newx = 0; x = (x < 0) ? -newx : newx; double newy = (Math.Abs(y) - xyDeadSpace) * (xyDeadSpace + 1); if (newy < 0) newy = 0; y = (y < 0) ? -newy : newy; double newz = (Math.Abs(z) - zDeadSpace) * (zDeadSpace + 1); if (newz < 0) newz = 0; z = (z < 0) ? -newz : newz; x = Limit(x); //these values should saturate at 1 y = Limit(y); z = Limit(z); double magnitude = Math.Sqrt(x * x + y * y); double direction = Math.Atan2(x, y); double rotation = z; if (magnitude == 0) direction = 0; //Into degrees direction = direction * 180.0 / Math.PI; if (gyroLocked) { direction -= heading; direction += headingLockPoint; } if (direction < 0) direction += 360; if (direction > 360) direction -= 360; MecanumDrive(magnitude, direction, rotation); } Wiel snelheid berekenen
Als we eenmaal een richting, rubriek en rotatie moet we berekenen hoeveel macht te geven van elke motor met het oog op de gewenste resolutie. In een normale voertuig zou dit vrij ongecompliceerd, maar met mecanum wiel er is een beetje van het werk betrokken.
void MecanumDrive(double magnitude, double direction, double rotation) { //Limit limits magnitude to 1.0 magnitude = Limit(magnitude); // Normalized for full power along the Cartesian axes. magnitude = magnitude * Math.Sqrt(2.0); // The rollers are at 45 degree angles. double dirInRad = (direction + 45.0) * Math.PI / 180.0; double cosD = Math.Cos(dirInRad); double sinD = Math.Sin(dirInRad); double[] wheelSpeeds = new double[4]; wheelSpeeds[0] = sinD * magnitude + rotation; wheelSpeeds[1] = cosD * magnitude - rotation; wheelSpeeds[2] = cosD * magnitude + rotation; wheelSpeeds[3] = sinD * magnitude - rotation; //Only if any are > 1.0 Normalize(wheelSpeeds); frontLeftSpeed = (wheelSpeeds[0] * m_maxOutput * frontLeftDir); frontRightSpeed = (wheelSpeeds[1] * m_maxOutput * frontRightDir); rearLeftSpeed = (wheelSpeeds[2] * m_maxOutput * rearLeftDir); rearRightSpeed = (wheelSpeeds[3] * m_maxOutput * rearRightDir); } Het eerste ding dat lijkt vreemde hier is de omvang = omvang * Math.Sqrt(2.0); lijn. Dit is een artefact van hoe joysticks functie. Om beter te begrijpen, hebben een blik bij het volgende diagram:
Aangezien joysticks een circulaire bereik van de beweging die u in het probleem hebben uitvoert dat u zal alleen bereiken een maximale X of Y-waarde wanneer u langs de X- of Y-as. Dus bent u op een willekeurige post dan 0, 90, 180 of 270 ° u zal nooit in staat zijn om te reizen op volle snelheid. Door de omvang van de vierkantswortel van 2 schalen kunt u er zeker van dat u zal volledige snelheid op alle koppen te bereiken, en aangezien het alle wiel-snelheden krijgen later genormaliseerd u niet zich ongerust maken over meer dan de maximale snelheid die de motoren kunnen bereiken.
Er zijn twee algemene manieren om weer te geven van een punt op een 2d-vlak bij de berekening van de richting en snelheid te draaien van de wielen bij gebaseerde op de joystick input: cartesiaanse coördinaten en polaire coördinaten. Om te generaliseren van onze code aan elke controller hebben we gebruikt polar, hoewel het algoritme misschien gemakkelijker kan worden begrepen als we in plaats daarvan in de meer traditionele Cartesisch coördinatenstelsel kijken. Laten we gebruiken opnieuw het geval van twin joysticks wordt gebruikt om de robot te besturen:
Door optelling van kracht vectoren voor elk van de primaire richtingen kunnen we een resulterende vector, dat van de juiste grootte en richting worden zal. Merk op dat het diagram alleen aanwijzingen voor positieve waarden van X toont, Y en Z (MovementX, MovementY en RotationX). Voor negatieve waarden die zou gewoon de pijlen worden teruggedraaid, blijft de wiskunde hetzelfde. Het laatste wat dat we doen is de waarden normaliseren zodat ze alles wat er tussen een magnitude van 0 en 1, schaal hen door de maximale snelheid van de motor zodat ze een evenredig percentage van max snelheid, en in de richting modifier toevoegen:
Normalize(wheelSpeeds); frontLeftSpeed = (wheelSpeeds[0] * m_maxOutput * frontLeftDir); frontRightSpeed = (wheelSpeeds[1] * m_maxOutput * frontRightDir); rearLeftSpeed = (wheelSpeeds[2] * m_maxOutput * rearLeftDir); rearRightSpeed = (wheelSpeeds[3] * m_maxOutput * rearRightDir);
De frontLeftDir, frontRightDir enz... parameters worden gebruikt om de waarden voor de motoren die worden geconfronteerd met de tegenovergestelde richting omkeren. Vergeet niet dat twee van de motoren staan unidirectioneel en de andere twee worden geconfronteerd met de andere kant. Dit betekent dat we de waarden omkeren moeten, omdat anders de motoren lijken zal te werken in omgekeerde richting (denk aan een bout in van bovenaf versus onder de bout schroeven).
Volledige Code
U kunt de volledige Visual Studio-project hier downloaden:
Dit project bevat code aan interface met de webcam. U kunt meer informatie over interfacing met webcams in de USB-Webcam Primer.
