Arduino Sand Table: Creating Hypnotic Trinity Designs

Kinetic sand table powered by Arduino

Jun 5, 2024

•

12444 views

•

9 respects

•

Arduino User Group

Entertainment System

Components and supplies

Arduino Uno Rev3

CNC Shield V3

Apps and platforms

Arduino IDE 2.0 (beta)

Project description

Code

sand table code

cpp

Arduino Code

1#include <math.h>
2// Pins
3
4
5// circle size radius = 100 units
6
7#define enPin_rot 12      // Stepper motor enable pin
8#define lights_pin 11      // Stepper motor enable pin
9#define stepPin_rot 2    // X axis step pin
10#define dirPin_rot 5     // X axis direction pin
11#define enPin_InOut 8    // Stepper motor enable pin
12#define stepPin_InOut 3  // Y axis step pin
13#define dirPin_InOut 6   // Y axis direction pin
14#define PI 3.1415926535897932384626433832795
15
16
17// Changable variables.
18#define rot_total_steps 16000.0
19#define inOut_total_steps 5000.0
20#define desired_scale 100.0
21
22//int knob1 = analogRead(A1);
23int knob1 = 0;
24int knob2 = 0;
25
26
27volatile float r = 0;  // length of extended arm
28volatile float x = 0;
29volatile float y = 0;  // current location of ball
30
31volatile float x2 = 0;
32volatile float y2 = 0;
33
34//int relStep=0;
35
36float xLoc = 0;
37float yLoc = 0;
38
39int offset = 0;
40int moving = 0;
41
42//volatile float xTarget = 0;
43//volatile float yTarget = 0;  // current location of ball
44
45
46
47float radAngle = 0;
48float rotAngle = 0;
49
50
51int inOutSteps = 0;  /// Y
52int inOutStepsTo = 0;
53int rotSteps = 0;  //  X
54int rotStepsTo = 0;
55byte rotON = 0;
56byte inOutON = 1;
57
58
59
60// Variables to store results
61float rTo, angleTo;
62//float targetX = 0;  // Default target coordinates
63//float targetY = 0;
64
65
66
67//int xAng = 0;
68
69int pwm = 0;
70int pwm2 = 0;
71
72byte inOut = 1;  // 1=OUT, 0=IN
73byte ccwCW = 1;  // 1 CCW 0=CW
74
75int speed_InOut = 1000;  // speed of second motor
76int speed_rot = 500;   // speed of first motor
77
78// change in distanceq
79
80int dx, dy;  //delta x and y
81int go = 0;
82float m = 0;  //slope between points
83float m2 = 0;
84float mx, my;  //move distance
85
86
87
88int line[] = {
89  50, 0,
90  0, -50,
91  -50, 0,
92  0, 50
93};
94
95int star[]={
96 0,0,
97-71,-71,
98-71,-71,
99-69,-73,
100-99,5,
101-100,5,
102-100,10,
103-99,15,
104-98,20,
105-97,25,
106-96,29,
107-95,31,
108-50,-88,
109-50,-88,
110-45,-90,
111-41,-92,
112-36,-94,
113-33,-95,
114-88,48,
115-88,48,
116-85,52,
117-83,56,
118-80,60,
119-80,61,
120-18,-99,
121-18,-99,
122-13,-100,
123-8,-100,
124-3,-100,
125-70,71,
126-70,71,
127-67,75,
128-63,78,
129-60,80,
13011,-100,
13111,-100,
13216,-99,
13320,-98,
13424,-98,
135-48,87,
136-48,87,
137-44,90,
138-39,92,
139-37,93,
14036,-94,
14136,-94,
14241,-92,
14345,-90,
14448,-88,
145-24,97,
146-24,97,
147-19,98,
148-14,99,
149-11,99,
15059,-80,
15159,-81,
15263,-78,
15367,-75,
15469,-72,
1553,99,
1563,99,
1578,99,
15812,99,
15917,98,
16079,-61,
16179,-61,
16282,-57,
16385,-53,
16487,-49,
16533,93,
16633,94,
16738,92,
16842,90,
16946,88,
17050,86,
17194,-32,
17294,-32,
17396,-27,
17497,-22,
17598,-17,
17699,-13,
17799,-8,
17869,71
179
180
181};
182
183
184
185void dir() {
186  // Determine how to go IN or OUT?
187  if (inOutSteps > inOutStepsTo) { inOut = 0; }  // go in
188  if (inOutSteps < inOutStepsTo) { inOut = 1; }  // go out
189  digitalWrite(dirPin_InOut, !inOut);            // Set direction
190
191
192  if ((rotStepsTo > rotSteps && (rotStepsTo - rotSteps < rot_total_steps / 2)) || (rotStepsTo < rotSteps && (rotSteps - rotStepsTo > rot_total_steps / 2))) {
193    ccwCW = 1;
194    digitalWrite(dirPin_rot, !ccwCW);  // Set direction
195  } else {
196    ccwCW = 0;
197    digitalWrite(dirPin_rot, !ccwCW);  // Set direction
198  }
199}
200
201
202void plotShape(int shape[], int size) {
203
204  for (int t = 0; t < (size / 2); t++)  // size of array div 2.
205  {
206    // if (button > 0) { return; }
207    x2 = shape[t * 2];
208    y2 = shape[t * 2 + 1];
209    Serial.print(x2);
210    Serial.print(",");
211    Serial.println(y2);
212    gotoXY(x2, y2);//shift data left and down 50 units to make gride x100 by y100
213    
214  }
215}
216
217void gotoXY(float x3, float y3) {
218  // Calculate the step increments in x and y directions
219  float dx = x3 - x;
220  float dy = y3 - y;
221
222  // Determine the total number of steps to move
223  int totalSteps = max(abs(dx), abs(dy));
224
225  // Calculate the step increments for x and y
226  float stepX = dx / totalSteps;
227  float stepY = dy / totalSteps;
228
229  // Calculate initial position
230  float currentX = x;
231  float currentY = y;
232
233  // Move incrementally to the new position
234  for (int i = 0; i < totalSteps; i++) {
235    // Calculate the new target steps for inout motor
236    float newX = currentX + stepX;
237    float newY = currentY + stepY;
238    reverseKinematics(newX, newY);
239
240    // Move to the next position
241    moveTo(newX, newY);
242
243    // Adjust current position for the next iteration
244    currentX = newX;
245    currentY = newY;
246   
247  }
248   x=x3;
249    y=y3;
250}
251
252
253
254
255
256void display() {
257
258  if (moving == 1) {
259    r = (inOutSteps / inOut_total_steps) * desired_scale;  // changing
260    x = r * cos(radAngle);
261    y = r * sin(radAngle);
262  }
263  //Serial.print("m");
264  //Serial.print(m);
265
266 Serial.print(ccwCW);
267  Serial.print(",");
268  Serial.print(inOut);
269  Serial.print(",");
270  Serial.print(speed_rot);
271  Serial.print(",");
272  Serial.print(knob2);
273  Serial.print(" ");
274  
275  //Serial.print("m");
276  //Serial.print(m);
277
278
279 Serial.print(offset);
280  Serial.print(" *  ");
281  Serial.print(inOutON);
282  Serial.print(",");
283  Serial.print(rotON);
284  Serial.print(" *  ");
285  Serial.print(" m");
286  Serial.print(m);
287  Serial.print(" m2");
288  Serial.print(m2);
289  //Serial.print(" go");
290  //Serial.print(go);
291
292  Serial.print(" %");
293
294
295  Serial.print(x);
296  Serial.print("x,y");
297  Serial.print(y);
298  Serial.print(" ");
299
300
301  // Serial.print("rotAng");
302  //Serial.print("  ");
303  //Serial.print((rotAngle));
304  //Serial.print(" ");
305  //Serial.print((angleTo));
306  //Serial.print(" ");
307  //Serial.print("rad=");
308  //Serial.print((radAngle));
309  //Serial.print("  ");
310  //Serial.print("ySpeed");
311  //Serial.print("  ");
312  // Serial.print(speed_InOut);
313  //  Serial.print("  ");
314
315
316
317
318  Serial.print(rotSteps);
319  Serial.print(">");
320
321  Serial.print(rotStepsTo);
322  Serial.print(" ");
323  Serial.print(inOutSteps);
324  Serial.print(">");
325
326  Serial.print(inOutStepsTo);
327  Serial.print(" (");
328  Serial.print(x2);
329  Serial.print(",");
330  Serial.print(y2);
331  Serial.println("");
332}
333
334
335
336// Function to calculate reverse kinematics
337void reverseKinematics(float xx, float yy) {
338  // Calculate radial distance (r)
339  rTo = sqrt(xx * xx + yy * yy);
340  //rTo=rTo* (rot_total_steps/desired_scale);
341  // Calculate angle (theta) using atan2
342  // atan2 returns the angle in radians in the range -pi to pi
343  angleTo = atan2(yy, xx);
344  if (angleTo < 0) angleTo = (2 * PI) + angleTo;  // if neg
345
346  //rotAngle=(ccwCW_Steps/16000.0)*360.0;
347  rotStepsTo = (angleTo / (2 * PI)) * rot_total_steps;
348  inOutStepsTo = (rTo / desired_scale) * inOut_total_steps;
349
350
351  //(turn on the direction )
352  //if (inOutSteps != inOutStepsTo) inOutON = 1;
353  //if (rotSteps!= rotStepsTo) rotON = 1;
354
355
356
357  // Determine how to go IN or OUT?
358  if (inOutSteps > inOutStepsTo) { inOut = 0; }  // go in
359  if (inOutSteps < inOutStepsTo) { inOut = 1; }  // go out
360  digitalWrite(dirPin_InOut, !inOut);            // Set direction
361
362
363  if ((rotStepsTo > rotSteps && (rotStepsTo - rotSteps < rot_total_steps / 2)) || (rotStepsTo < rotSteps && (rotSteps - rotStepsTo > rot_total_steps / 2))) {
364    ccwCW = 1;
365    digitalWrite(dirPin_rot, !ccwCW);  // Set direction
366  } else {
367    ccwCW = 0;
368    digitalWrite(dirPin_rot, !ccwCW);  // Set direction
369  }
370}
371
372
373// Function to move to a specific target position
374void moveTo(float x4, float y4) {
375
376
377  x2 = x4;
378  y2 = y4;
379  reverseKinematics(x4, y4);
380
381
382
383  //first rotate then extend
384  //while (rotSteps!= rotStepsTo)
385  //{
386  //rotON = 1;
387  //display();
388  // }
389
390  if (rotSteps != rotStepsTo) rotON = 1;
391   if (inOutSteps != inOutStepsTo) inOutON = 1;
392while (rotON == 1||inOutON == 1) {
393    // DO NOTHING!!! wait until hits rot Target location.
394   // display();
395    delay(1);
396  }
397  
398
399
400  //delay(10);
401
402  //while (inOutSteps != inOutStepsTo)
403  //if (inOutSteps != inOutStepsTo) inOutON = 1;
404  //while (inOutON == 1) {
405  // DO NOTHING!!! wait until hits inOut Target location.
406  //  display();
407  // delay(1);
408  //}
409  // delay(10);
410
411  ////if (rotSteps!= rotStepsTo) rotON = 1;
412  //x=xTarget;
413  //y=yTarget;
414}
415
416
417void setup() {
418
419
420  //Setup Interupts
421  TCCR1A = 0;  // Init Timer1A
422  TCCR1B = 0;  // Init Timer1B
423  //TCNT1 = 0;            // initialize counter value to 0
424  TCCR1B |= B00000011;  // Prescaler = 64
425  OCR1A = 1000;         // Timer Compare1A Register  // = 16000000 / (64 * 1000) - 1 (must be <65536)
426  OCR1B = 1000;         // Timer Compare1B Register
427  //TIMSK1 |= B00000110;  // Enable Timer COMPA(y)   +COMPB (x) Interrupts
428  TIMSK1 |= B00000110;  // only use one interupt
429
430  // Define pins as outputs
431  pinMode(stepPin_rot, OUTPUT);
432  pinMode(dirPin_rot, OUTPUT);
433  pinMode(stepPin_InOut, OUTPUT);
434  pinMode(dirPin_InOut, OUTPUT);
435  pinMode(enPin_InOut, OUTPUT);
436  pinMode(enPin_rot, OUTPUT);
437
438  pinMode(lights_pin, OUTPUT);
439
440digitalWrite(lights_pin, HIGH);  // DIS-Enable the stepper motor
441
442  digitalWrite(enPin_InOut, HIGH);  // DIS-Enable the stepper motor
443  digitalWrite(enPin_rot, HIGH);    // DIS-Enable the stepper motor
444
445  // digitalWrite(enPin_InOut, LOW);  // Enable the stepper motor
446  // Initialize serial communication
447  Serial.begin(9600);
448  Serial.println("sTARting ");
449  display();
450}
451
452void loop() {
453
454
455  //display if motors are moving
456
457  if (rotON == 1 || inOutON == 1) {display(); digitalWrite(lights_pin, HIGH);}  // DIS-Enable the stepper motor
458  else digitalWrite(lights_pin, LOW); 
459
460  knob1 = analogRead(A0);
461  knob2 = analogRead(A1);
462
463  speed_rot = map(knob1, 0, 1023, 100, 2000);
464  speed_InOut = map(knob2, 0, 1023, 150, 1000);
465if (knob2>1015) 
466{
467inOutON=0;
468 if (rotON==0) moving=0;
469 digitalWrite(enPin_InOut, HIGH);
470}
471else
472{
473inOutON=1;
474moving=1;
475//digitalWrite(enPin_InOut, LOW);
476}
477
478
479if (knob1>1015) 
480{
481rotON=0;
482 if (inOutON==0) moving=0;
483 digitalWrite(enPin_rot, HIGH);
484}
485else
486{
487rotON=1;
488moving=1;
489//digitalWrite(enPin_rot, LOW);
490}
491
492
493
494
495
496
497  //if (inOutSteps != inOutStepsTo) {
498
499  //dir();
500  // Determine how to go IN or OUT?
501  //if (inOutSteps > inOutStepsTo) { inOut = 0; }  // go in
502  //if (inOutSteps < inOutStepsTo) { inOut = 1; }  // go out
503  //digitalWrite(dirPin_InOut, inOut);            // Set direction
504
505  //     inOutON = 1;}
506
507
508  if (Serial.available() > 0) {
509    // Read the incoming byte
510    char command = Serial.read();
511
512    // Move X axis
513
514
515
516
517    if (command == 'q') {  // turn on x motor
518      rotON = !rotON;
519      if (inOutON==0) moving = !moving;
520      digitalWrite(enPin_rot, !rotON);  // Enable the stepper motor
521    }
522
523    if (command == 'w') {               // change dir of x motor
524      digitalWrite(dirPin_rot, ccwCW);  // Set direction
525      ccwCW = !ccwCW;
526    }
527
528
529    if (command == 'e') {  // change dir of x motor
530
531      speed_rot += 50;
532      //digitalWrite(dirPin_rot,ccwCW); // Set direction
533      // ccwCW=!ccwCW;
534    }
535    if (command == 'r') {  // change dir of x motor
536
537      speed_rot -= 50;
538      //digitalWrite(dirPin_rot,ccwCW); // Set direction
539      // ccwCW=!ccwCW;
540    }
541
542
543    
544
545
546
547
548
549
550    if (command == 'a') {  // turn on x motor
551      inOutON = !inOutON;
552      if (rotON==0) moving = !moving;
553      digitalWrite(enPin_InOut, !inOutON);  // Enable the stepper motor
554    }
555
556    if (command == 's') {                 // change dir of x motor
557      digitalWrite(dirPin_InOut, inOut);  // Set direction
558      inOut = !inOut;
559    }
560
561
562    if (command == 'd') {  // change dir of x motor
563
564      speed_InOut -= 50;
565      //digitalWrite(dirPin_rot,ccwCW); // Set direction
566      // ccwCW=!ccwCW;
567    }
568    if (command == 'f') {  // change dir of x motor
569
570      speed_InOut += 50;
571      //digitalWrite(dirPin_rot,ccwCW); // Set direction
572      // ccwCW=!ccwCW;
573    }
574
575    if (command =='x')
576    {
577      plotShape(star, sizeof(star) / sizeof(star[0]));
578    }
579
580    if (command == 'z') {
581
582plotShape(line, sizeof(line) / sizeof(line[0]));
583      //moveTo(40, 0);
584    //  gotoXY(57, 0);
585     // x = 57;
586      //y = 0;
587      
588     // gotoXY(0, 57);
589      
590    //  gotoXY(-57, 0);
591    //  gotoXY(0, -57);
592     // gotoXY(57, 0);
593      
594      
595     // gotoXY(0, 0);
596      
597    }
598
599
600    // Move Y axis
601    if (command == 'Y') {
602      //digitalWrite(enPin, LOW); // Enable the stepper motor
603      digitalWrite(dirPin_InOut, HIGH);  // Set direction
604      for (int i = 0; i < 200; i++) {    // Steps for 360 degree rotation
605        digitalWrite(stepPin_InOut, HIGH);
606        delayMicroseconds(500);  // Adjust delay for speed
607        digitalWrite(stepPin_InOut, LOW);
608        delayMicroseconds(500);  // Adjust delay for speed
609      }
610      //  digitalWrite(enPin, HIGH); // Disable the stepper motor
611    }
612  }
613}
614
615
616
617
618
619
620// First interupt
621ISR(TIMER1_COMPA_vect) {
622  OCR1A += speed_rot;  // Advance The COMPA Register
623
624  
625
626if (rotON==0&&inOutON==0)offset=0;
627
628  if (rotON == 1) {
629
630
631//only if one is rotating 
632    digitalWrite(enPin_rot, LOW);  // Enable the stepper motor
633  pwm = !pwm;
634
635    digitalWrite(stepPin_rot, pwm);
636   
637
638
639  
640
641 if (pwm == 0&&rotON==1&&inOutON==1) 
642 
643 offset += 1;
644if (offset > 9 )  
645
646{
647  if (ccwCW == 1) inOutSteps =inOutSteps+ 1;// only increase 1/10 the speed
648  if (ccwCW == 0) inOutSteps =inOutSteps- 1;// only increase 1/10 the speed
649  offset=0;
650  
651}
652
653
654    /**
655        if (offset==0) digitalWrite(stepPin_InOut, pwm);
656    
657
658
659    if (pwm == 0) {
660
661      offset += 1;
662if (offset > 9 )  // only increase 1/10 the speed
663
664{
665offset = 0;
666
667
668
669 if (ccwCW==1) 
670 {
671   if (inOutON==0)digitalWrite(dirPin_InOut, ccwCW);  // Set one step
672   if (inOutON==1&&rotON==1)inOutSteps+=1;
673 }
674  if (ccwCW==0) 
675  {
676    
677    //inOutSteps-=1;
678    if (inOutON==0)digitalWrite(dirPin_InOut, ccwCW);  // Set one step
679    if (inOutON==1&&rotON==1)inOutSteps-=1;
680  }
681
682}
683
684*/
685
686      if (ccwCW == 1) rotSteps = rotSteps + 1;
687      if (ccwCW == 0)rotSteps = rotSteps - 1;
688
689     
690
691      if (rotSteps > 16000) rotSteps = 0;  //happens only once
692
693      if (rotSteps < 0) rotSteps = 15999;                  //happens only once
694      radAngle = (rotSteps / rot_total_steps) * (2 * PI);  /// changing
695      rotAngle = (rotSteps / rot_total_steps) * 360.0;
696
697
698
699      
700      if (rotSteps == rotStepsTo && moving == 0)  rotON = 0;  // turn off motor if it reaches the set point
701
702        
703     
704     
705    }
706}
707
708    //inOutStepsTo = (1600.0 / (360.0 / (T2)));
709  //} //else
710    //digitalWrite(enPin_rot, HIGH);  // TURN OFF THE MOTOR.
711//}
712//else
713//digitalWrite(enPin_rot, HIGH);  // TURN OFF THE MOTOR.
714//}
715
716ISR(TIMER1_COMPB_vect) {  // speed for second motor
717
718
719  OCR1B += speed_InOut;  // Advance The COMPB Register //8750
720
721  
722
723  if (inOutON == 1) {
724
725
726
727    digitalWrite(enPin_InOut, LOW);  // Enable the stepper motor
728
729    pwm2 = !pwm2;
730    digitalWrite(stepPin_InOut, pwm2);
731
732
733    if (pwm2 == 0) {
734
735
736      if (moving == 0) {
737        int state = inOut;
738        if (inOutSteps > inOutStepsTo) { inOut = 0; }  // go in
739        if (inOutSteps < inOutStepsTo) { inOut = 1; }  // go out
740                                                       // if (state!=inOut) inOutSteps+=135;// a change has occured.
741        digitalWrite(dirPin_InOut, !inOut);            // Set direction
742      }
743
744
745
746
747      if (inOut == 1) inOutSteps = inOutSteps + 1;
748      if (inOut == 0) inOutSteps = inOutSteps - 1;
749
750
751
752      if (inOutSteps > 5000 && moving == 1) {
753        inOut = 0;
754        digitalWrite(dirPin_InOut, 1);  // Set direction
755      }
756      if (inOutSteps < 0 && moving == 1) {
757
758        inOut = 1;
759        digitalWrite(dirPin_InOut, 0);  // Set direction
760      }
761
762
763
764
765
766      
767
768
769      if (inOutSteps == inOutStepsTo && moving == 0) {
770        inOutON = 0;
771        }
772    }
773  } //else
774
775
776}

