API tools faq
paste
Advertisement
Stella_209

MotorTest.ino

Stella_209
Jun 20th, 2018
157
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 9.38 KB | None | 0 0
raw download clone embed print report
  1. /*
  2.  *  Stepper motor control by Timer interrupt
  3.  *  by Agócs László
  4.  *  __________________________________________________________________________
  5.  *  Ez a teszt a motorvezérlés ellenőrzésére szolgál az alapvető GOTO funkciók
  6.  *  nagy részének felhasználásával.
  7.  *  S     : START, csak óramű megy;
  8.  *  O     : STOP, mindkét motor áll;
  9.  *  + és -: Növeli/csökkenti az óramű sebességét 0.02 Hz-el;
  10.  *  Rn    : RATE beállítása: R0..9;
  11.  *  RAnnn : RA cél megadása
  12.  *  DEnnn : DE cél megadása
  13.  *  C     : Koordináták kiírása soros monitorra;
  14.  *  G     : Goto cél;
  15.  *  MN    : Mozgatás É;
  16.  *  MS    : Mozgatás D;
  17.  *  ME    : Mozgatás K;
  18.  *  MW    : Mozgatás NY;
  19.  *  NN    : Mozgatás stop É;
  20.  *  NS    : Mozgatás stop D;
  21.  *  NE    : Mozgatás stop K;
  22.  *  NW    : Mozgatás stop NY;
  23.  */
  24.  
  25. #include <AccelStepper.h>
  26. #include <TimerOne.h>
  27. #include "DS3231.h"
  28.  
  29. AccelStepper RAMot(AccelStepper::DRIVER, 10, 9);
  30. AccelStepper DEMot(AccelStepper::DRIVER, 12, 11);
  31.  
  32. // Init the DS3231 using the hardware interface
  33. DS3231  rtc(7, 8);
  34.  
  35. // Távcső aktuális státusza: áll, óramű, pozícionál, kézi mozgatás, home, parkolt
  36. enum TStatus { none, tracking, moving, sleewing, homed, parked };
  37. TStatus Status;
  38.  
  39. enum TDirect { dNone, dN, dS, dE, dW };   // a kézi mozgatás iránya: É,D,K,Ny
  40. TDirect Direct;
  41. byte DirectByte = 0;            // alsó 4 bit jelzi az irányokat: NSEW pl. 1001 = North és West
  42.  
  43. double RA_SECSTEP = 1.8;        // 1.8"/LÉPÉS
  44. double DE_SECSTEP = 4.3;        // 4.3"/LÉPÉS
  45. double RA_START   = 6.5;        // 6h30m  start pozíció
  46. double DE_START   = 8.5;        // 8.5 fok
  47. double RA_POS     = 6.5;        // 6h30m  aktuális pozíció (a lépésekből kalkulálva)
  48. double DE_POS     = 8.5;        // 8.5 fok
  49. double RA_TARGET  = 8.0;         // Ra cél : 8h0m
  50. double DE_TARGET  = 22.0;        // De cél : 22 fok
  51.  
  52. // RATES : Ennyiszeres csillagsebesség : 0..9 értékekre
  53. double RATES[10] = { 0.5, 1.0, 2.0, 4.0, 8.0, 16.0, 32.0, 128.0, 512.0, 2048.0 };
  54. int    RA_MOVERATE = 5;               // 16x-os csillag sebesség
  55. int    DE_MOVERATE = 5;               // 16x-os csillag sebesség
  56. int    RA_SLEERATE = 5;               // 256x-os csillag sebesség
  57. int    DE_SLEERATE = 8;               // 256x-os csillag sebesség
  58. double RA_BASESPEED = 5/RA_SECSTEP;   // 1 sec alatt az égbolt 5"-et forsul -> 2.777... lépés/sec RA-ban a követéshez
  59. double DE_BASESPEED = 5/DE_SECSTEP;   // -> 1.162... lépés/sec DE-ben, a RA-val azonos sebességhez
  60. double RA_SPEED;                      // RA aktuális sebessége lépés/sec
  61. double DE_SPEED;                      // DE aktuális sebessége lépés/sec
  62.  
  63. Time  START_TIME;         // Induláskor megjegyezzük az időt
  64. Time  CURRENT_TIME;       // aktuális idő
  65.  
  66. String myString;
  67. String ch;
  68. long   interval = 100;          // 100 mikrosec = 0.0001 sec
  69.  
  70. boolean debug = true;
  71. // _____________________________________________________________________
  72.  
  73. void setup()
  74. {
  75.   Serial.begin(9600);
  76.   // Pin 13 has an LED : jelzi az óramű léptetéseket
  77.   pinMode( 13, OUTPUT );
  78.  
  79.   RA_SPEED = RA_BASESPEED;
  80.   DE_SPEED = 0;
  81.   RAMot.setMaxSpeed(10000.0);
  82.   RAMot.setSpeed(RA_BASESPEED);       // Óramű sebesség
  83.   RAMot.setCurrentPosition(0);
  84.   DEMot.setMaxSpeed(10000.0);
  85.   DEMot.setSpeed(DE_BASESPEED);
  86.   DEMot.setCurrentPosition(0);
  87.  
  88.   rtc.begin();  
  89.   START_TIME = rtc.getTime();
  90.   Timer1.initialize(interval);        // set a timer of length 100 microseconds (or 0.0001 sec - or 10 KHz
  91.   Timer1.attachInterrupt( timerIsr ); // attach the service routine here
  92.   if (debug) Serial.println("S = Start");
  93. }
  94.  
  95. void loop()
  96. {
  97.   String c;
  98.  
  99.   if (Serial.available() > 0)
  100.   {
  101.     myString = "";
  102.  
  103.     // Soros porti karakteres parancs neolvasása és értelmazése
  104.    
  105.     myString = Serial.readString();
  106.     ch = myString.charAt(0);
  107.  
  108.        if ((ch == "S") || (ch == "s"))
  109.        {
  110.             Status = tracking;
  111.             Timer1.resume();
  112.             digitalWrite( 13, HIGH );
  113.             if (debug) Serial.println("* Start");
  114.        }
  115.        if ((ch == "O") || (ch == "o"))
  116.        {
  117.             Status = none;
  118.             Timer1.stop();
  119.             digitalWrite( 13, LOW );
  120.             Serial.println("* Stop");
  121.        }
  122.        if (ch == "+")
  123.        {
  124.             interval -= 10000;
  125.             RA_BASESPEED -= 0.02;
  126.        }
  127.        if (ch == "-")
  128.        {
  129.             interval += 10000;
  130.             RA_BASESPEED += 0.02;
  131.        }
  132.        if ((ch == "R") || (ch == "r"))
  133.        {
  134.             myString = myString.substring(1);
  135.             int rate = myString.toInt();
  136.             RA_SLEERATE = rate;
  137.             DE_SLEERATE = rate;
  138.             Serial.print("* RATE = "); Serial.println(rate,DEC);
  139.        }
  140.        
  141.        // Innn : sebesség állítás usec
  142.        if ((myString[0] == 73) || (myString[0] == 105))
  143.        {
  144.             myString = myString.substring(1);
  145.             interval = myString.toInt();
  146.        }
  147.  
  148.        if (ch == "C")
  149.        {
  150.          RA_POS += 15*RA_SECSTEP/3600;
  151.          DE_POS += DE_SECSTEP/3600;
  152.        }
  153.  
  154.        if (ch == "M")
  155.        {
  156.             c = myString.charAt(1);
  157.             if (c == "N") {
  158.               Direct = dN;
  159.               DirectByte |= 8;
  160.               if ((DirectByte & 4)>0) DirectByte ^= 4;
  161.               Serial.println("North");
  162.             }
  163.             if (c == "S") {
  164.               Direct = dS;
  165.               DirectByte |= 4;
  166.               if ((DirectByte & 8)>0) DirectByte ^= 8;
  167.               Serial.println("South");
  168.             }
  169.             if (c == "E") {
  170.               Direct = dE;
  171.               DirectByte |= 2;
  172.               if ((DirectByte & 1)>0) DirectByte ^= 1;
  173.               Serial.println("East");
  174.             }
  175.             if (c == "W") {
  176.               Direct = dW;
  177.               DirectByte |= 1;
  178.               if ((DirectByte & 2)>0) DirectByte ^= 2;
  179.               Serial.println("West");
  180.             }
  181.             if (DirectByte>0) Status = sleewing;
  182.             else Status = tracking;
  183.        }
  184.  
  185.        if (ch == "N")
  186.        {
  187.             c = myString.charAt(1);
  188.             if (c == "N") {
  189.               Direct = dN;
  190.               if ((DirectByte & 8)>0) DirectByte ^= 8;
  191.             }
  192.             if (c == "S") {
  193.               Direct = dS;
  194.               if ((DirectByte & 4)>0) DirectByte ^= 4;
  195.             }
  196.             if (c == "E") {
  197.               Direct = dE;
  198.               if ((DirectByte & 2)>0) DirectByte ^= 2;
  199.             }
  200.             if (c == "W") {
  201.               Direct = dW;
  202.               if ((DirectByte & 1)>0) DirectByte ^= 1;
  203.             }
  204.             if (DirectByte>0) Status = sleewing;
  205.             else Status = tracking;
  206.        }
  207.  
  208.  
  209.        switch (Status) {
  210.        case none:
  211.        {
  212.           DirectByte = 0;
  213.           RAMot.setSpeed(0);
  214.           DEMot.setSpeed(0);
  215.           Serial.println("Status : NONE");
  216.           break;
  217.        }
  218.        case tracking:
  219.        {
  220.           DirectByte = 0;
  221.           RAMot.setSpeed(RA_BASESPEED);
  222.           DEMot.setSpeed(0);
  223.           Serial.println("Status : TRACKING");
  224.           break;
  225.        }
  226.        case moving:
  227.        {
  228.           RAMot.setSpeed( RATES[RA_MOVERATE] * RA_BASESPEED );
  229.           DEMot.setSpeed( RATES[DE_MOVERATE] * DE_BASESPEED );
  230.           Serial.println("Status : MOVING");
  231.           break;
  232.        }
  233.        case sleewing:
  234.        {
  235.           Serial.println("Status : SLEEWING");
  236.           // Kézi NY-ra
  237.           if ((DirectByte & 1)==1) {
  238.             RA_SPEED = RATES[RA_SLEERATE] * RA_BASESPEED;
  239.           }
  240.           // Kézi K-ra
  241.           if ((DirectByte & 2)==2) {
  242.             RA_SPEED = -RATES[RA_SLEERATE] * RA_BASESPEED;
  243.           }
  244.           // Kézi D-re
  245.           if ((DirectByte & 4)==4) {
  246.             DE_SPEED = -RATES[RA_SLEERATE] * RA_BASESPEED;
  247.           }
  248.           // Kézi É-ra
  249.           if ((DirectByte & 8)==8) {
  250.             DE_SPEED = RATES[RA_SLEERATE] * RA_BASESPEED;
  251.           }
  252.          
  253.           // Ha nincs É-D-i mozg+ás, akkor vissza óramű sebességre
  254.           if ( (DirectByte & 3) == 0 ) RA_SPEED = RA_BASESPEED;
  255.          
  256.           // Ha nincs K-NY-i mozgás, akkor a DE sebesség 0
  257.           if ( (DirectByte & 12) == 0) DE_SPEED = 0;
  258.          
  259.           RAMot.setSpeed( RA_SPEED );
  260.           DEMot.setSpeed( DE_SPEED );
  261.        }
  262.        } // End switch
  263.  
  264.        RA_SPEED = RAMot.speed();
  265.        DE_SPEED = DEMot.speed();
  266.   }
  267.  
  268.  
  269. if ((millis() % 10000)==0) {
  270.   Serial.print(RAMot.currentPosition());
  271.   Serial.print(" - ");
  272.   Serial.println(DEMot.currentPosition());
  273.   delay(10);
  274.   getCoords();
  275. //  RA_POS = RA_START + RA_SECSTEP*RAMot.currentPosition()/3600;
  276. //  DE_POS = DE_START + DE_SECSTEP*DEMot.currentPosition()/3600;
  277.   Serial.print("RA: "); Serial.print(RA_POS);
  278.   Serial.print(" DE: "); Serial.println(DE_POS);
  279.  }  
  280.  
  281. } // End LOOP
  282.  
  283.  
  284.  
  285. /// --------------------------
  286. /// Custom ISR Timer Routine
  287. /// --------------------------
  288. void timerIsr()
  289. {
  290.   RAMot.runSpeed();
  291.   DEMot.runSpeed();
  292. }
  293.  
  294. double getRA()
  295. {
  296.   RA_POS = RA_START + RA_SECSTEP*RAMot.currentPosition()/3600;
  297.   return RA_POS;
  298. }
  299.  
  300. double getDE()
  301. {
  302.   DE_POS = DE_START + DE_SECSTEP*DEMot.currentPosition()/3600;
  303.   return DE_POS;
  304. }
  305.  
  306. void getCoords()
  307. {
  308.   CURRENT_TIME = rtc.getTime();
  309.   RA_POS = RA_START + RA_SECSTEP*RAMot.currentPosition()/3600;
  310.   DE_POS = DE_START + DE_SECSTEP*DEMot.currentPosition()/3600;
  311. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!