Construir un robot que sigue el cursor del mouse (6 / 35 paso)

 #include <Servo.h> // to activate debug outputs, uncomment the following line: //#define DEBUG Servo servoLeft; Servo servoRight; Servo servoUp; const float pi = 3.14159; float RadianToDegree = 180/pi; float DegreeToRadian = pi/180; // the length of the arm segment (dimensions are scaled so that arm segment length is equal to 1.) const float a = 1.; // the offset between the two servomotor axes: const float d = 5.7/14.7; // to convert incoming bytes to float values: const float ByteToFloat = 1.0/255.0; // security margins to avoid exiting the safe drawing zone: // note that the drawing area has the same x/y ratio here than in the processing sketch const float thetaMax = acos(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; // pen position (dimensions are scaled so that arm segment length is equal to 1.) float x=0.; float y=1.2; // for serial port communication: int MyDelay=10; int incomingByteX = 0; int incomingByteY = 0; int incomingByteU = 0; // initialize servomotor positions and serial port void setup() { Serial.begin(9600); servoLeft.attach(9); servoRight.attach(10); servoUp.attach(6); servoLeft.writeMicroseconds(1940); servoRight.writeMicroseconds(1095); servoUp.writeMicroseconds(1100); } // the following method reads the serial inputs, // converts incoming data to pen positions // converts pen positions to servomotor angles // sends angle values to servomotors void loop() { // wait for 3 bytes if(Serial.available() >= 3) ; { incomingByteX = Serial.read(); incomingByteY = Serial.read(); incomingByteU = Serial.read(); // check that incoming data respects the convetions we set in the processing sketch // this avoids misinterpreting data that comes unordered if((incomingByteU==100 || incomingByteU==200) && incomingByteX>=0 && incomingByteX<=127 && incomingByteY>=127 && incomingByteY<=255) { #ifdef DEBUG Serial.print("SERIAL X : ");Serial.print(incomingByteX); Serial.print(" || SERIAL Y : ");Serial.print(incomingByteY); Serial.print(" || SERIAL U : ");Serial.print(incomingByteU);Serial.println(); #endif x = incomingByteX*ByteToFloat*2*(XMax-XMin) + XMin; y = (incomingByteY*ByteToFloat - .5)*2*(YMax-YMin) + YMin; // convert (x,y) values to servomotor angles float ThetaLeft = GetThetaLeft(x,y); float ThetaRight = GetThetaRight(x,y); // send the positions to the servomotors SetServoLeftToTheta(ThetaLeft); SetServoRightToTheta(ThetaRight); ServoUp(incomingByteU==100); } } delay(MyDelay); } // the following methods convert (x,y) positions to servomotor angles float GetThetaLeft(float x, float y){ float R = hypot (x,y); float Theta = atan2(y,x); float Phi = acos(R/2.); float Out = RadianToDegree*(Theta + Phi); return Out; } float GetThetaRight(float x, float y){ x = (x-d); float R = hypot (x,y); float Theta = atan2(y,x); float Phi = acos(R/2.); float Out = RadianToDegree*(Theta - Phi); return Out; } // the following methods set angular positions on the two arm servomotors float SetServoLeftToTheta(float ThetaLeft){ // calibration for left servo Hitech-5645MG // theta | uS //--------------------------- // 2200 // 90 1940 // 180 1020 // 750 float mS = 1940. + (ThetaLeft-90.)*(1020.-1940.)/90.; servoLeft.writeMicroseconds(round(mS)); } float SetServoRightToTheta(float ThetaRight){ // calibration for left servo Hitech-5645MG // theta | uS //--------------------------- // 2200 // 0 1995 // 90 1095 // 750 float mS = 1995. + (ThetaRight)*(1095.-1995.)/90.; servoRight.writeMicroseconds(round(mS)); } // the following method brings the lifting servo to pen-up or pen-down position void ServoUp(boolean PenUp){ if(PenUp){ servoUp.writeMicroseconds(1000); } else { servoUp.writeMicroseconds(1200); } }