#include <math.h>
// Pins


// circle size radius = 100 units

#define enPin_rot 12      // Stepper motor enable pin
#define lights_pin 11      // Stepper motor enable pin
#define stepPin_rot 2    // X axis step pin
#define dirPin_rot 5     // X axis direction pin
#define enPin_InOut 8    // Stepper motor enable pin
#define stepPin_InOut 3  // Y axis step pin
#define dirPin_InOut 6   // Y axis direction pin
#define PI 3.1415926535897932384626433832795


// Changable variables.
#define rot_total_steps 16000.0
#define inOut_total_steps 5000.0
#define desired_scale 100.0

//int knob1 = analogRead(A1);
int knob1 = 0;
int knob2 = 0;


volatile float r = 0;  // length of extended arm
volatile float x = 0;
volatile float y = 0;  // current location of ball

volatile float x2 = 0;
volatile float y2 = 0;

//int relStep=0;

float xLoc = 0;
float yLoc = 0;

int offset = 0;
int moving = 0;

//volatile float xTarget = 0;
//volatile float yTarget = 0;  // current location of ball



float radAngle = 0;
float rotAngle = 0;


int inOutSteps = 0;  /// Y
int inOutStepsTo = 0;
int rotSteps = 0;  //  X
int rotStepsTo = 0;
byte rotON = 0;
byte inOutON = 1;



