SHARE
TWEET

Arduino code for Delta Robot

a guest Jul 23rd, 2010 3,873 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>
  2.  
  3. /*
  4.  * Nunchuck -- Use a Wii Nunchuck
  5.  * Tim Hirzel http://www.growdown.com
  6.  *
  7.  notes on Wii Nunchuck Behavior.
  8.  This library provides an improved derivation of rotation angles from the nunchuck accelerometer data.
  9.  The biggest different over existing libraries (that I know of ) is the full 360 degrees of Roll data
  10.  from teh combination of the x and z axis accelerometer data using the math library atan2.
  11.  
  12.  It is accurate with 360 degrees of roll (rotation around axis coming out of the c button, the front of the wii),
  13.  and about 180 degrees of pitch (rotation about the axis coming out of the side of the wii).  (read more below)
  14.  
  15.  In terms of mapping the wii position to angles, its important to note that while the Nunchuck
  16.  sense Pitch, and Roll, it does not sense Yaw, or the compass direction.  This creates an important
  17.  disparity where the nunchuck only works within one hemisphere.  At a result, when the pitch values are
  18.  less than about 10, and greater than about 170, the Roll data gets very unstable.  essentially, the roll
  19.  data flips over 180 degrees very quickly.   To understand this property better, rotate the wii around the
  20.  axis of the joystick.  You see the sensor data stays constant (with noise).  Because of this, it cant know
  21.  the difference between arriving upside via 180 degree Roll, or 180 degree pitch.  It just assumes its always
  22.  180 roll.
  23.  
  24.  
  25.  *
  26.  * This file is an adaptation of the code by these authors:
  27.  * Tod E. Kurt, http://todbot.com/blog/
  28.  *
  29.  * The Wii Nunchuck reading code is taken from Windmeadow Labs
  30.  * http://www.windmeadow.com/node/42
  31.  */
  32.  
  33. #ifndef WiiChuck_h
  34. #define WiiChuck_h
  35.  
  36. #include "WProgram.h"
  37. #include <Wire.h>
  38. #include <math.h>
  39.  
  40.  
  41. // these may need to be adjusted for each nunchuck for calibration
  42. #define ZEROX 510  
  43. #define ZEROY 490
  44. #define ZEROZ 460
  45. #define RADIUS 210  // probably pretty universal
  46.  
  47. #define DEFAULT_ZERO_JOY_X 124
  48. #define DEFAULT_ZERO_JOY_Y 132
  49.  
  50.  
  51.  
  52. class WiiChuck {
  53.     private:
  54.         byte cnt;
  55.         uint8_t status[6];              // array to store wiichuck output
  56.         byte averageCounter;
  57.         //int accelArray[3][AVERAGE_N];  // X,Y,Z
  58.         int i;
  59.         int total;
  60.         uint8_t zeroJoyX;   // these are about where mine are
  61.         uint8_t zeroJoyY; // use calibrateJoy when the stick is at zero to correct
  62.         int lastJoyX;
  63.         int lastJoyY;
  64.         int angles[3];
  65.  
  66.         boolean lastZ, lastC;
  67.  
  68.  
  69.     public:
  70.  
  71.         byte joyX;
  72.         byte joyY;
  73.         boolean buttonZ;
  74.         boolean buttonC;
  75.         void begin()
  76.         {
  77.             Wire.begin();
  78.             cnt = 0;
  79.             averageCounter = 0;
  80.             Wire.beginTransmission (0x52);      // transmit to device 0x52
  81.             Wire.send (0x40);           // sends memory address
  82.             Wire.send (0x00);           // sends memory address
  83.             Wire.endTransmission ();    // stop transmitting
  84.             update();            
  85.             for (i = 0; i<3;i++) {
  86.                 angles[i] = 0;
  87.             }
  88.             zeroJoyX = DEFAULT_ZERO_JOY_X;
  89.             zeroJoyY = DEFAULT_ZERO_JOY_Y;
  90.         }
  91.  
  92.  
  93.         void calibrateJoy() {
  94.             zeroJoyX = joyX;
  95.             zeroJoyY = joyY;
  96.         }
  97.  
  98.         void update() {
  99.  
  100.             Wire.requestFrom (0x52, 6); // request data from nunchuck
  101.             while (Wire.available ()) {
  102.                 // receive byte as an integer
  103.                 status[cnt] = _nunchuk_decode_byte (Wire.receive()); //
  104.                 cnt++;
  105.             }
  106.             if (cnt > 5) {
  107.                 lastZ = buttonZ;
  108.                 lastC = buttonC;
  109.                 lastJoyX = readJoyX();
  110.                 lastJoyY = readJoyY();
  111.                 //averageCounter ++;
  112.                 //if (averageCounter >= AVERAGE_N)
  113.                 //    averageCounter = 0;
  114.  
  115.                 cnt = 0;
  116.                 joyX = (status[0]);
  117.                 joyY = (status[1]);
  118.                 for (i = 0; i < 3; i++)
  119.                     //accelArray[i][averageCounter] = ((int)status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2)));
  120.                     angles[i] = (status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2)));
  121.  
  122.                 //accelYArray[averageCounter] = ((int)status[3] << 2) + ((status[5] & B00110000) >> 4);
  123.                 //accelZArray[averageCounter] = ((int)status[4] << 2) + ((status[5] & B11000000) >> 6);
  124.  
  125.                 buttonZ = !( status[5] & B00000001);
  126.                 buttonC = !((status[5] & B00000010) >> 1);
  127.                 _send_zero(); // send the request for next bytes
  128.  
  129.             }
  130.         }
  131.  
  132.  
  133.     // UNCOMMENT FOR DEBUGGING
  134.     //byte * getStatus() {
  135.     //    return status;
  136.     //}
  137.  
  138.     float readAccelX() {
  139.        // total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
  140.         return (float)angles[0] - ZEROX;
  141.     }
  142.     float readAccelY() {
  143.         // total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
  144.         return (float)angles[1] - ZEROY;
  145.     }
  146.     float readAccelZ() {
  147.         // total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
  148.         return (float)angles[2] - ZEROZ;
  149.     }
  150.  
  151.     boolean zPressed(boolean raw = false) {
  152.         return (buttonZ && (raw || ! lastZ));
  153.     }
  154.     boolean cPressed(boolean raw = false) {
  155.         return (buttonC && (raw || ! lastC));
  156.     }
  157.  
  158.     // for using the joystick like a directional button
  159.     boolean rightJoy(int thresh=60) {
  160.         return (readJoyX() > thresh and lastJoyX <= thresh);
  161.     }
  162.  
  163.     // for using the joystick like a directional button
  164.     boolean leftJoy(int thresh=60) {
  165.         return (readJoyX() < -thresh and lastJoyX >= -thresh);
  166.     }
  167.  
  168.  
  169.     int readJoyX() {
  170.         return (int) joyX - zeroJoyX;
  171.     }
  172.  
  173.     int readJoyY() {
  174.         return (int)joyY - zeroJoyY;
  175.     }
  176.  
  177.  
  178.     // R, the radius, generally hovers around 210 (at least it does with mine)
  179.    // int R() {
  180.    //     return sqrt(readAccelX() * readAccelX() +readAccelY() * readAccelY() + readAccelZ() * readAccelZ());  
  181.    // }
  182.  
  183.  
  184.     // returns roll degrees
  185.     int readRoll() {
  186.         return (int)(atan2(readAccelX(),readAccelZ())/ M_PI * 180.0);
  187.     }
  188.  
  189.     // returns pitch in degrees
  190.     int readPitch() {        
  191.         return (int) (acos(readAccelY()/RADIUS)/ M_PI * 180.0);  // optionally swap 'RADIUS' for 'R()'
  192.     }
  193.  
  194.     private:
  195.         byte _nunchuk_decode_byte (byte x)
  196.         {
  197.             x = (x ^ 0x17) + 0x17;
  198.             return x;
  199.         }
  200.  
  201.         void _send_zero()
  202.         {
  203.             Wire.beginTransmission (0x52);      // transmit to device 0x52
  204.             Wire.send (0x00);           // sends one byte
  205.             Wire.endTransmission ();    // stop transmitting
  206.         }
  207.  
  208. };
  209.  
  210. #endif
  211.  
  212. // CODE FROM http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/
  213.  // robot geometry
  214.  // (look at pics above for explanation)
  215.  const float e = 110.0;     // end effector
  216.  const float f = 170.3;     // base
  217.  const float re = 190.0;
  218.  const float rf = 95.0;
  219.  
  220.  // trigonometric constants
  221.  const float sqrt3 = sqrt(3.0);
  222.  const float pi = 3.141592653;    // PI
  223.  const float sin120 = sqrt3/2.0;  
  224.  const float cos120 = -0.5;        
  225.  const float tan60 = sqrt3;
  226.  const float sin30 = 0.5;
  227.  const float tan30 = 1/sqrt3;
  228.  
  229.  // forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0)
  230.  // returned status: 0=OK, -1=non-existing position
  231.  int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) {
  232.      float t = (f-e)*tan30/2;
  233.      float dtr = pi/(float)180.0;
  234.  
  235.      theta1 *= dtr;
  236.      theta2 *= dtr;
  237.      theta3 *= dtr;
  238.  
  239.      float y1 = -(t + rf*cos(theta1));
  240.      float z1 = -rf*sin(theta1);
  241.  
  242.      float y2 = (t + rf*cos(theta2))*sin30;
  243.      float x2 = y2*tan60;
  244.      float z2 = -rf*sin(theta2);
  245.  
  246.      float y3 = (t + rf*cos(theta3))*sin30;
  247.      float x3 = -y3*tan60;
  248.      float z3 = -rf*sin(theta3);
  249.  
  250.      float dnm = (y2-y1)*x3-(y3-y1)*x2;
  251.  
  252.      float w1 = y1*y1 + z1*z1;
  253.      float w2 = x2*x2 + y2*y2 + z2*z2;
  254.      float w3 = x3*x3 + y3*y3 + z3*z3;
  255.      
  256.      // x = (a1*z + b1)/dnm
  257.      float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
  258.      float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;
  259.  
  260.      // y = (a2*z + b2)/dnm;
  261.      float a2 = -(z2-z1)*x3+(z3-z1)*x2;
  262.      float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;
  263.  
  264.      // a*z^2 + b*z + c = 0
  265.      float a = a1*a1 + a2*a2 + dnm*dnm;
  266.      float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
  267.      float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);
  268.  
  269.      // discriminant
  270.      float d = b*b - (float)4.0*a*c;
  271.      if (d < 0) return -1; // non-existing point
  272.  
  273.      z0 = -(float)0.5*(b+sqrt(d))/a;
  274.      x0 = (a1*z0 + b1)/dnm;
  275.      y0 = (a2*z0 + b2)/dnm;
  276.      return 0;
  277.  }
  278.  
  279.  // inverse kinematics
  280.  // helper functions, calculates angle theta1 (for YZ-pane)
  281.  int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) {
  282.      float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
  283.      y0 -= 0.5 * 0.57735    * e;    // shift center to edge
  284.      // z = a + b*y
  285.      float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
  286.      float b = (y1-y0)/z0;
  287.      // discriminant
  288.      float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
  289.      if (d < 0) return -1; // non-existing point
  290.      float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
  291.      float zj = a + b*yj;
  292.      theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
  293.      return 0;
  294.  }
  295.  
  296.  // inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
  297.  // returned status: 0=OK, -1=non-existing position
  298.  int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
  299.      theta1 = theta2 = theta3 = 0;
  300.      int status = delta_calcAngleYZ(x0, y0, z0, theta1);
  301.      if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2);  // rotate coords to +120 deg
  302.      if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3);  // rotate coords to -120 deg
  303.      return status;
  304.  }
  305.  
  306.  
  307. // CODE by prutsers.wordpress.com
  308.  
  309. Servo servo1;
  310. Servo servo2;
  311. Servo servo3;
  312.  
  313. float xPos = 0;  // x axis position
  314. float yPos = 0; // y axis position
  315. float zPos = -200; // z axis position, negative is up, positive is down. Delta bots are usually used up side down...
  316.  
  317. float t1;  //servo angle t for 'theta', 1 for servo 1
  318. float t2;
  319. float t3;
  320. float oldT1;
  321. float oldT2;
  322. float oldT3;
  323.  
  324. int result;
  325.  
  326. #define MAXANGLE 90
  327. #define MINANGLE -90
  328. WiiChuck chuck = WiiChuck();
  329.  
  330. void setup(){
  331.   servo1.attach(10);
  332.   servo2.attach(11);
  333.   servo3.attach(12);
  334.  
  335.   Serial.begin(38400);
  336.  
  337.   chuck.begin();
  338.   chuck.update();
  339. }
  340.  
  341. float updateRate = 5.0;
  342. float updateRateZ = 10.0;
  343. float threshold = 3.0;
  344.  
  345. float zDevSq = 5;
  346.  
  347. float joyAmplify = 1.5;
  348.  
  349. void loop(){
  350.   chuck.update();
  351.   xPos = xPos*(updateRate - 1.0) / updateRate + joyAmplify * chuck.readJoyX()/updateRate;
  352.   yPos = yPos*(updateRate - 1.0) / updateRate + joyAmplify * chuck.readJoyY()/updateRate;
  353.  
  354.   // a simple statistical filter for removing pitch sensor noise
  355.   int z = -120 - chuck.readPitch();
  356.   float dZSq = (zPos - z)*(zPos - z);
  357.   // update moving standard deviation
  358.   if (dZSq < 9*zDevSq)  // 3 stdevs from mean, both sides squared
  359.   {
  360.     zDevSq = zDevSq * (updateRateZ - 1) + dZSq;
  361.     zPos = zPos*(updateRateZ - 1.0) / updateRateZ + z / updateRateZ;
  362.   }
  363.  
  364.   //if (chuck.zPressed(true)) zPos += 1.0;
  365.   //if (chuck.cPressed(true)) zPos -= 1.0;
  366.  
  367.   result = delta_calcInverse(xPos, yPos, zPos, t1, t2, t3);
  368.  
  369.   Serial.print(result == 0 ? "ok" : "no");
  370.   char buf[10];
  371.   dtostrf(xPos, 4, 0, buf);
  372.   Serial.print(" X");
  373.   Serial.print(buf);
  374.   dtostrf(yPos, 4, 0, buf);
  375.   Serial.print(" Y");
  376.   Serial.print(buf);
  377.   dtostrf(zPos, 4, 0, buf);
  378.   Serial.print(" Z");
  379.   Serial.print(buf);
  380.  
  381.   dtostrf(t1, 6, 2, buf);
  382.   Serial.print(" T1");
  383.   Serial.print(buf);
  384.   dtostrf(t2, 6, 2, buf);
  385.   Serial.print(" T2");
  386.   Serial.print(buf);
  387.   dtostrf(t3, 6, 2, buf);
  388.   Serial.print(" T3");
  389.   Serial.print(buf);
  390.  
  391.   Serial.println("");
  392.  
  393.   if (result == 0) {
  394.     if (abs(oldT1 - t1) > threshold || abs(oldT2 - t2) > threshold || abs(oldT3 - t3) > threshold)
  395.     {
  396.       servo1.write(t1+90.0-14.0); //-14 is the correction for the angle for my serve
  397.       oldT1 = t1;
  398.       servo2.write(t2+90.0-10.0);
  399.       oldT2 = t2;
  400.       servo3.write(t3+90.0-00.0);
  401.       oldT3 = t3;
  402.     }
  403.   }
  404.   //delay(5);
  405. }
RAW Paste Data
Pastebin PRO Autumn Special!
Get 40% OFF on Pastebin PRO accounts!
Top