├── SphereBot.pde ├── StepperModel.cpp ├── StepperModel.h ├── Utils ├── egg-displace.dat ├── feeder.py ├── feeder.tcl └── gcodeManage.py └── readme.txt /SphereBot.pde: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2011 by Eberhard Rensch 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see 16 | * 17 | * Part of this code is based on/inspired by the Helium Frog Delta Robot Firmware 18 | * by Martin Price 19 | * 20 | * !!!!!!!! 21 | * This sketch needs the following non-standard libraries (install them in the Arduino library directory): 22 | * SoftwareServo: http://www.arduino.cc/playground/ComponentLib/Servo 23 | * TimerOne: http://www.arduino.cc/playground/Code/Timer1 24 | * !!!!!!!! 25 | */ 26 | 27 | #include 28 | #include 29 | #include "StepperModel.h" 30 | 31 | 32 | #define TIMER_DELAY 64 33 | 34 | /* 35 | * PINS 36 | */ 37 | 38 | #define XAXIS_DIR_PIN 7 39 | #define XAXIS_STEP_PIN 8 40 | #define XAXIS_ENABLE_PIN 6 41 | #define XAXIS_ENDSTOP_PIN 3 42 | 43 | #define YAXIS_DIR_PIN 10 44 | #define YAXIS_STEP_PIN 11 45 | #define YAXIS_ENABLE_PIN 9 46 | #define YAXIS_ENDSTOP_PIN -1 // <0 0> No Endstop! 47 | 48 | #define SERVO_PIN 2 49 | 50 | /* 51 | * Other Configuration 52 | */ 53 | 54 | #define DEFAULT_PEN_UP_POSITION 50 55 | #define XAXIS_MIN_STEPCOUNT -467 56 | #define XAXIS_MAX_STEPCOUNT 467 57 | #define DEFAULT_ZOOM_FACTOR 1. // With a Zoom-Faktor of .65, I can print gcode for Makerbot Unicorn without changes. 58 | // The zoom factor can be also manipulated by the propretiary code M402 59 | 60 | 61 | /* --------- */ 62 | 63 | StepperModel xAxisStepper(XAXIS_DIR_PIN, XAXIS_STEP_PIN, XAXIS_ENABLE_PIN, XAXIS_ENDSTOP_PIN, 64 | XAXIS_MIN_STEPCOUNT, XAXIS_MAX_STEPCOUNT, 200.0, 16); 65 | StepperModel rotationStepper(YAXIS_DIR_PIN, YAXIS_STEP_PIN, YAXIS_ENABLE_PIN, YAXIS_ENDSTOP_PIN, 66 | 0, 0, 200.0, 16); 67 | 68 | SoftwareServo servo; 69 | boolean servoEnabled=true; 70 | 71 | long intervals=0; 72 | volatile long intervals_remaining=0; 73 | volatile boolean isRunning=false; 74 | 75 | // comm variables 76 | const int MAX_CMD_SIZE = 256; 77 | char buffer[MAX_CMD_SIZE]; // buffer for serial commands 78 | char serial_char; // value for each byte read in from serial comms 79 | int serial_count = 0; // current length of command 80 | char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc 81 | boolean comment_mode = false; 82 | // end comm variables 83 | 84 | // GCode States 85 | double currentOffsetX = 0.; 86 | double currentOffsetY = 0.; 87 | boolean absoluteMode = true; 88 | double feedrate = 2000.; // mm/minute 89 | double zoom = DEFAULT_ZOOM_FACTOR; 90 | 91 | const double maxFeedrate = 6000.; 92 | // ------ 93 | 94 | void setup() 95 | { 96 | Serial.begin(115200); 97 | 98 | clear_buffer(); 99 | 100 | servo.attach(SERVO_PIN); 101 | servo.write(DEFAULT_PEN_UP_POSITION); 102 | 103 | if(servoEnabled) 104 | { 105 | for(int i=0;i<100;i++) 106 | { 107 | SoftwareServo::refresh(); 108 | delay(4); 109 | } 110 | } 111 | 112 | //--- Activate the PWM timer 113 | Timer1.initialize(TIMER_DELAY); // Timer for updating pwm pins 114 | Timer1.attachInterrupt(doInterrupt); 115 | 116 | #ifdef AUTO_HOMING 117 | xAxisStepper.autoHoming(); 118 | xAxisStepper.setTargetPosition(0.); 119 | commitSteppers(maxFeedrate); 120 | delay(2000); 121 | xAxisStepper.enableStepper(false); 122 | #endif 123 | } 124 | 125 | void loop() // input loop, looks for manual input and then checks to see if and serial commands are coming in 126 | { 127 | get_command(); // check for Gcodes 128 | if(servoEnabled) 129 | SoftwareServo::refresh(); 130 | } 131 | 132 | //--- Interrupt-Routine: Move the steppers 133 | void doInterrupt() 134 | { 135 | if(isRunning) 136 | { 137 | if (intervals_remaining-- == 0) 138 | isRunning = false; 139 | else 140 | { 141 | rotationStepper.doStep(intervals); 142 | xAxisStepper.doStep(intervals); 143 | } 144 | } 145 | } 146 | 147 | void commitSteppers(double speedrate) 148 | { 149 | long deltaStepsX = xAxisStepper.delta; 150 | if(deltaStepsX != 0L) 151 | { 152 | xAxisStepper.enableStepper(true); 153 | } 154 | 155 | long deltaStepsY = rotationStepper.delta; 156 | if(deltaStepsY != 0L) 157 | { 158 | rotationStepper.enableStepper(true); 159 | } 160 | long masterSteps = (deltaStepsX>deltaStepsY)?deltaStepsX:deltaStepsY; 161 | 162 | double deltaDistanceX = xAxisStepper.targetPosition-xAxisStepper.getCurrentPosition(); 163 | double deltaDistanceY = rotationStepper.targetPosition-rotationStepper.getCurrentPosition(); 164 | 165 | // how long is our line length? 166 | double distance = sqrt(deltaDistanceX*deltaDistanceX+deltaDistanceY*deltaDistanceY); 167 | 168 | // compute number of intervals for this move 169 | double sub1 = (60000.* distance / speedrate); 170 | double sub2 = sub1 * 1000.; 171 | intervals = (long)sub2/TIMER_DELAY; 172 | 173 | intervals_remaining = intervals; 174 | const long negative_half_interval = -intervals / 2; 175 | 176 | rotationStepper.counter = negative_half_interval; 177 | xAxisStepper.counter = negative_half_interval; 178 | 179 | // Serial.print("Speedrate:"); 180 | // Serial.print(speedrate, 6); 181 | // Serial.print(" dX:"); 182 | // Serial.print(deltaStepsX); 183 | // Serial.print(" dY:"); 184 | // Serial.print(deltaStepsY); 185 | // Serial.print(" masterSteps:"); 186 | // Serial.print(masterSteps); 187 | // Serial.print(" dDistX:"); 188 | // Serial.print(deltaDistanceX); 189 | // Serial.print(" dDistY:"); 190 | // Serial.print(deltaDistanceY); 191 | // Serial.print(" distance:"); 192 | // Serial.print(distance); 193 | // Serial.print(" sub1:"); 194 | // Serial.print(sub1, 6); 195 | // Serial.print(" sub2:"); 196 | // Serial.print(sub2, 6); 197 | // Serial.print(" intervals:"); 198 | // Serial.print(intervals); 199 | // Serial.print(" negative_half_interval:"); 200 | // Serial.println(negative_half_interval); 201 | // Serial.print("Y currentStepCount:"); 202 | // Serial.print(rotationStepper.currentStepcount); 203 | // Serial.print(" targetStepCount:"); 204 | // Serial.println(rotationStepper.targetStepcount); 205 | 206 | isRunning=true; 207 | } 208 | 209 | void get_command() // gets commands from serial connection and then calls up subsequent functions to deal with them 210 | { 211 | if (!isRunning && Serial.available() > 0) // each time we see something 212 | { 213 | serial_char = Serial.read(); // read individual byte from serial connection 214 | 215 | if (serial_char == '\n' || serial_char == '\r') // end of a command character 216 | { 217 | buffer[serial_count]=0; 218 | process_commands(buffer, serial_count); 219 | clear_buffer(); 220 | comment_mode = false; // reset comment mode before each new command is processed 221 | } 222 | else // not end of command 223 | { 224 | if (serial_char == ';' || serial_char == '(') // semicolon signifies start of comment 225 | { 226 | comment_mode = true; 227 | } 228 | 229 | if (comment_mode != true) // ignore if a comment has started 230 | { 231 | buffer[serial_count] = serial_char; // add byte to buffer string 232 | serial_count++; 233 | if (serial_count > MAX_CMD_SIZE) // overflow, dump and restart 234 | { 235 | clear_buffer(); 236 | Serial.flush(); 237 | } 238 | } 239 | } 240 | } 241 | } 242 | 243 | void clear_buffer() // empties command buffer from serial connection 244 | { 245 | serial_count = 0; // reset buffer placement 246 | } 247 | 248 | boolean getValue(char key, char command[], double* value) 249 | { 250 | // find key parameter 251 | strchr_pointer = strchr(buffer, key); 252 | if (strchr_pointer != NULL) // We found a key value 253 | { 254 | *value = (double)strtod(&command[strchr_pointer - command + 1], NULL); 255 | return true; 256 | } 257 | return false; 258 | } 259 | 260 | void process_commands(char command[], int command_length) // deals with standardized input from serial connection 261 | { 262 | if (command_length>0 && command[0] == 'G') // G code 263 | { 264 | int codenum = (int)strtod(&command[1], NULL); 265 | 266 | double tempX = xAxisStepper.getCurrentPosition(); 267 | double tempY = rotationStepper.getCurrentPosition(); 268 | 269 | double xVal; 270 | boolean hasXVal = getValue('X', command, &xVal); 271 | if(hasXVal) xVal*=zoom; 272 | double yVal; 273 | boolean hasYVal = getValue('Y', command, &yVal); 274 | if(hasYVal) yVal*=zoom; 275 | double iVal; 276 | boolean hasIVal = getValue('I', command, &iVal); 277 | if(hasIVal) iVal*=zoom; 278 | double jVal; 279 | boolean hasJVal = getValue('J', command, &jVal); 280 | if(hasJVal) jVal*=zoom; 281 | double rVal; 282 | boolean hasRVal = getValue('R', command, &rVal); 283 | if(hasRVal) rVal*=zoom; 284 | double pVal; 285 | boolean hasPVal = getValue('P', command, &pVal); 286 | 287 | getValue('F', command, &feedrate); 288 | 289 | xVal+=currentOffsetX; 290 | yVal+=currentOffsetY; 291 | 292 | if(absoluteMode) 293 | { 294 | if(hasXVal) 295 | tempX=xVal; 296 | if(hasYVal) 297 | tempY=yVal; 298 | } 299 | else 300 | { 301 | if(hasXVal) 302 | tempX+=xVal; 303 | if(hasYVal) 304 | tempY+=yVal; 305 | } 306 | 307 | switch(codenum) 308 | { 309 | case 0: // G0, Rapid positioning 310 | xAxisStepper.setTargetPosition(tempX); 311 | rotationStepper.setTargetPosition(tempY); 312 | commitSteppers(maxFeedrate); 313 | break; 314 | case 1: // G1, linear interpolation at specified speed 315 | xAxisStepper.setTargetPosition(tempX); 316 | rotationStepper.setTargetPosition(tempY); 317 | commitSteppers(feedrate); 318 | break; 319 | case 2: // G2, Clockwise arc 320 | case 3: // G3, Counterclockwise arc 321 | if(hasIVal && hasJVal) 322 | { 323 | double centerX=xAxisStepper.getCurrentPosition()+iVal; 324 | double centerY=rotationStepper.getCurrentPosition()+jVal; 325 | drawArc(centerX, centerY, tempX, tempY, (codenum==2)); 326 | } 327 | else if(hasRVal) 328 | { 329 | //drawRadius(tempX, tempY, rVal, (codenum==2)); 330 | } 331 | break; 332 | case 4: // G4, Delay P ms 333 | if(hasPVal) 334 | { 335 | unsigned long endDelay = millis()+ (unsigned long)pVal; 336 | while(millis()0 && command[0] == 'M') // M code 353 | { 354 | double value; 355 | int codenum = (int)strtod(&command[1], NULL); 356 | switch(codenum) 357 | { 358 | case 18: // Disable Drives 359 | xAxisStepper.resetStepper(); 360 | rotationStepper.resetStepper(); 361 | break; 362 | 363 | case 300: // Servo Position 364 | if(getValue('S', command, &value)) 365 | { 366 | servoEnabled=true; 367 | if(value<0.) 368 | value=0.; 369 | else if(value>180.) 370 | { 371 | value=DEFAULT_PEN_UP_POSITION; 372 | servo.write((int)value); 373 | for(int i=0;i<100;i++) 374 | { 375 | SoftwareServo::refresh(); 376 | delay(4); 377 | } 378 | servoEnabled=false; 379 | } 380 | servo.write((int)value); 381 | } 382 | break; 383 | 384 | case 400: // Propretary: Reset X-Axis-Stepper settings to new object diameter 385 | if(getValue('S', command, &value)) 386 | { 387 | xAxisStepper.resetSteppersForObjectDiameter(value); 388 | xAxisStepper.setTargetPosition(0.); 389 | commitSteppers(maxFeedrate); 390 | delay(2000); 391 | xAxisStepper.enableStepper(false); 392 | } 393 | break; 394 | 395 | case 401: // Propretary: Reset Y-Axis-Stepper settings to new object diameter 396 | if(getValue('S', command, &value)) 397 | { 398 | rotationStepper.resetSteppersForObjectDiameter(value); 399 | rotationStepper.setTargetPosition(0.); 400 | commitSteppers(maxFeedrate); 401 | delay(2000); 402 | rotationStepper.enableStepper(false); 403 | } 404 | break; 405 | 406 | case 402: // Propretary: Set global zoom factor 407 | if(getValue('S', command, &value)) 408 | { 409 | zoom = value; 410 | } 411 | 412 | } 413 | } 414 | 415 | // done processing commands 416 | if (Serial.available() <= 0) { 417 | Serial.print("ok:"); 418 | Serial.println(command); 419 | } 420 | } 421 | 422 | /* This code was ported from the Makerbot/ReplicatorG java sources */ 423 | void drawArc(double centerX, double centerY, double endpointX, double endpointY, boolean clockwise) 424 | { 425 | // angle variables. 426 | double angleA; 427 | double angleB; 428 | double angle; 429 | double radius; 430 | double length; 431 | 432 | // delta variables. 433 | double aX; 434 | double aY; 435 | double bX; 436 | double bY; 437 | 438 | // figure out our deltas 439 | double currentX = xAxisStepper.getCurrentPosition(); 440 | double currentY = rotationStepper.getCurrentPosition(); 441 | aX = currentX - centerX; 442 | aY = currentY - centerY; 443 | bX = endpointX - centerX; 444 | bY = endpointY - centerY; 445 | 446 | // Clockwise 447 | if (clockwise) { 448 | angleA = atan2(bY, bX); 449 | angleB = atan2(aY, aX); 450 | } 451 | // Counterclockwise 452 | else { 453 | angleA = atan2(aY, aX); 454 | angleB = atan2(bY, bX); 455 | } 456 | 457 | // Make sure angleB is always greater than angleA 458 | // and if not add 2PI so that it is (this also takes 459 | // care of the special case of angleA == angleB, 460 | // ie we want a complete circle) 461 | if (angleB <= angleA) 462 | angleB += 2. * M_PI; 463 | angle = angleB - angleA; 464 | 465 | // calculate a couple useful things. 466 | radius = sqrt(aX * aX + aY * aY); 467 | length = radius * angle; 468 | 469 | // for doing the actual move. 470 | int steps; 471 | int s; 472 | int step; 473 | 474 | // Maximum of either 2.4 times the angle in radians 475 | // or the length of the curve divided by the curve section constant 476 | steps = (int)ceil(max(angle * 2.4, length)); 477 | 478 | // this is the real draw action. 479 | double newPointX = 0.; 480 | double newPointY = 0.; 481 | 482 | for (s = 1; s <= steps; s++) { 483 | // Forwards for CCW, backwards for CW 484 | if (!clockwise) 485 | step = s; 486 | else 487 | step = steps - s; 488 | 489 | // calculate our waypoint. 490 | newPointX = centerX + radius * cos(angleA + angle * ((double) step / steps)); 491 | newPointY= centerY + radius * sin(angleA + angle * ((double) step / steps)); 492 | 493 | // start the move 494 | xAxisStepper.setTargetPosition(newPointX); 495 | rotationStepper.setTargetPosition(newPointY); 496 | commitSteppers(feedrate); 497 | 498 | while(isRunning) 499 | { 500 | delay(1); 501 | if(servoEnabled) 502 | SoftwareServo::refresh(); 503 | }; 504 | } 505 | } 506 | 507 | /* Make life easier for vim users */ 508 | /* vim:set filetype=cpp: */ 509 | -------------------------------------------------------------------------------- /StepperModel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2011 by Eberhard Rensch 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see 16 | */ 17 | 18 | #include "StepperModel.h" 19 | #include "WProgram.h" 20 | 21 | 22 | /* 23 | * inEnablePin < 0 => No Endstop 24 | */ 25 | StepperModel::StepperModel(int inDirPin, int inStepPin, int inEnablePin, int inEndStopPin, 26 | long minSC, long maxSC, 27 | double in_kStepsPerRevolution, int in_kMicroStepping) 28 | { 29 | kStepsPerRevolution=in_kStepsPerRevolution; 30 | kMicroStepping=in_kMicroStepping; 31 | 32 | dirPin = inDirPin; 33 | stepPin = inStepPin; 34 | enablePin = inEnablePin; 35 | endStopPin = inEndStopPin; 36 | 37 | minStepCount=minSC; 38 | maxStepCount=maxSC; 39 | 40 | pinMode(dirPin, OUTPUT); 41 | pinMode(stepPin, OUTPUT); 42 | pinMode(enablePin, OUTPUT); 43 | if(endStopPin>=0) 44 | pinMode(endStopPin, INPUT); 45 | 46 | digitalWrite(dirPin, LOW); 47 | digitalWrite(stepPin, LOW); 48 | 49 | currentStepcount=0; 50 | targetStepcount=0; 51 | 52 | steps_per_mm = (int)((kStepsPerRevolution/(45.*M_PI))*kMicroStepping+0.5); // default value for a "normal" egg (45 mm diameter) 53 | enableStepper(false); 54 | } 55 | 56 | void StepperModel::resetSteppersForObjectDiameter(double diameter) 57 | { 58 | // Calculate the motor steps required to move per mm. 59 | steps_per_mm = (int)((kStepsPerRevolution/(diameter*M_PI))*kMicroStepping+0.5); 60 | if(endStopPin>=0) 61 | { 62 | #ifdef AUTO_HOMING 63 | autoHoming(); 64 | #endif 65 | enableStepper(false); 66 | } 67 | else 68 | resetStepper(); 69 | } 70 | 71 | long StepperModel::getStepsForMM(double mm) 72 | { 73 | long steps = (long)(steps_per_mm*mm); 74 | 75 | // Serial.print("steps for "); 76 | // Serial.print(mm); 77 | // Serial.print(" mm: "); 78 | // Serial.println(steps); 79 | 80 | return steps; 81 | } 82 | 83 | /* Currently unused */ 84 | /* 85 | void StepperModel::setTargetStepcount(long tsc) 86 | { 87 | targetPosition = (double)tsc/steps_per_mm; 88 | targetStepcount = tsc; 89 | delta = targetStepcount-currentStepcount; 90 | direction = true; 91 | if (delta != 0) { 92 | enableStepper(true); 93 | } 94 | if (delta < 0) { 95 | delta = -delta; 96 | direction = false; 97 | } 98 | }*/ 99 | 100 | void StepperModel::setTargetPosition(double pos) 101 | { 102 | targetPosition = pos; 103 | targetStepcount = getStepsForMM(targetPosition); 104 | //Serial.println(targetStepcount); 105 | delta = targetStepcount-currentStepcount; 106 | direction = true; 107 | if (delta != 0) { 108 | enableStepper(true); 109 | } 110 | if (delta < 0) { 111 | delta = -delta; 112 | direction = false; 113 | } 114 | } 115 | 116 | double StepperModel::getCurrentPosition() 117 | { 118 | return (double)currentStepcount/steps_per_mm; 119 | } 120 | 121 | void StepperModel::enableStepper(bool enabled) 122 | { 123 | digitalWrite(enablePin, !enabled); 124 | } 125 | 126 | void StepperModel::resetStepper() 127 | { 128 | enableStepper(false); 129 | currentStepcount=0; 130 | targetStepcount=0; 131 | delta=0; 132 | } 133 | 134 | void StepperModel::doStep(long intervals) 135 | { 136 | counter += delta; 137 | if (counter >= 0) { 138 | digitalWrite(dirPin, direction?HIGH:LOW); 139 | counter -= intervals; 140 | if (direction) { 141 | if(maxStepCount==0 || currentStepcount<=maxStepCount) 142 | { 143 | digitalWrite(stepPin, HIGH); 144 | currentStepcount++; 145 | } 146 | } else { 147 | if(minStepCount==0 || currentStepcount>=minStepCount) 148 | { 149 | digitalWrite(stepPin, HIGH); 150 | currentStepcount--; 151 | } 152 | } 153 | digitalWrite(stepPin, LOW); 154 | } 155 | } 156 | 157 | #ifdef AUTO_HOMING 158 | void StepperModel::autoHoming() 159 | { 160 | enableStepper(true); 161 | digitalWrite(dirPin, LOW); 162 | 163 | while(digitalRead(endStopPin)) 164 | { 165 | digitalWrite(stepPin, HIGH); 166 | digitalWrite(stepPin, LOW); 167 | delay(1); 168 | } 169 | 170 | currentStepcount= minStepCount-16; 171 | } 172 | #endif 173 | -------------------------------------------------------------------------------- /StepperModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2011 by Eberhard Rensch 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see 16 | */ 17 | 18 | #ifndef STEPPERMODEL 19 | #define STEPPERMODEL 20 | 21 | // Uncomment if You have Autohoming: 22 | //#define AUTO_HOMING 23 | 24 | class StepperModel 25 | { 26 | private: 27 | 28 | int dirPin; 29 | int stepPin; 30 | int enablePin; 31 | int endStopPin; 32 | 33 | long minStepCount; 34 | long maxStepCount; 35 | double steps_per_mm; 36 | 37 | double kStepsPerRevolution; 38 | int kMicroStepping; 39 | 40 | volatile long currentStepcount; 41 | volatile long targetStepcount; 42 | 43 | volatile bool direction; 44 | 45 | long getStepsForMM(double mm); 46 | 47 | public: 48 | volatile long delta; 49 | volatile long counter; 50 | double targetPosition; 51 | 52 | StepperModel(int inDirPin, int inStepPin, int inEnablePin, int inEndStopPin, 53 | long minSC, long maxSC, 54 | double in_kStepsPerRevolution, int in_kMicroStepping 55 | ); 56 | 57 | void resetSteppersForObjectDiameter(double diameter); 58 | 59 | #ifdef AUTO_HOMING 60 | void autoHoming(); 61 | #endif 62 | 63 | 64 | void setTargetPosition(double pos); 65 | double getCurrentPosition(); 66 | 67 | void enableStepper(bool enabled); 68 | 69 | void resetStepper(); 70 | 71 | void doStep(long intervals); 72 | }; 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /Utils/egg-displace.dat: -------------------------------------------------------------------------------- 1 | # Gnuplot Session 2 | # 3 | # Erste Spalte=x, Zweite=y 4 | # Ei Spitze zeigt nach rechts (positive x) 5 | # 6 | # plot "egg-displace.dat" using 1:2 title "Ei" 7 | # ==> graph looks like a parabola 8 | # f(x)=a*x*x + b*x + c 9 | # fit f(x) "egg-displace.dat" using 1:2 via a,b,c 10 | # a=0.00795338 11 | # b=0.0734545 12 | # c=0.15711 13 | # plot "egg-displace.dat" using 1:2 title "Ei" with lines, f(x) 14 | # ==> fits reasonable well 15 | -25 3 16 | -20 2 17 | -15 1 18 | -10 0.5 19 | -5 0.1 20 | 0 0 21 | 5 0.5 22 | 10 1.5 23 | 15 3 24 | 20 5 25 | 25 7 26 | -------------------------------------------------------------------------------- /Utils/feeder.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Feeds a gcode File to the SphereBot. 4 | # 5 | # Write a line to the serial device and wait for an "ok" response. 6 | 7 | # prerequisite: http://pyserial.sourceforge.net 8 | # Installation on Ubuntu: sudo aptitude install python-serial 9 | 10 | 11 | 12 | 13 | # Configure: 14 | BAUDRATE = 57600 15 | DEVICE = "/dev/ttyUSB1" 16 | DEVICE = "/dev/tty.PL2303-00001004" 17 | DEVICE = "/dev/tty.PL2303-00004006" 18 | 19 | # End configuration 20 | 21 | 22 | 23 | import sys 24 | import serial 25 | import re 26 | from optparse import OptionParser 27 | 28 | def y_displacement(x): 29 | # look into file egg-displace.dat for documentation 30 | return (0.00795338*x*x + 0.0734545*x + 0.15711) 31 | 32 | lastX = 0.0 33 | 34 | def correctDisplacement(lineIn): 35 | # extract x and y 36 | # calculate new y 37 | # return line with alter y 38 | 39 | global lastX 40 | foundY = False 41 | 42 | line = lineIn.upper() 43 | words = pattern.findall(line) 44 | for word in words: 45 | if word[0] == 'X': 46 | lastX = eval(word[1:]) 47 | 48 | if word[0] == 'Y': 49 | y = eval(word[1:]) 50 | foundY=True 51 | 52 | if foundY: 53 | y = y + y_displacement(lastX) 54 | else: 55 | return lineIn 56 | 57 | lineOut="" 58 | for word in words: 59 | if word[0] == 'Y': 60 | lineOut = lineOut + "Y{0}".format(y) 61 | else: 62 | lineOut = lineOut + word 63 | 64 | return lineOut 65 | 66 | def penChange(lineIn): 67 | # Test Line for a Pen change request (M1) 68 | # If true, wait for user input 69 | 70 | if penChangePattern.match(lineIn): 71 | raw_input('Change pen ... press when finished ') 72 | 73 | 74 | ######################## Main ######################### 75 | 76 | parser = OptionParser(usage="usage: %prog [options] gcode-file") 77 | parser.add_option("-e", "--egg-displace", dest="wantDisplaceCorrection", 78 | action="store_true", default=False, 79 | help="Correct displacement if drawn on a egg. The tip of the egg is pointing right hand.") 80 | parser.add_option("-d", "--dont-send", dest="wantToSend", 81 | action="store_false", default=True, 82 | help="Dont send GCode to SphereBot") 83 | 84 | (options, args) = parser.parse_args() 85 | 86 | 87 | 88 | if len(args) != 1: 89 | parser.error("incorrect number of arguments: need one gcode file to send to the sphereBot!") 90 | 91 | 92 | if options.wantDisplaceCorrection: 93 | pattern = re.compile('([(!;].*|\s+|[a-zA-Z0-9_:](?:[+-])?\d*(?:\.\d*)?|\w\#\d+|\(.*?\)|\#\d+\=(?:[+-])?\d*(?:\.\d*)?)') 94 | 95 | penChangePattern = re.compile('^M01') 96 | 97 | fileToFeed = args[0] 98 | gcode = open(fileToFeed, "r") 99 | 100 | if options.wantToSend: 101 | sphereBot = serial.Serial(DEVICE, BAUDRATE, timeout=30) 102 | 103 | currentLine = 0.0 104 | lines = gcode.readlines() 105 | totalLines = len(lines) 106 | for line in lines: 107 | currentLine = currentLine + 1 108 | 109 | print line, "({0:.1f}%)".format((currentLine / totalLines)*100), 110 | 111 | penChange(line) 112 | 113 | if options.wantDisplaceCorrection: 114 | line = correctDisplacement(line) 115 | print ">> ", line, 116 | 117 | 118 | if options.wantToSend: 119 | sphereBot.write(line) 120 | 121 | response = sphereBot.readline() 122 | while response[:3] != "ok:": 123 | print " ", response, 124 | response = sphereBot.readline() 125 | 126 | -------------------------------------------------------------------------------- /Utils/feeder.tcl: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | ## \ 3 | exec tclsh "$0" ${1+"$@"} 4 | 5 | # Feeds a gcode File to the SphereBot. 6 | # 7 | # Write a line to the serial device and wait for an "ok" response. 8 | # 9 | # Configure: 10 | 11 | set BAUDRATE 57600 12 | set DEVICE /dev/ttyUSB1 13 | 14 | 15 | # End configuration 16 | 17 | 18 | set fileToFeed [lindex $argv 0] 19 | 20 | 21 | set fdIn [open $fileToFeed] 22 | set fdOut [open $DEVICE r+] 23 | fconfigure $fdOut -mode $BAUDRATE,n,8,1 24 | 25 | 26 | while {[gets $fdIn line] >= 0} { 27 | puts $line 28 | puts $fdOut $line 29 | flush $fdOut 30 | while {[gets $fdOut answer]} { 31 | puts "\t$answer" 32 | if {[regexp {^ok:} $answer]} { 33 | break 34 | } 35 | } 36 | } 37 | 38 | close $fdIn 39 | close $fdOut 40 | -------------------------------------------------------------------------------- /Utils/gcodeManage.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Some usfull gcode info - manipulation functions 4 | # 5 | 6 | # prerequisite: http://pyserial.sourceforge.net 7 | # Installation on Ubuntu: sudo aptitude install python-serial 8 | 9 | 10 | 11 | 12 | # Configure: 13 | 14 | # End configuration 15 | 16 | 17 | 18 | import sys 19 | import re 20 | import serial 21 | 22 | class Range: 23 | def __init__(self): 24 | self.min = 999999 25 | self.max = -999999 26 | 27 | def __str__(self): 28 | return 'Range({self.min}, {self.max})'.format(self=self) 29 | 30 | def adjust(self, value): 31 | """ 32 | If value is out of range expand range 33 | """ 34 | if value > self.max: 35 | self.max = value 36 | if value < self.min: 37 | self.min = value 38 | 39 | 40 | fileToFeed = sys.argv[1] 41 | gcode = open(fileToFeed, "r") 42 | 43 | 44 | xRange = Range() 45 | yRange = Range() 46 | pattern = re.compile('([(!;].*|\s+|[a-zA-Z0-9_:](?:[+-])?\d*(?:\.\d*)?|\w\#\d+|\(.*?\)|\#\d+\=(?:[+-])?\d*(?:\.\d*)?)') 47 | 48 | lines = gcode.readlines() 49 | for line in lines: 50 | line = line.lower() 51 | words = pattern.findall(line) 52 | for word in words: 53 | if word[0] == 'x': 54 | x = eval(word[1:]) 55 | xRange.adjust(x) 56 | if word[0] == 'y': 57 | y = eval(word[1:]) 58 | yRange.adjust(y) 59 | 60 | print "x=", xRange, " y=",yRange 61 | -------------------------------------------------------------------------------- /readme.txt: -------------------------------------------------------------------------------- 1 | v0.1 2 | 3 | This is the firmware for an EggBot-style SphereBot. 4 | The firmware directly interprets GCode send over the serial port. 5 | 6 | There will be more information on the SphereBot at http://pleasantsoftware.com/developer/3d (in the near future...) 7 | 8 | !!!!!!!! 9 | This sketch needs the following non-standard libraries (install them in the Arduino library directory): 10 | SoftwareServo: http://www.arduino.cc/playground/ComponentLib/Servo 11 | TimerOne: http://www.arduino.cc/playground/Code/Timer1 12 | !!!!!!!! 13 | 14 | Copyright 2011 by Eberhard Rensch 15 | 16 | This program is free software: you can redistribute it and/or modify 17 | it under the terms of the GNU General Public License as published by 18 | the Free Software Foundation, either version 3 of the License, or 19 | (at your option) any later version. 20 | 21 | This program is distributed in the hope that it will be useful, 22 | but WITHOUT ANY WARRANTY; without even the implied warranty of 23 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 | GNU General Public License for more details. 25 | 26 | You should have received a copy of the GNU General Public License 27 | along with this program. If not, see 28 | 29 | Part of this code is based on/inspired by the Helium Frog Delta Robot Firmware 30 | by Martin Price 31 | 32 | To create tags File: ctags --langmap="C++:+.pde" S* 33 | 34 | 35 | GCode commands: 36 | 37 | G90 Absolut modus 38 | G91 Icremental modus: 39 | 40 | M300S0 Servo 0 degree 41 | M300S90 Servo 90 degree 42 | 43 | 44 | M18 Stepper off 45 | 46 | G0X0Y40 Rapid movement (pen 0mm, rotation 40mm) 47 | G1X40Y0 Slower movement (pen 40mm, rotation 0mm) 48 | G1Y10F660 Movement with feed 660mm/s (rotation 10mm) 49 | 50 | ; Comment 51 | ( .. ) Comment 52 | --------------------------------------------------------------------------------