// Variables to store results
float rTo, angleTo;
//float targetX = 0;  // Default target coordinates
//float targetY = 0;



//int xAng = 0;

int pwm = 0;
int pwm2 = 0;

byte inOut = 1;  // 1=OUT, 0=IN
byte ccwCW = 1;  // 1 CCW 0=CW

int speed_InOut = 1000;  // speed of second motor
int speed_rot = 500;   // speed of first motor

// change in distanceq

int dx, dy;  //delta x and y
int go = 0;
float m = 0;  //slope between points
float m2 = 0;
float mx, my;  //move distance



int line[] = {
  50, 0,
  0, -50,
  -50, 0,
  0, 50
};

int star[]={
 0,0,
-71,-71,
-71,-71,
-69,-73,
-99,5,
-100,5,
-100,10,
-99,15,
-98,20,
-97,25,
-96,29,
-95,31,
-50,-88,
-50,-88,
-45,-90,
-41,-92,
-36,-94,
-33,-95,
-88,48,
-88,48,
-85,52,
-83,56,
-80,60,
-80,61,
-18,-99,
-18,-99,
-13,-100,
-8,-100,
-3,-100,
-70,71,
-70,71,
-67,75,
-63,78,
-60,80,
11,-100,
11,-100,
16,-99,
20,-98,
24,-98,
-48,87,
-48,87,
-44,90,
-39,92,
-37,93,
36,-94,
36,-94,
41,-92,
45,-90,
48,-88,
-24,97,
-24,97,
-19,98,
-14,99,
-11,99,
59,-80,
59,-81,
63,-78,
67,-75,
69,-72,
3,99,
3,99,
8,99,
12,99,
17,98,
79,-61,
79,-61,
82,-57,
85,-53,
87,-49,
33,93,
33,94,
38,92,
42,90,
46,88,
50,86,
94,-32,
94,-32,
96,-27,
97,-22,
98,-17,
99,-13,
99,-8,
69,71


};



