// Motion Replicator Arduino Code #include <Servo.h> #include <firFilter.h> // Initialize servo and low-pass filter objects Servo s1; Servo s2; Servo s3; firFilter filter1; firFilter filter2; firFilter filter3; // VARIABLE DECLARATION AND INITIATION // Point tracking variables float as; // unmapped x-coordinate of target point float bs; // unmapped y-coordinate of target point float cs; // unmapped z-coordinate of target point float m1; // length of string 1 (-x-axis) float m2; // length of string 2 (+x-axis) float m3; // length of string 3 (y-axis) float w=54.25; // base equilateral triangle half-length (mm) float alt2; // altitude of triangle comprised of 2w, m1, and m2 float alt3; // altitude of (right) triangle comprised of alt3, a, and m3 float rad1=7.52+0.65; // radius of pulley 1 (+0.65 for rubber bands) float rad2=9.62+0.65; // radius of pulley 2 float rad3=9.625+0.65; // radius of pulley 3 float offset1; float offset2; float offset3; // Circle intersection calculation variables float a; // endpoint x-coordinate float b; // endpoint y-coordinate float c; // endpoint z-coordinate float d; // endpoint cylindrical r-coordinate float r1=200; // lower arm radius (mm) float r2=190; // upper arm radius (mm) float x; // joint x-coordinate (UNUSED) float y; // joint y-coordinate (UNUSED) float z; // joint z-coordinate float zrot; // rotated joint z-coordinate float r; // joint cylindrical r-coordinate float rrot; // rotated joint cylindrical r-coordinate float e; // distance from origin to endpoint float h; // angle between xy-plane and endpoint position vector // Servo limit variables float s1min=658; // lower arm 0 deg -> microseconds float s1max=2400; // lower arm 180 deg -> microseconds float s2min=642; // upper arm 0 deg -> microseconds float s2max=2294; // upper arm 180 deg -> microseconds float s3min=547; // base servo 0 deg -> microseconds float s3max=2289; // base servo 180 deg -> microseconds // Output variables float ang1; // lower arm angle w/r/t xy-plane float ang2; // upper arm angle w/r/t xy-plane float ang2a; // rotated/scaled ang2 sent to servo to accommodate servo limits float ang3; // base servo angle w/r/t negative x-axis towards positive y-axis int pulse1; // lower arm angle in microseconds int pulse2; // upper arm angle in microseconds int pulse3; // base servo angle in microseconds void setup() { s1.attach(9); s2.attach(10); s3.attach(11); Serial.begin(9600); // Offset calibration calculations // (target point should initiate in center of equilateral triangle) m1=analogRead(1)*M_PI*2*rad1*10/1023; m2=analogRead(2)*M_PI*2*rad1*10/1023; m3=analogRead(3)*M_PI*2*rad1*10/1023; offset1=m1-w/sqrt(3)*2; offset2=m2-w/sqrt(3)*2; offset3=m3-w/sqrt(3)*2; } void loop() { m1=filter1.run(analogRead(1))*M_PI*2*rad1*10/1023-offset1; // "float mapping" of signal from 0->1023 to 0->max length of string m2=filter2.run(analogRead(2))*M_PI*2*rad1*10/1023-offset2; m3=filter3.run(analogRead(3))*M_PI*2*rad1*10/1023-offset3; // Calculate (x,y,z) coordinate of target point from cord lengths m1, m2, and m3 // This method is taken from: http://mathworld.wolfram.com/Circle-CircleIntersection.html as=w-(4*pow(w,2)-pow(m1,2)+pow(m2,2))/4/w; alt2=sqrt(pow(m2,2)-pow(w-as,2)); alt3=sqrt(pow(m3,2)-pow(as,2)); bs=(3*pow(w,2)-pow(alt3,2)+pow(alt2,2))/2/w/sqrt(3); cs=pow(alt2,2)-pow(bs,2); // determinant of cs if(cs<0) { // excludes imaginary cs case cs=0; } else { cs=sqrt(cs); } a=3*as; b=3*bs; c=3*cs; d=sqrt(pow(a,2)+pow(b,2)); // Calculate circle intersection coordinates in r-z cylindrical coordinate system // This method is taken from: http://mathworld.wolfram.com/Circle-CircleIntersection.html if(abs(d)<1E-5 && abs(c)<1E-5) {} else { e=sqrt(pow(c,2)+pow(d,2)); rrot=(pow(e,2)-pow(r2,2)+pow(r1,2))/2/e; zrot=sqrt(abs(pow(r1,2)-pow(rrot,2))); // Rotate r and z coordinates if(d==0) { h=M_PI/2; } else { h=atan2(c,d); // in radians } r=rrot*cos(h)+zrot*-sin(h); z=rrot*sin(h)+zrot*cos(h); } // Calculate arm angles ang1=atan2(z,r)*180/M_PI; ang2=atan2(c-z,d-r)*180/M_PI; ang2a=180-(constrain(ang2,-90,90)+90); /* "constrain" to limit to -90->+90 +90 to get to 0->180 range 180- to flip 0->180 to */ ang3=atan2(b,-a)*180/M_PI; // Account for divide-by-zero edge cases if(abs(r)<1E-5) { ang1=90; if(abs(d)<1E-5) { ang2a=90; } } else if(abs(a)<1E-5) { ang3=90; } pulse1=ang1*(s1max-s1min)/180+s1min; // "float maps" ang1 from 0->180 to s1min->s1max pulse2=ang2a*(s2max-s2min)/180+s2min; pulse3=ang3*(s3max-s3min)/180+s3min; // Move servos s1.writeMicroseconds(constrain(pulse1,s1min,s1max)); // constrain to avoid overstepping servo limits s2.writeMicroseconds(constrain(pulse2,s2min,s2max)); s3.writeMicroseconds(constrain(pulse3,s3min,s3max)); delay(15); // Print values Serial.print(m1); Serial.print(" "); Serial.print(m2); Serial.print(" "); Serial.print(m3); Serial.print(" "); Serial.print(c-z); Serial.print(" "); Serial.print(d-r); Serial.print(" "); Serial.print(ang2); Serial.println(); }