├── .gitignore ├── RSP_MasterSlave ├── .DS_Store └── RSP_MasterSlave.ino ├── README.textile ├── RSP_Center └── RSP_Center.ino ├── RSP_Listen └── RSP_Listen.ino ├── RSP_Test └── RSP_Test.ino └── RSPv1 ├── Vector3.h └── RSPv1.ino /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | RSPv1/.DS_Store 3 | -------------------------------------------------------------------------------- /RSP_MasterSlave/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarginallyClever/RotaryStewartPlatform/HEAD/RSP_MasterSlave/.DS_Store -------------------------------------------------------------------------------- /README.textile: -------------------------------------------------------------------------------- 1 | h1. Rotary Stewart Platform for students 2 | 3 | Please visit "Marginally Clever":http://www.marginallyclever.com/ for more great robot goodness. 4 | 5 | Please visit "the wiki":https://github.com/MarginallyClever/RotaryStewartPlatform/wiki for installation, troubleshooting, and more. -------------------------------------------------------------------------------- /RSP_Center/RSP_Center.ino: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------------ 2 | // Center for Rotary Stewart Platform 3 | // dan@marginallycelver.com 2013-03-11 4 | //------------------------------------------------------------------------------ 5 | // Copyright at end of file. 6 | // please see http://www.marginalyclever.com/ for more information. 7 | 8 | 9 | #include 10 | 11 | 12 | Servo arm[6]; 13 | 14 | 15 | void setup() { 16 | Serial.begin(57600); 17 | Serial.println(F("Hello, World! Centering Servos...")); 18 | 19 | int i; 20 | for(i=0;i<6;++i) { 21 | arm[i].attach(7-i); 22 | arm[i].write( 90 ); 23 | } 24 | } 25 | 26 | 27 | void loop() {} 28 | 29 | 30 | 31 | //------------------------------------------------------------------------------ 32 | // Copyright (C) 2012 Dan Royer (dan@marginallyclever.com) 33 | // Permission is hereby granted, free of charge, to any person obtaining a 34 | // copy of this software and associated documentation files (the "Software"), 35 | // to deal in the Software without restriction, including without limitation 36 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 37 | // and/or sell copies of the Software, and to permit persons to whom the 38 | // Software is furnished to do so, subject to the following conditions: 39 | // 40 | // The above copyright notice and this permission notice shall be included in 41 | // all copies or substantial portions of the Software. 42 | // 43 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 44 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 45 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 46 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 47 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 48 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 49 | // DEALINGS IN THE SOFTWARE. 50 | //------------------------------------------------------------------------------ 51 | 52 | -------------------------------------------------------------------------------- /RSP_Listen/RSP_Listen.ino: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------------ 2 | // Test for Rotary Stewart Platform 3 | // dan@marginallycelver.com 2013-03-11 4 | //------------------------------------------------------------------------------ 5 | // Copyright at end of file. 6 | // please see http://www.marginalyclever.com/ for more information. 7 | 8 | 9 | void setup() { 10 | Serial.begin(57600); 11 | } 12 | 13 | 14 | void loop() { 15 | Serial.print(analogRead(0)); Serial.print('\t'); 16 | Serial.print(analogRead(1)); Serial.print('\t'); 17 | Serial.print(analogRead(2)); Serial.print('\t'); 18 | Serial.print(analogRead(3)); Serial.print('\t'); 19 | Serial.print(analogRead(4)); Serial.print('\t'); 20 | Serial.print(analogRead(5)); Serial.print('\n'); 21 | delay(100); 22 | } 23 | 24 | 25 | //------------------------------------------------------------------------------ 26 | // Copyright (C) 2012 Dan Royer (dan@marginallyclever.com) 27 | // Permission is hereby granted, free of charge, to any person obtaining a 28 | // copy of this software and associated documentation files (the "Software"), 29 | // to deal in the Software without restriction, including without limitation 30 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 31 | // and/or sell copies of the Software, and to permit persons to whom the 32 | // Software is furnished to do so, subject to the following conditions: 33 | // 34 | // The above copyright notice and this permission notice shall be included in 35 | // all copies or substantial portions of the Software. 36 | // 37 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 38 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 39 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 40 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 41 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 42 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 43 | // DEALINGS IN THE SOFTWARE. 44 | //------------------------------------------------------------------------------ 45 | 46 | -------------------------------------------------------------------------------- /RSP_Test/RSP_Test.ino: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------------ 2 | // Test for Rotary Stewart Platform 3 | // dan@marginallycelver.com 2013-03-11 4 | //------------------------------------------------------------------------------ 5 | // Copyright at end of file. 6 | // please see http://www.marginalyclever.com/ for more information. 7 | 8 | 9 | #include 10 | 11 | 12 | Servo arm[6]; 13 | 14 | 15 | void setup() { 16 | Serial.begin(57600); 17 | Serial.println(F("Hello, World!")); 18 | 19 | int i; 20 | for(i=0;i<6;++i) { 21 | arm[i].attach(2+i); 22 | } 23 | } 24 | 25 | 26 | // reverse the direction for every second servo. 27 | void set_servo(int index,int angle) { 28 | int j = 90 + angle * ((index%2)?1:-1); 29 | arm[index].write( j ); 30 | } 31 | 32 | 33 | void loop() { 34 | float t=millis()*0.001; 35 | 36 | int i,j; 37 | for(i=0;i<6;++i) { 38 | j = sin(t+(float)i*PI/3.0)*30; 39 | set_servo(i,j); 40 | Serial.print(j); 41 | Serial.print('\t'); 42 | } 43 | 44 | Serial.print('\n'); 45 | delay(50); 46 | } 47 | 48 | 49 | 50 | //------------------------------------------------------------------------------ 51 | // Copyright (C) 2012 Dan Royer (dan@marginallyclever.com) 52 | // Permission is hereby granted, free of charge, to any person obtaining a 53 | // copy of this software and associated documentation files (the "Software"), 54 | // to deal in the Software without restriction, including without limitation 55 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 56 | // and/or sell copies of the Software, and to permit persons to whom the 57 | // Software is furnished to do so, subject to the following conditions: 58 | // 59 | // The above copyright notice and this permission notice shall be included in 60 | // all copies or substantial portions of the Software. 61 | // 62 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 63 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 64 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 65 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 66 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 67 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 68 | // DEALINGS IN THE SOFTWARE. 69 | //------------------------------------------------------------------------------ 70 | 71 | -------------------------------------------------------------------------------- /RSP_MasterSlave/RSP_MasterSlave.ino: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------------ 2 | // Master Slave Demo for Rotary Stewart Platform 3 | // dan@marginallycelver.com 2013-04-22 4 | //------------------------------------------------------------------------------ 5 | // Copyright at end of file. 6 | // please see http://www.marginalyclever.com/ for more information. 7 | 8 | #include 9 | 10 | // uncomment the next line to see more information in the serial output 11 | //#define DEBUG_ON 1 12 | #ifdef DEBUG_ON 13 | #define DEBUG(x) { Serial.print(x); } 14 | #else 15 | #define DEBUG(x) 16 | #endif 17 | 18 | 19 | struct Arm { 20 | Servo servo; 21 | float angle; 22 | int flip; 23 | int angle_min; 24 | int angle_max; 25 | }; 26 | 27 | Arm output[6]; 28 | 29 | 30 | int i; 31 | 32 | 33 | void setup() { 34 | Serial.begin(9600); 35 | Serial.println(F("START")); 36 | 37 | for(i=0;i<6;++i) { 38 | output[i].servo.attach(7-i); 39 | output[i].servo.write(90); 40 | output[i].flip = 1; 41 | } 42 | 43 | output[1].flip ^= 1; // servo 1 is acting backwards, reason unknown. 44 | 45 | configureLimits(); 46 | } 47 | 48 | 49 | void loop() { 50 | for(i=0;i<6;++i) { 51 | // analogRead can only return a value from 0 to 1023. 52 | output[i].angle = analogRead(A0+i); 53 | // grow the max/min to make sure we never leave acceptable range. 54 | if(output[i].angle > output[i].angle_max) output[i].angle_max = output[i].angle; 55 | if(output[i].angle < output[i].angle_min) output[i].angle_min = output[i].angle; 56 | // make it range from 0 to 1 57 | output[i].angle = ( output[i].angle - output[i].angle_min ) / ( output[i].angle_max - output[i].angle_min ); 58 | if(output[i].flip==1) output[i].angle = 1.0 - output[i].angle; 59 | 60 | DEBUG( output[i].angle ); 61 | DEBUG('\t'); 62 | 63 | // servo::writeMicroseconds takes a value from 1000...2000 64 | // servo::write takes a value from 0...180 (less precise) 65 | // tests showed that the inputs are rever 66 | output[i].servo.writeMicroseconds(1000.0 + 1000.0 * output[i].angle); 67 | } 68 | 69 | DEBUG('\n'); 70 | delay(50); 71 | } 72 | 73 | 74 | void configureLimits() { 75 | // configure minimums 76 | Serial.println(F("Push the joystick all the way down and send \"MIN\".")); 77 | while(Serial.available()<3); 78 | i=0; 79 | while(Serial.available()>0) { 80 | Serial.read(); 81 | ++i; 82 | } 83 | Serial.println(i); 84 | 85 | for(i=0;i<6;++i) { 86 | output[i].angle_min=analogRead(A0+i); 87 | } 88 | 89 | // configure maximums 90 | Serial.println(F("Push the joystick all the way down and send \"MAX\".")); 91 | while(Serial.available()<3); 92 | i=0; 93 | while(Serial.available()>0) { 94 | Serial.read(); 95 | ++i; 96 | } 97 | Serial.println(i); 98 | 99 | for(i=0;i<6;++i) { 100 | output[i].angle_max=analogRead(A0+i); 101 | } 102 | 103 | Serial.println(F("Calibrated. Enjoy!")); 104 | } 105 | 106 | //------------------------------------------------------------------------------ 107 | // Copyright (C) 2013 Dan Royer (dan@marginallyclever.com) 108 | // Permission is hereby granted, free of charge, to any person obtaining a 109 | // copy of this software and associated documentation files (the "Software"), 110 | // to deal in the Software without restriction, including without limitation 111 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 112 | // and/or sell copies of the Software, and to permit persons to whom the 113 | // Software is furnished to do so, subject to the following conditions: 114 | // 115 | // The above copyright notice and this permission notice shall be included in 116 | // all copies or substantial portions of the Software. 117 | // 118 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 119 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 120 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 121 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 122 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 123 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 124 | // DEALINGS IN THE SOFTWARE. 125 | //------------------------------------------------------------------------------ 126 | 127 | -------------------------------------------------------------------------------- /RSPv1/Vector3.h: -------------------------------------------------------------------------------- 1 | #ifndef VECTOR3_H 2 | #define VECTOR3_H 3 | //------------------------------------------------------------------------------ 4 | // Vector3 class 5 | // dan@marginallycelver.com 2013-03-16 6 | //------------------------------------------------------------------------------ 7 | // Copyright at end of file. 8 | // please see http://www.github.com/MarginallyClever/RotaryStewartPlatform for more information. 9 | 10 | 11 | #include "Arduino.h" 12 | 13 | 14 | //------------------------------------------------------------------------------ 15 | class Vector3 { 16 | public: 17 | // these usions allow the Vector3 to be used as a color component 18 | float x; 19 | float y; 20 | float z; 21 | 22 | public: 23 | inline Vector3() {} 24 | 25 | 26 | inline Vector3( float xx, float yy, float zz ) { 27 | x = xx; 28 | y = yy; 29 | z = zz; 30 | } 31 | 32 | 33 | inline Vector3( float v[ 3 ] ) { 34 | x = v[ 0 ]; 35 | y = v[ 1 ]; 36 | z = v[ 2 ]; 37 | } 38 | 39 | 40 | ~Vector3() {}; 41 | 42 | 43 | inline Vector3 &MakeZero() { 44 | x=0; 45 | y=0; 46 | z=0; 47 | 48 | return *this; 49 | } 50 | 51 | 52 | inline Vector3 &Set( float xx, float yy, float zz ) { 53 | x = xx; 54 | y = yy; 55 | z = zz; 56 | 57 | return *this; 58 | } 59 | 60 | 61 | inline Vector3 operator + () const { // Unary negation 62 | return Vector3(*this); 63 | } 64 | 65 | 66 | inline Vector3 operator - () const { // Unary negation 67 | return Vector3( -x, -y, -z ); 68 | } 69 | 70 | 71 | inline Vector3 operator *= ( float v ) { // assigned multiply by a float 72 | x *= v; 73 | y *= v; 74 | z *= v; 75 | 76 | return *this; 77 | } 78 | 79 | 80 | inline Vector3 operator /= ( float t ) { // assigned division by a float 81 | float v; 82 | 83 | if( t == 0.0f ) 84 | v = 0; 85 | else 86 | v = 1.0f / t; 87 | 88 | x *= v; 89 | y *= v; 90 | z *= v; 91 | 92 | return *this; 93 | } 94 | 95 | 96 | inline Vector3 operator -= ( const Vector3 &v ) { // assigned subtraction 97 | x -= v.x; 98 | y -= v.y; 99 | z -= v.z; 100 | 101 | return *this; 102 | } 103 | 104 | 105 | inline Vector3 operator += ( const Vector3 &v ) { // assigned addition 106 | x += v.x; 107 | y += v.y; 108 | z += v.z; 109 | 110 | return *this; 111 | } 112 | 113 | 114 | inline Vector3 operator *= ( const Vector3 &v ) { // assigned mult. 115 | x *= v.x; 116 | y *= v.y; 117 | z *= v.z; 118 | 119 | return *this; 120 | } 121 | 122 | 123 | inline Vector3 operator ^= ( const Vector3 &v ) { // assigned cross product 124 | float nx, ny, nz; 125 | 126 | nx = ( y * v.z - z * v.y ); 127 | ny =-( x * v.z - z * v.x ); 128 | nz = ( x * v.y - y * v.x ); 129 | x = nx; 130 | y = ny; 131 | z = nz; 132 | 133 | return *this; 134 | } 135 | 136 | 137 | inline bool operator == ( const Vector3 &v ) const { 138 | return ( fabs( x - v.x ) < 0.01f && 139 | fabs( y - v.y ) < 0.01f && 140 | fabs( z - v.z ) < 0.01f ); 141 | } 142 | 143 | 144 | inline bool operator != ( const Vector3 &v ) const { 145 | return ( fabs( x - v.x ) > 0.01f || 146 | fabs( y - v.y ) > 0.01f || 147 | fabs( z - v.z ) > 0.01f ); 148 | } 149 | 150 | 151 | // METHODS 152 | inline float Length() const { 153 | return (float)sqrt( *this | *this ); 154 | } 155 | 156 | 157 | inline float LengthSquared() const { 158 | return *this | *this; 159 | } 160 | 161 | 162 | inline void Normalize() { 163 | float len, iLen; 164 | 165 | len = Length(); 166 | if( !len ) iLen = 0; 167 | else iLen = 1.0f / len; 168 | 169 | x *= iLen; 170 | y *= iLen; 171 | z *= iLen; 172 | } 173 | 174 | 175 | inline float NormalizeLength() { 176 | float len, iLen; 177 | 178 | len = Length(); 179 | if( !len ) iLen = 0; 180 | else iLen = 1.0f / len; 181 | 182 | x *= iLen; 183 | y *= iLen; 184 | z *= iLen; 185 | 186 | return len; 187 | } 188 | 189 | 190 | inline void ClampMin( float min ) { // Clamp to minimum 191 | if( x < min ) x = min; 192 | if( y < min ) y = min; 193 | if( z < min ) z = min; 194 | } 195 | 196 | 197 | inline void ClampMax( float max ) { // Clamp to maximum 198 | if( x > max ) x = max; 199 | if( y > max ) y = max; 200 | if( z > max ) z = max; 201 | } 202 | 203 | 204 | inline void Clamp( float min, float max ) { // Clamp to range ]min,max[ 205 | ClampMin( min ); 206 | ClampMax( max ); 207 | } 208 | 209 | 210 | // Interpolate between *this and v 211 | inline void Interpolate( const Vector3 &v, float a ) { 212 | float b( 1.0f - a ); 213 | 214 | x = b * x + a * v.x; 215 | y = b * y + a * v.y; 216 | z = b * z + a * v.z; 217 | } 218 | 219 | 220 | inline float operator | ( const Vector3 &v ) const { // Dot product 221 | return x * v.x + y * v.y + z * v.z; 222 | } 223 | 224 | 225 | inline Vector3 operator / ( float t ) const { // vector / float 226 | if( t == 0.0f ) 227 | return Vector3( 0, 0, 0 ); 228 | 229 | float s( 1.0f / t ); 230 | 231 | return Vector3( x * s, y * s, z * s ); 232 | } 233 | 234 | 235 | inline Vector3 operator + ( const Vector3 &b ) const { // vector + vector 236 | return Vector3( x + b.x, y + b.y, z + b.z ); 237 | } 238 | 239 | 240 | inline Vector3 operator - ( const Vector3 &b ) const { // vector - vector 241 | return Vector3( x - b.x, y - b.y, z - b.z ); 242 | } 243 | 244 | 245 | inline Vector3 operator * ( const Vector3 &b ) const { // vector * vector 246 | return Vector3( x * b.x, y * b.y, z * b.z ); 247 | } 248 | 249 | 250 | inline Vector3 operator ^ ( const Vector3 &b ) const { // cross(a,b) 251 | float nx, ny, nz; 252 | 253 | nx = y * b.z - z * b.y; 254 | ny = z * b.x - x * b.z; 255 | nz = x * b.y - y * b.x; 256 | 257 | return Vector3( nx, ny, nz ); 258 | } 259 | 260 | 261 | inline Vector3 operator * ( float s ) const { 262 | return Vector3( x * s, y * s, z * s ); 263 | } 264 | 265 | 266 | inline void Rotate( Vector3 &axis, float angle ) { 267 | float sa = (float)sin( angle ); 268 | float ca = (float)cos( angle ); 269 | Vector3 axis2( axis ); 270 | float m[9]; 271 | 272 | axis2.Normalize(); 273 | 274 | m[ 0 ] = ca + (1 - ca) * axis2.x * axis2.x; 275 | m[ 1 ] = (1 - ca) * axis2.x * axis2.y - sa * axis2.z; 276 | m[ 2 ] = (1 - ca) * axis2.z * axis2.x + sa * axis2.y; 277 | m[ 3 ] = (1 - ca) * axis2.x * axis2.y + sa * axis2.z; 278 | m[ 4 ] = ca + (1 - ca) * axis2.y * axis2.y; 279 | m[ 5 ] = (1 - ca) * axis2.y * axis2.z - sa * axis2.x; 280 | m[ 6 ] = (1 - ca) * axis2.z * axis2.x - sa * axis2.y; 281 | m[ 7 ] = (1 - ca) * axis2.y * axis2.z + sa * axis2.x; 282 | m[ 8 ] = ca + (1 - ca) * axis2.z * axis2.z; 283 | 284 | Vector3 src( *this ); 285 | 286 | x = m[0] * src.x + m[1] * src.y + m[2] * src.z; 287 | y = m[3] * src.x + m[4] * src.y + m[5] * src.z; 288 | z = m[6] * src.x + m[7] * src.y + m[8] * src.z; 289 | } 290 | 291 | inline operator float *() { 292 | return &this->x; 293 | } 294 | }; 295 | 296 | //------------------------------------------------------------------------------ 297 | // Copyright (C) 2013 Dan Royer (dan@marginallyclever.com) 298 | // Permission is hereby granted, free of charge, to any person obtaining a 299 | // copy of this software and associated documentation files (the "Software"), 300 | // to deal in the Software without restriction, including without limitation 301 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 302 | // and/or sell copies of the Software, and to permit persons to whom the 303 | // Software is furnished to do so, subject to the following conditions: 304 | // 305 | // The above copyright notice and this permission notice shall be included in 306 | // all copies or substantial portions of the Software. 307 | // 308 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 309 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 310 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 311 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 312 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 313 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 314 | // DEALINGS IN THE SOFTWARE. 315 | //------------------------------------------------------------------------------ 316 | #endif // VECTOR3_H 317 | -------------------------------------------------------------------------------- /RSPv1/RSPv1.ino: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------------ 2 | // Rotary Stewart Platform 3 | // dan@marginallycelver.com 2013-03-16 4 | //------------------------------------------------------------------------------ 5 | // Copyright at end of file. 6 | // please see http://www.github.com/MarginallyClever/RotaryStewartPlatform for more information. 7 | 8 | 9 | //------------------------------------------------------------------------------ 10 | // for duemilanove atmega328 arduino board + easydriver stepper controller 11 | // dan@marginallyclever.com 2011-06-21 12 | //------------------------------------------------------------------------------ 13 | #include 14 | #include "Vector3.h" 15 | 16 | 17 | //------------------------------------------------------------------------------ 18 | // constants 19 | //------------------------------------------------------------------------------ 20 | // Change this to your board type. 21 | #define BOARD_UNO 22 | //#define BOARD_MEGA 23 | 24 | #define TWOPI (PI*2.0f) 25 | #define DEG2RAD (PI/180.0f) 26 | #define RAD2DEG (180.0f/PI) 27 | 28 | #define BAUD (57600) 29 | #define MAX_BUF (64) 30 | 31 | 32 | #ifdef BOARD_UNO 33 | #define AXIS0 (11) 34 | #define AXIS1 (10) 35 | #define AXIS2 (9) 36 | #define AXIS3 (6) 37 | #define AXIS4 (5) 38 | #define AXIS5 (3) 39 | #endif 40 | #ifdef BOARD_MEGA 41 | #define AXIS0 (7) 42 | #define AXIS1 (6) 43 | #define AXIS2 (5) 44 | #define AXIS3 (4) 45 | #define AXIS4 (3) 46 | #define AXIS5 (2) 47 | #endif 48 | 49 | 50 | //------------------------------------------------------------------------------ 51 | struct RobotEffector { 52 | Vector3 up; 53 | Vector3 left; 54 | Vector3 forward; 55 | 56 | Vector3 pos; 57 | Vector3 relative; 58 | 59 | Vector3 angle; 60 | }; 61 | 62 | 63 | //------------------------------------------------------------------------------ 64 | struct RobotArm { 65 | Vector3 shoulder; 66 | Vector3 elbow; 67 | Vector3 elbow_relative; 68 | Vector3 wrist_pos; 69 | Vector3 wrist_relative; 70 | 71 | Servo s; 72 | float angle; 73 | int last_angle; 74 | // adjust for real world inaccuracies 75 | float scale; 76 | float zero; 77 | }; 78 | 79 | 80 | //------------------------------------------------------------------------------ 81 | class Hexapod { 82 | public: 83 | RobotArm arms[6]; 84 | RobotEffector endeffector; 85 | float default_height; 86 | 87 | public: 88 | void Setup(); 89 | }; 90 | 91 | 92 | //------------------------------------------------------------------------------ 93 | static float shoulder_to_elbow=1.6; // cm 94 | static float elbow_to_wrist=9.5; // cm 95 | 96 | // ee_to_w - end effector center to each wrist. 97 | // Imagine a plane that passes through all wrist joints, centered between them. 98 | // What is the X & Y on that plane to reach each a joint? 99 | // The software will flip & rotate to figure out the other 5. 100 | static float ee_to_w_y=0.4; 101 | static float ee_to_w_x=2.15; 102 | 103 | // c_to_s - base center to each shoulder (servo output gear). 104 | // Imagine a plane that passes through all shoulder joints, centered between them. 105 | // What is the X & Y on that plane to reach each a joint? 106 | // The software will flip & rotate to figure out the other 5. 107 | static float c_to_s_x=4.14; 108 | static float c_to_s_y=1.28; 109 | 110 | Hexapod hexapod; 111 | 112 | char buffer[MAX_BUF]; 113 | int sofar; 114 | 115 | 116 | //------------------------------------------------------------------------------ 117 | void Hexapod::Setup() { 118 | int i; 119 | float aa,bb,cc; 120 | Vector3 temp,n,ortho,t1,t2,t3; 121 | 122 | for(i=0;i<3;++i) { 123 | RobotArm &arma=this->arms[i*2+0]; 124 | RobotArm &armb=this->arms[i*2+1]; 125 | 126 | // find wrist position 127 | n.x=cos(i*TWOPI/3.0f); 128 | n.y=sin(i*TWOPI/3.0f); 129 | n.z=0; 130 | ortho.x=-n.y; 131 | ortho.y=n.x; 132 | ortho.z=0; 133 | arma.wrist_pos = n*ee_to_w_x + ortho*ee_to_w_y; 134 | armb.wrist_pos = n*ee_to_w_x - ortho*ee_to_w_y; 135 | arma.shoulder = n*c_to_s_x + ortho*c_to_s_y; 136 | armb.shoulder = n*c_to_s_x - ortho*c_to_s_y; 137 | arma.elbow = n*c_to_s_x + ortho*(c_to_s_y+shoulder_to_elbow); 138 | armb.elbow = n*c_to_s_x - ortho*(c_to_s_y+shoulder_to_elbow); 139 | 140 | arma.wrist_relative=arma.wrist_pos; 141 | armb.wrist_relative=armb.wrist_pos; 142 | 143 | // the tricky part is getting wrist_pos.z 144 | aa=(arma.elbow-arma.wrist_pos).Length(); 145 | cc=elbow_to_wrist; 146 | bb=sqrt((cc*cc)-(aa*aa)); 147 | arma.wrist_pos.z=bb; 148 | armb.wrist_pos.z=bb; 149 | 150 | arma.elbow_relative=arma.elbow-arma.shoulder; 151 | armb.elbow_relative=armb.elbow-armb.shoulder; 152 | } 153 | this->default_height=bb; 154 | this->endeffector.pos.z=0; 155 | this->endeffector.up.Set(0,0,1); 156 | this->endeffector.forward.Set(1,0,0); 157 | this->endeffector.left.Set(0,1,0); 158 | this->endeffector.angle.Set(0,0,0); 159 | 160 | this->arms[0].s.attach(AXIS0); 161 | this->arms[1].s.attach(AXIS1); 162 | this->arms[2].s.attach(AXIS2); 163 | this->arms[3].s.attach(AXIS3); 164 | this->arms[4].s.attach(AXIS4); 165 | this->arms[5].s.attach(AXIS5); 166 | this->arms[0].scale=-1; 167 | this->arms[1].scale=1; 168 | this->arms[2].scale=-1; 169 | this->arms[3].scale=1; 170 | this->arms[4].scale=-1; 171 | this->arms[5].scale=1; 172 | } 173 | 174 | 175 | //------------------------------------------------------------------------------ 176 | void line(float x,float y,float z,float a,float b,float c) { 177 | Vector3 axis; 178 | Vector3 target_pos(x,y,z); 179 | Vector3 target_ang(a,b,c); 180 | Vector3 start_pos = hexapod.endeffector.pos; 181 | Vector3 start_ang = hexapod.endeffector.angle; 182 | Vector3 da, dp, ortho, w, wop, temp, p1, p2,n,r; 183 | float r0,r1,d,hh,dt; 184 | int i; 185 | 186 | target_pos.z+=hexapod.default_height; 187 | start_pos.z+=hexapod.default_height; 188 | 189 | // @TODO: measure distance from start to destination 190 | // @TODO: divide by feed rate to get travel time 191 | float travel_time = 1.0; 192 | 193 | Serial.print(F("START\n")); 194 | 195 | float start=millis()*0.001; 196 | do { 197 | // Interpolate over travel time to move end effector 198 | float now = millis()*0.001; 199 | dt = ( now - start ) / travel_time; 200 | if(dt > 1) dt = 1; 201 | 202 | //Serial.print(now); 203 | //Serial.print(F(" - ")); 204 | //Serial.print(start); 205 | //Serial.print(F(" = ")); 206 | Serial.print(dt); 207 | Serial.print(F("\t")); 208 | 209 | hexapod.endeffector.pos = ( target_pos - start_pos ) * dt + start_pos; 210 | 211 | // roll pitch & yaw 212 | da = ( target_ang - start_ang ) * dt + start_ang; 213 | da *= DEG2RAD; 214 | 215 | hexapod.endeffector.up.Set(0,0,1); 216 | hexapod.endeffector.forward.Set(1,0,0); 217 | hexapod.endeffector.left.Set(0,1,0); 218 | 219 | // roll 220 | axis.Set(1,0,0); 221 | hexapod.endeffector.up.Rotate(axis,da.x); 222 | hexapod.endeffector.forward.Rotate(axis,da.x); 223 | hexapod.endeffector.left.Rotate(axis,da.x); 224 | 225 | // pitch 226 | axis.Set(0,1,0); 227 | hexapod.endeffector.up.Rotate(axis,da.y); 228 | hexapod.endeffector.forward.Rotate(axis,da.y); 229 | hexapod.endeffector.left.Rotate(axis,da.y); 230 | 231 | // yaw 232 | axis.Set(0,0,1); 233 | hexapod.endeffector.up.Rotate(axis,da.z); 234 | hexapod.endeffector.forward.Rotate(axis,da.z); 235 | hexapod.endeffector.left.Rotate(axis,da.z); 236 | 237 | 238 | // update wrist positions 239 | RobotEffector &ee = hexapod.endeffector; 240 | for(i=0;i<6;++i) { 241 | RobotArm &arm = hexapod.arms[i]; 242 | arm.wrist_pos = ee.pos + 243 | ee.forward * arm.wrist_relative.x + 244 | ee.left * arm.wrist_relative.y + 245 | ee.up * arm.wrist_relative.z; 246 | } 247 | 248 | // find shoulder angles & elbow positions 249 | for(i=0;i<6;++i) { 250 | RobotArm &arm=hexapod.arms[i]; 251 | 252 | // get wrist position on plane of bicep 253 | // @TODO: calculate once in setup 254 | ortho.x=cos((i>>1)*TWOPI/3.0f); 255 | ortho.y=sin((i>>1)*TWOPI/3.0f); 256 | ortho.z=0; 257 | 258 | w = arm.wrist_pos - arm.shoulder; 259 | 260 | a=w | ortho; //endeffector' distance 261 | wop = w - ortho * a; 262 | //arm.wop.pos=wop + arm.shoulder; // e' location 263 | //vector_add(arm.wop,wop,arm.shoulder); 264 | 265 | // because wop is projected onto the bicep plane, wop-elbow is not the same as wrist-elbow. 266 | // we need to find wop-elbow before we can calculate the angle at the shoulder. 267 | c=elbow_to_wrist; 268 | b=sqrt(c*c-a*a); // e'j distance 269 | 270 | // use intersection of circles to find elbow point. 271 | //a = (r0r0 - r1r1 + d*d ) / (2 d) 272 | r1=b; // circle 1 centers on e' 273 | r0=shoulder_to_elbow; // circle 0 centers on shoulder 274 | d=wop.Length(); 275 | // distance from shoulder to the midpoint between the two possible intersections 276 | a = ( r0 * r0 - r1 * r1 + d*d ) / ( 2*d ); 277 | // find the midpoint 278 | n=wop; 279 | n.Normalize(); 280 | 281 | temp=arm.shoulder+(n*a); 282 | 283 | // with a and r0 we can find h, the distance from midpoint to intersections. 284 | hh=sqrt(r0*r0-a*a); 285 | 286 | // get a normal to the line wop in the plane orthogonal to ortho 287 | r = ortho ^ n; 288 | p1 = temp + r * hh; 289 | p2 = temp - r * hh; 290 | 291 | if(i%2==0) arm.elbow=p1; 292 | else arm.elbow=p2; 293 | 294 | // use atan2 to find theta 295 | temp=arm.elbow-arm.shoulder; 296 | y=temp.z; 297 | temp.z=0; 298 | x=temp.Length(); 299 | 300 | if( ( arm.elbow_relative | temp ) < 0 ) x=-x; 301 | 302 | arm.angle=atan2(-y,x) * RAD2DEG; 303 | 304 | if(arm.angle<-90) arm.angle=-90; 305 | if(arm.angle> 90) arm.angle= 90; 306 | 307 | // we don't care about elbow angle. we could find it here if we needed it. 308 | 309 | // update servo 310 | Serial.print(arm.angle); 311 | Serial.print(F("\t")); 312 | 313 | arm.angle = (arm.angle * arm.scale) * (500.0f/90.0f) + 1500.0f; 314 | if(arm.last_angle!=arm.angle) { 315 | arm.s.writeMicroseconds(arm.angle); 316 | arm.last_angle=arm.angle; 317 | } 318 | } 319 | Serial.print(F("\n")); 320 | } while( dt < 1 ); 321 | 322 | hexapod.endeffector.pos = target_pos; 323 | hexapod.endeffector.pos.z -= hexapod.default_height; 324 | 325 | hexapod.endeffector.angle = target_ang; 326 | 327 | Serial.print(F("END\n")); 328 | } 329 | 330 | 331 | //------------------------------------------------------------------------------ 332 | void processCommand() { 333 | if(!strncmp(buffer,"M114",4)) { 334 | Serial.print(hexapod.endeffector.pos.x); Serial.print(F(", ")); 335 | Serial.print(hexapod.endeffector.pos.y); Serial.print(F(", ")); 336 | Serial.print(hexapod.endeffector.pos.z); Serial.print(F(" - ")); 337 | Serial.print(hexapod.endeffector.angle.x); Serial.print(F(", ")); 338 | Serial.print(hexapod.endeffector.angle.y); Serial.print(F(", ")); 339 | Serial.println(hexapod.endeffector.angle.z); 340 | } else if(!strncmp(buffer,"G00",3) || !strncmp(buffer,"G01",3)) { 341 | float xx=hexapod.endeffector.pos.x; 342 | float yy=hexapod.endeffector.pos.y; 343 | float zz=hexapod.endeffector.pos.z; 344 | float aa=hexapod.endeffector.angle.x; 345 | float bb=hexapod.endeffector.angle.y; 346 | float cc=hexapod.endeffector.angle.z; 347 | 348 | char *ptr=buffer; 349 | while(ptr && ptr 0) { 395 | buffer[sofar++]=Serial.read(); 396 | } 397 | 398 | if(sofar>0 && buffer[sofar-1]==';') { 399 | // what if message fails/garbled? 400 | 401 | // echo confirmation 402 | buffer[sofar]=0; 403 | Serial.println(buffer); 404 | 405 | processCommand(); 406 | 407 | sofar=0; 408 | 409 | // echo completion 410 | Serial.println("Done."); // @TODO: improve. 411 | } 412 | } 413 | 414 | 415 | 416 | //------------------------------------------------------------------------------ 417 | // Copyright (C) 2013 Dan Royer (dan@marginallyclever.com) 418 | // Permission is hereby granted, free of charge, to any person obtaining a 419 | // copy of this software and associated documentation files (the "Software"), 420 | // to deal in the Software without restriction, including without limitation 421 | // the rights to use, copy, modify, merge, publish, distribute, sublicense, 422 | // and/or sell copies of the Software, and to permit persons to whom the 423 | // Software is furnished to do so, subject to the following conditions: 424 | // 425 | // The above copyright notice and this permission notice shall be included in 426 | // all copies or substantial portions of the Software. 427 | // 428 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 429 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 430 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 431 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 432 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 433 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 434 | // DEALINGS IN THE SOFTWARE. 435 | //------------------------------------------------------------------------------ 436 | 437 | --------------------------------------------------------------------------------