void dir() {
  // Determine how to go IN or OUT?
  if (inOutSteps > inOutStepsTo) { inOut = 0; }  // go in
  if (inOutSteps < inOutStepsTo) { inOut = 1; }  // go out
  digitalWrite(dirPin_InOut, !inOut);            // Set direction


  if ((rotStepsTo > rotSteps && (rotStepsTo - rotSteps < rot_total_steps / 2)) || (rotStepsTo < rotSteps && (rotSteps - rotStepsTo > rot_total_steps / 2))) {
    ccwCW = 1;
    digitalWrite(dirPin_rot, !ccwCW);  // Set direction
  } else {
    ccwCW = 0;
    digitalWrite(dirPin_rot, !ccwCW);  // Set direction
  }
}


void plotShape(int shape[], int size) {

  for (int t = 0; t < (size / 2); t++)  // size of array div 2.
  {
    // if (button > 0) { return; }
    x2 = shape[t * 2];
    y2 = shape[t * 2 + 1];
    Serial.print(x2);
    Serial.print(",");
    Serial.println(y2);
    gotoXY(x2, y2);//shift data left and down 50 units to make gride x100 by y100
    
  }
}

void gotoXY(float x3, float y3) {
  // Calculate the step increments in x and y directions
  float dx = x3 - x;
  float dy = y3 - y;

  // Determine the total number of steps to move
  int totalSteps = max(abs(dx), abs(dy));

  // Calculate the step increments for x and y
  float stepX = dx / totalSteps;
  float stepY = dy / totalSteps;

  // Calculate initial position
  float currentX = x;
  float currentY = y;

  // Move incrementally to the new position
  for (int i = 0; i < totalSteps; i++) {
    // Calculate the new target steps for inout motor
    float newX = currentX + stepX;
    float newY = currentY + stepY;
    reverseKinematics(newX, newY);

    // Move to the next position
    moveTo(newX, newY);

    // Adjust current position for the next iteration
    currentX = newX;
    currentY = newY;
   
  }
   x=x3;
    y=y3;
}





