Bygga en robot som följer muspekaren (6 / 35 steg)
#include < Servo.h >
för att aktivera debug utgångar, avkommentera följande rad:
#define DEBUG
Servo servoLeft;
Servo servoRight;
Servo servoUp;
CONST float pi = 3.14159;
flyta RadianToDegree = 180/pi;
flyta DegreeToRadian = pi/180;
längden på segmentet arm (dimensioner skalas så att arm segment längd är lika med 1.)
CONST float en = 1.;
förskjutning mellan de två servomotor axlarna:
CONST float d = 5,7/14,7;
konvertera inkommande byte till float värden:
CONST float ByteToFloat = 1.0/255.0;
säkerhet marginaler att undvika spännande zonen säker ritning:
Observera att arbetsytan har samma x / y-förhållande här än i bearbetning skiss
CONST float thetaMax = arccos (d/2. - 1.);
CONST float YMin = sin(thetaMax) + d;
CONST float XAmplitude = 2.2.
CONST float YAmplitude = 1.1;
CONST float XMin = - 0.5*(XAmplitude-d);
CONST float XMax = d-XMin;
CONST float YMax = YMin + YAmplitude;
penna position (dimensioner skalas så att arm segment längd är lika med 1.)
float x = 0.;
float y = 1,2.
för seriell kommunikation:
int MyDelay = 10;
int incomingByteX = 0;
int incomingByteY = 0;
int incomingByteU = 0;
initiera servomotor positioner och seriell port
void setup()
{
Serial.BEGIN(9600);
servoLeft.attach(9);
servoRight.attach(10);
servoUp.attach(6);
servoLeft.writeMicroseconds(1940);
servoRight.writeMicroseconds(1095);
servoUp.writeMicroseconds(1100);
}
följande metod läser de seriella ingångarna,
omvandlar inkommande data till penna positioner
konverterar penna positioner till servomotor vinklar
skickar vinkel värden till servomotorer
void loop()
{
vänta 3 bytes
IF(Serial.available() > = 3);
{
incomingByteX = Serial.read();
incomingByteY = Serial.read();
incomingByteU = Serial.read();
Kontrollera att inkommande data följer den convetions som vi satt i bearbetning skissen
Detta undviker misstolka data som kommer unordered
om ((incomingByteU == 100 || incomingByteU == 200)
& & incomingByteX > = 0 & & incomingByteX < = 127
& & incomingByteY > = 127 & & incomingByteY < = 255)
{
#ifdef DEBUG
Serial.Print ("SERIAL X:"); Serial.Print(incomingByteX);
Serial.Print("|| SERIELLA Y: "); Serial.Print(incomingByteY);
Serial.Print("|| SERIELLA U: "); Serial.Print(incomingByteU); Serial.println();
#endif
x = incomingByteX*ByteToFloat*2*(XMax-XMin) + XMin;
y = (incomingByteY * ByteToFloat - .5)*2*(YMax-YMin) + YMin;
omvandla (x, y) värden till servomotor vinklar
flyta ThetaLeft = GetThetaLeft(x,y);
flyta ThetaRight = GetThetaRight(x,y);
Skicka positioner till servomotorer
SetServoLeftToTheta(ThetaLeft);
SetServoRightToTheta(ThetaRight);
ServoUp(incomingByteU==100);
}
}
Delay(MyDelay);
}
följande metoder omvandla (x, y) positioner till servomotor vinklar
float GetThetaLeft (float, float XY) {
float R = hypot (x, y);
flyta Theta = atan2(y,x);
flyta Phi = acos(R/2.);
flyta ut = RadianToDegree * (Theta + Phi);
återvända ut;
}
float GetThetaRight (float, float XY) {
x = (x-d);
float R = hypot (x, y);
flyta Theta = atan2(y,x);
flyta Phi = acos(R/2.);
flyta ut = RadianToDegree * (Theta - Phi);
återvända ut;
}
följande metoder som kantiga positioner på två arm servomotorer
SetServoLeftToTheta(float ThetaLeft) {float
kalibrering för vänster servo Hitech - 5645MG
theta | Oss
//---------------------------
2200
90 1940
180 1020
750
flyta mS = 1940. + (ThetaLeft-90.) *(1020.-1940.) / 90.;
servoLeft.writeMicroseconds(round(mS));
}
SetServoRightToTheta(float ThetaRight) {float
kalibrering för vänster servo Hitech - 5645MG
theta | Oss
//---------------------------
2200
0 1995
90 1095
750
flyta mS = 1995. + (ThetaRight)*(1095.-1995.) / 90.;
servoRight.writeMicroseconds(round(mS));
}
följande metod för lyftande servo till pen-up eller pen-down position
void ServoUp(boolean PenUp) {
IF(PenUp) {servoUp.writeMicroseconds(1000);}
annat {servoUp.writeMicroseconds(1200);}
}