MURVV - mobila roboten (7 / 9 steg)
Steg 7: Programvara: PC-sidan
Eftersom SBC fungerar som hjärnan för detta projekt, de flesta av interaktioner med Phidgets kommer att tas om hand av det. Dock om du har för avsikt roboten att vara självständig, måste någon form av anslutning till en extern PC göras så att du kan styra den. Detta innebär slutligen att vi kommer att ha både ett program som körs på datorn som tar indata från användaren, och ett program på SBC som kontrollerar de motoriska styrenheterna osv... Kommunikation mellan de två programmen kommer att hanteras med hjälp av Phidget ordbok.
Datorn hanterar tar input från användaren och översätta den till en uppsättning av önskad motor hastigheter, visar lite information och kontroller och Visa webbkameran foder. Eftersom mecanum hjulet tillåter måste vara kontrollerbar på 3 separata axlar (x, y, och en roterande) den mest populära metoden för att kontrollera denna typ av robot är en twin joysticken stil med en joystick styrning x och y rörelse och andra styrspaken Kontrollera rotation. Vi slutade upp med en enda joystick som hade en extra axel inbyggd i det, men i praktiken allt som låter dig styra position i 3 olika kanaler skulle fungera. Joysticken vi använde ansluts direkt till de analoga ingången hamnarna på en 1018, detta är trevligt eftersom det gör koden för tar input från joysticken lätt.
Ta indata
void controller_SensorChange (objekt avsändare, 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);
}
Denna funktion samlar data från varje kanal av joysticken varje gång det sker en förändring i en kanal och sedan skickar den till en funktion som omvandlar joystick position till en magnitud, riktning (x och y vektor) och en rotation (den sista axeln på joysticken). Matematik på varje sensor värde är att centrera den runt 0 snarare än 500 (eller 2000 när det gäller RawValue). Den 3: e kanalen för våra joystick har mer begränsade utbud än de andra två, därav vi bara delar med 1500, inte 2000.
Konvertera indata
Den funktion som omvandlar informationen till något användbart ser ut så här:
void JoystickDrive (dubbel XY, dubbel, dubbel z)
{
Dead Space
Double xyDeadSpace = 0,1;
Double zDeadSpace = 0,25;
Kontrollera att ståndpunkten är utanför dödutrymme
Double newx = (Math.Abs(x) - xyDeadSpace) * (xyDeadSpace + 1);
om (newx < 0) newx = 0;
x = (x < 0)? -newx: newx;
Double newy = (Math.Abs(y) - xyDeadSpace) * (xyDeadSpace + 1);
om (newy < 0) newy = 0;
y = (y < 0)? -newy: newy;
dubbel newz = (Math.Abs(z) - zDeadSpace) * (zDeadSpace + 1);
om (newz < 0) newz = 0;
z = (z < 0)? -newz: newz;
x = Limit(x); dessa värden bör mätta på 1
y = Limit(y);
z = Limit(z);
dubbel storlek = Math.Sqrt (x * x + y * y);
dubbel riktning = Math.Atan2 (x, y);
dubbel rotation = z;
om (magnitud == 0)
riktning = 0;
I grader
riktning = riktning * 180,0 / Math.PI;
om (gyroLocked)
{
-= riktning;
riktning += headingLockPoint;
}
om (riktning < 0)
riktning += 360;
om (riktning > 360)
riktning-= 360;
MecanumDrive (storlek, riktning, rotation);
}
Beräkna hjulet hastigheter
När vi har en riktning, kurs och rotation måste vi beräkna hur mycket makt att ge varje motor för att uppnå önskad rörelse. I en normal bil skulle detta vara ganska rakt fram men med mecanum rullar det finns lite arbete inblandade.
void MecanumDrive (dubbel storlek, dubbel riktning, dubbel rotation)
{
Gräns gränser magnitud till 1.0
storlek = Limit(magnitude);
Normaliserade för full effekt längs de kartesiska axlarna.
storlek = storlek * Math.Sqrt(2.0);
Rullarna är i 45 graders vinkel.
Double dirInRad = (riktning + 45,0) * Math.PI / 180.0;
Double cosD = Math.Cos(dirInRad);
dubbel sinD = Math.Sin(dirInRad);
dubbel [] wheelSpeeds = ny dubbel [4].
wheelSpeeds [0] = sinD * magnitud + rotationssystem.
wheelSpeeds [1] = cosD * magnitud - rotation;
wheelSpeeds [2] = cosD * magnitud + rotationssystem.
wheelSpeeds [3] = sinD * magnitud - rotation;
Om någon är > 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);
}
Det första som kan tyckas märkliga här är omfattningen = storlek * Math.Sqrt(2.0); linje. Detta är en artefakt av hur joysticks funktion. För att bättre förstå, ta en titt på följande diagram:
Eftersom joysticks har en cirkulär rörelseomfång du stöter på problemet att du kommer att bara nå en maximal X eller Y-värde när du är längs X- eller Y-axeln. Så om du på något annat nummer än 0, 90, 180 eller 270 ° du kommer aldrig att kunna resa i full fart. Genom att skala omfattningen av kvadratroten ur 2 kan du vara säker på att du kommer att nå full fart på alla rubriker och sedan rullar rusar alla få normaliserade senare du inte behöver oroa sig mer än den högsta hastighet motorerna kan uppnå.
När beräkningen av riktning och hastighet att snurra hjulen på baserat på joysticken ingång det finns två vanliga sätt att representera en punkt på ett 2d-plan: kartesiska koordinater och Polära koordinater. För att generalisera våra koden till valfri controller har vi använt polar, men algoritmen kan kanske förstås lättare om vi istället tittar på det i det mer traditionella Cartesian koordinerat systemet. Låt oss återigen använda fallet med twin styrspakar som används för att styra roboten:
Genom att summera kraft vektorer för vart och ett av de primära riktningarna kan vi få en resulterande vektorn som kommer att vara av rätt storlek och riktning. Observera att diagrammet endast visar riktningar för positiva värden för X, Y och Z (MovementX, MovementY och RotationX). För negativa värden pilarna skulle helt enkelt återföras, förblir matten densamma. Det sista vi gör är normalisera värdena så att de är allt mellan en magnitud av 0 och 1, skala dem av den maximala varvtal så att de är en proportionerlig andel av max hastighet, och lägga till i riktning modifieraren:
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);
FrontLeftDir, frontRightDir osv... modifierare används för att Invertera värden för de motorer som vänd åt motsatt håll. Kom ihåg att två av motorerna står inför ett sätt och de andra två står inför det andra sättet. Detta innebär att vi behöver att Invertera värden eftersom annars motorerna visas att arbeta i omvänd (tror att skruva en bult i från ovan kontra nedan bulten).
Fullständiga koden
Du kan hämta det Visual Studio-projektet här:
Detta projekt innehåller koden till gränssnittet med webbkameran. Du kan läsa mer om gränssnitt med webbkameror i USB Webcam Primer.