void display() {

  if (moving == 1) {
    r = (inOutSteps / inOut_total_steps) * desired_scale;  // changing
    x = r * cos(radAngle);
    y = r * sin(radAngle);
  }
  //Serial.print("m");
  //Serial.print(m);

 Serial.print(ccwCW);
  Serial.print(",");
  Serial.print(inOut);
  Serial.print(",");
  Serial.print(speed_rot);
  Serial.print(",");
  Serial.print(knob2);
  Serial.print(" ");
  
  //Serial.print("m");
  //Serial.print(m);


 Serial.print(offset);
  Serial.print(" *  ");
  Serial.print(inOutON);
  Serial.print(",");
  Serial.print(rotON);
  Serial.print(" *  ");
  Serial.print(" m");
  Serial.print(m);
  Serial.print(" m2");
  Serial.print(m2);
  //Serial.print(" go");
  //Serial.print(go);

  Serial.print(" %");


  Serial.print(x);
  Serial.print("x,y");
  Serial.print(y);
  Serial.print(" ");


  // Serial.print("rotAng");
  //Serial.print("  ");
  //Serial.print((rotAngle));
  //Serial.print(" ");
  //Serial.print((angleTo));
  //Serial.print(" ");
  //Serial.print("rad=");
  //Serial.print((radAngle));
  //Serial.print("  ");
  //Serial.print("ySpeed");
  //Serial.print("  ");
  // Serial.print(speed_InOut);
  //  Serial.print("  ");




  Serial.print(rotSteps);
  Serial.print(">");

  Serial.print(rotStepsTo);
  Serial.print(" ");
  Serial.print(inOutSteps);
  Serial.print(">");

  Serial.print(inOutStepsTo);
  Serial.print(" (");
  Serial.print(x2);
  Serial.print(",");
  Serial.print(y2);
  Serial.println("");
}



// Function to calculate reverse kinematics
void reverseKinematics(float xx, float yy) {
  // Calculate radial distance (r)
  rTo = sqrt(xx * xx + yy * yy);
  //rTo=rTo* (rot_total_steps/desired_scale);
  // Calculate angle (theta) using atan2
  // atan2 returns the angle in radians in the range -pi to pi
  angleTo = atan2(yy, xx);
  if (angleTo < 0) angleTo = (2 * PI) + angleTo;  // if neg

  //rotAngle=(ccwCW_Steps/16000.0)*360.0;
  rotStepsTo = (angleTo / (2 * PI)) * rot_total_steps;
  inOutStepsTo = (rTo / desired_scale) * inOut_total_steps;


  //(turn on the direction )
  //if (inOutSteps != inOutStepsTo) inOutON = 1;
  //if (rotSteps!= rotStepsTo) rotON = 1;



  // Determine how to go IN or OUT?
  if (inOutSteps > inOutStepsTo) { inOut = 0; }  // go in
  if (inOutSteps < inOutStepsTo) { inOut = 1; }  // go out
  digitalWrite(dirPin_InOut, !inOut);            // Set direction


  if ((rotStepsTo > rotSteps && (rotStepsTo - rotSteps < rot_total_steps / 2)) || (rotStepsTo < rotSteps && (rotSteps - rotStepsTo > rot_total_steps / 2))) {
    ccwCW = 1;
    digitalWrite(dirPin_rot, !ccwCW);  // Set direction
  } else {
    ccwCW = 0;
    digitalWrite(dirPin_rot, !ccwCW);  // Set direction
  }
}


// Function to move to a specific target position
void moveTo(float x4, float y4) {


  x2 = x4;
  y2 = y4;
  reverseKinematics(x4, y4);



  //first rotate then extend
  //while (rotSteps!= rotStepsTo)
  //{
  //rotON = 1;
  //display();
  // }

  if (rotSteps != rotStepsTo) rotON = 1;
   if (inOutSteps != inOutStepsTo) inOutON = 1;
while (rotON == 1||inOutON == 1) {
    // DO NOTHING!!! wait until hits rot Target location.
   // display();
    delay(1);
  }
  


  //delay(10);

  //while (inOutSteps != inOutStepsTo)
  //if (inOutSteps != inOutStepsTo) inOutON = 1;
  //while (inOutON == 1) {
  // DO NOTHING!!! wait until hits inOut Target location.
  //  display();
  // delay(1);
  //}
  // delay(10);

  ////if (rotSteps!= rotStepsTo) rotON = 1;
  //x=xTarget;
  //y=yTarget;
}


void setup() {


  //Setup Interupts
  TCCR1A = 0;  // Init Timer1A
  TCCR1B = 0;  // Init Timer1B
  //TCNT1 = 0;            // initialize counter value to 0
  TCCR1B |= B00000011;  // Prescaler = 64
  OCR1A = 1000;         // Timer Compare1A Register  // = 16000000 / (64 * 1000) - 1 (must be <65536)
  OCR1B = 1000;         // Timer Compare1B Register
  //TIMSK1 |= B00000110;  // Enable Timer COMPA(y)   +COMPB (x) Interrupts
  TIMSK1 |= B00000110;  // only use one interupt

  // Define pins as outputs
  pinMode(stepPin_rot, OUTPUT);
  pinMode(dirPin_rot, OUTPUT);
  pinMode(stepPin_InOut, OUTPUT);
  pinMode(dirPin_InOut, OUTPUT);
  pinMode(enPin_InOut, OUTPUT);
  pinMode(enPin_rot, OUTPUT);

  pinMode(lights_pin, OUTPUT);

digitalWrite(lights_pin, HIGH);  // DIS-Enable the stepper motor

  digitalWrite(enPin_InOut, HIGH);  // DIS-Enable the stepper motor
  digitalWrite(enPin_rot, HIGH);    // DIS-Enable the stepper motor

  // digitalWrite(enPin_InOut, LOW);  // Enable the stepper motor
  // Initialize serial communication
  Serial.begin(9600);
  Serial.println("sTARting ");
  display();
}

void loop() {


  //display if motors are moving

  if (rotON == 1 || inOutON == 1) {display(); digitalWrite(lights_pin, HIGH);}  // DIS-Enable the stepper motor
  else digitalWrite(lights_pin, LOW); 

  knob1 = analogRead(A0);
  knob2 = analogRead(A1);

  speed_rot = map(knob1, 0, 1023, 100, 2000);
  speed_InOut = map(knob2, 0, 1023, 150, 1000);
if (knob2>1015) 
{
inOutON=0;
 if (rotON==0) moving=0;
 digitalWrite(enPin_InOut, HIGH);
}
else
{
inOutON=1;
moving=1;
//digitalWrite(enPin_InOut, LOW);
}


if (knob1>1015) 
{
rotON=0;
 if (inOutON==0) moving=0;
 digitalWrite(enPin_rot, HIGH);
}
else
{
rotON=1;
moving=1;
//digitalWrite(enPin_rot, LOW);
}






  //if (inOutSteps != inOutStepsTo) {

  //dir();
  // Determine how to go IN or OUT?
  //if (inOutSteps > inOutStepsTo) { inOut = 0; }  // go in
  //if (inOutSteps < inOutStepsTo) { inOut = 1; }  // go out
  //digitalWrite(dirPin_InOut, inOut);            // Set direction

  //     inOutON = 1;}


  if (Serial.available() > 0) {
    // Read the incoming byte
    char command = Serial.read();

    // Move X axis




    if (command == 'q') {  // turn on x motor
      rotON = !rotON;
      if (inOutON==0) moving = !moving;
      digitalWrite(enPin_rot, !rotON);  // Enable the stepper motor
    }

    if (command == 'w') {               // change dir of x motor
      digitalWrite(dirPin_rot, ccwCW);  // Set direction
      ccwCW = !ccwCW;
    }


    if (command == 'e') {  // change dir of x motor

      speed_rot += 50;
      //digitalWrite(dirPin_rot,ccwCW); // Set direction
      // ccwCW=!ccwCW;
    }
    if (command == 'r') {  // change dir of x motor

      speed_rot -= 50;
      //digitalWrite(dirPin_rot,ccwCW); // Set direction
      // ccwCW=!ccwCW;
    }


    






    if (command == 'a') {  // turn on x motor
      inOutON = !inOutON;
      if (rotON==0) moving = !moving;
      digitalWrite(enPin_InOut, !inOutON);  // Enable the stepper motor
    }

    if (command == 's') {                 // change dir of x motor
      digitalWrite(dirPin_InOut, inOut);  // Set direction
      inOut = !inOut;
    }


    if (command == 'd') {  // change dir of x motor

      speed_InOut -= 50;
      //digitalWrite(dirPin_rot,ccwCW); // Set direction
      // ccwCW=!ccwCW;
    }
    if (command == 'f') {  // change dir of x motor

      speed_InOut += 50;
      //digitalWrite(dirPin_rot,ccwCW); // Set direction
      // ccwCW=!ccwCW;
    }

    if (command =='x')
    {
      plotShape(star, sizeof(star) / sizeof(star[0]));
    }

    if (command == 'z') {

plotShape(line, sizeof(line) / sizeof(line[0]));
      //moveTo(40, 0);
    //  gotoXY(57, 0);
     // x = 57;
      //y = 0;
      
     // gotoXY(0, 57);
      
    //  gotoXY(-57, 0);
    //  gotoXY(0, -57);
     // gotoXY(57, 0);
      
      
     // gotoXY(0, 0);
      
    }


    // Move Y axis
    if (command == 'Y') {
      //digitalWrite(enPin, LOW); // Enable the stepper motor
      digitalWrite(dirPin_InOut, HIGH);  // Set direction
      for (int i = 0; i < 200; i++) {    // Steps for 360 degree rotation
        digitalWrite(stepPin_InOut, HIGH);
        delayMicroseconds(500);  // Adjust delay for speed
        digitalWrite(stepPin_InOut, LOW);
        delayMicroseconds(500);  // Adjust delay for speed
      }
      //  digitalWrite(enPin, HIGH); // Disable the stepper motor
    }
  }
}






// First interupt
ISR(TIMER1_COMPA_vect) {
  OCR1A += speed_rot;  // Advance The COMPA Register

  

if (rotON==0&&inOutON==0)offset=0;

  if (rotON == 1) {


//only if one is rotating 
    digitalWrite(enPin_rot, LOW);  // Enable the stepper motor
  pwm = !pwm;

    digitalWrite(stepPin_rot, pwm);
   


  

 if (pwm == 0&&rotON==1&&inOutON==1) 
 
 offset += 1;
if (offset > 9 )  

{
  if (ccwCW == 1) inOutSteps =inOutSteps+ 1;// only increase 1/10 the speed
  if (ccwCW == 0) inOutSteps =inOutSteps- 1;// only increase 1/10 the speed
  offset=0;
  
}


    /**
        if (offset==0) digitalWrite(stepPin_InOut, pwm);
    


    if (pwm == 0) {

      offset += 1;
if (offset > 9 )  // only increase 1/10 the speed

{
offset = 0;



 if (ccwCW==1) 
 {
   if (inOutON==0)digitalWrite(dirPin_InOut, ccwCW);  // Set one step
   if (inOutON==1&&rotON==1)inOutSteps+=1;
 }
  if (ccwCW==0) 
  {
    
    //inOutSteps-=1;
    if (inOutON==0)digitalWrite(dirPin_InOut, ccwCW);  // Set one step
    if (inOutON==1&&rotON==1)inOutSteps-=1;
  }

}

*/

      if (ccwCW == 1) rotSteps = rotSteps + 1;
      if (ccwCW == 0)rotSteps = rotSteps - 1;

     

      if (rotSteps > 16000) rotSteps = 0;  //happens only once

      if (rotSteps < 0) rotSteps = 15999;                  //happens only once
      radAngle = (rotSteps / rot_total_steps) * (2 * PI);  /// changing
      rotAngle = (rotSteps / rot_total_steps) * 360.0;



      
      if (rotSteps == rotStepsTo && moving == 0)  rotON = 0;  // turn off motor if it reaches the set point

        
     
     
    }
}

    //inOutStepsTo = (1600.0 / (360.0 / (T2)));
  //} //else
    //digitalWrite(enPin_rot, HIGH);  // TURN OFF THE MOTOR.
//}
//else
//digitalWrite(enPin_rot, HIGH);  // TURN OFF THE MOTOR.
//}

ISR(TIMER1_COMPB_vect) {  // speed for second motor


  OCR1B += speed_InOut;  // Advance The COMPB Register //8750

  

  if (inOutON == 1) {



    digitalWrite(enPin_InOut, LOW);  // Enable the stepper motor

    pwm2 = !pwm2;
    digitalWrite(stepPin_InOut, pwm2);


    if (pwm2 == 0) {


      if (moving == 0) {
        int state = inOut;
        if (inOutSteps > inOutStepsTo) { inOut = 0; }  // go in
        if (inOutSteps < inOutStepsTo) { inOut = 1; }  // go out
                                                       // if (state!=inOut) inOutSteps+=135;// a change has occured.
        digitalWrite(dirPin_InOut, !inOut);            // Set direction
      }




      if (inOut == 1) inOutSteps = inOutSteps + 1;
      if (inOut == 0) inOutSteps = inOutSteps - 1;



      if (inOutSteps > 5000 && moving == 1) {
        inOut = 0;
        digitalWrite(dirPin_InOut, 1);  // Set direction
      }
      if (inOutSteps < 0 && moving == 1) {

        inOut = 1;
        digitalWrite(dirPin_InOut, 0);  // Set direction
      }





      


      if (inOutSteps == inOutStepsTo && moving == 0) {
        inOutON = 0;
        }
    }
  } //else


}

Downloadable files

Laser Cutting Files

DXF and google sketchup

GearedSandTable.dxf

sketchup files

can edit

GearedSandTable sketchup.skp

Comments

Only logged in users can leave comments

newsonator

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content