API tools faq
paste
Advertisement
Szerelo

bcn3d moveo pre beta Arduino nano IR and Bluetooth control

Szerelo
May 12th, 2019
351
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 22.58 KB | None | 0 0
raw download clone embed print report
  1. // v 0.6    Mega 2560 Ir + Bluetooth
  2. #include <IRremote.h>
  3. #include <Wire.h>
  4. #include <LiquidCrystal_I2C.h>
  5. #include <Timer.h>
  6. #include <Servo.h>
  7. Timer t;
  8. Servo myservo;  // create servo object to control a servo
  9.  
  10. IRrecv irrecv(54);  // nano-nál 17 - A3, mega 54 - A0
  11. decode_results results;
  12. LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);  // Set the LCD I2C address
  13.  
  14. #define M1AIrany 51 //  3
  15. #define M1BIrany 31 //  13    nincs használva
  16. #define M1Lepes 49  //  4
  17. #define M2Irany 47  //  5   //  Kar forgatás
  18. #define M2Lepes 45  //  6
  19. #define M3Irany 43  //  7   //  Alkar billentés
  20. #define M3Lepes 41  //  8
  21. #define M4Irany 39  //  9   //  Csipesz billentés
  22. #define M4Lepes 37  //  10
  23. #define M5Irany 35  //  11  //  Csipesz forgatás
  24. #define M5Lepes 33  //  12
  25.  
  26.  
  27. uint8_t hossz=0, IRcode, sbc=0, motor; // sbc - serial buffer counter, value - value from serial1-bluetooth
  28. uint8_t sbuffer[10];  // serial buffer
  29. int lepes=10;
  30. long IRValue=0, oldValue=0, value;
  31. long  M1dest=0, M2dest=0, M3dest=0, M4dest=0, M5dest=0; // destination position
  32. long  M1pos=0, M2pos=0, M3pos=0, M4pos=0, M5pos=0;
  33. long  M1min=-600, M2min=-800, M3min=-1400, M4min=-50, M5min=-250;
  34. long  M1max= 600, M2max= 400, M3max= 1400, M4max= 150, M5max= 250;
  35. uint8_t M1dir, M1del, M1delc, M1stepnum;   // M1 várakozás, várakozás számláló, lépés
  36. uint8_t M2dir, M2del, M2delc, M2stepnum;  
  37. uint8_t M3dir, M3del, M3delc, M3stepnum;  
  38. uint8_t M4dir, M4del, M4delc, M4stepnum;
  39. uint8_t M5dir, M5del, M5delc, M5stepnum;
  40. // Motorok mozgás parancsai: irány, késleltetés, lépés szám. Ha irány=2 akkor nem lép, de számol
  41. uint8_t prg=2;
  42. uint8_t M1clong=57, M1cc=0; // Hány parancsot hajtson végre, 1 parancs 3 byte
  43. uint8_t M2clong=57, M2cc=0;
  44. uint8_t M3clong=57, M3cc=0;
  45. uint8_t M4clong=57, M4cc=0;
  46. uint8_t M5clong=57, M5cc=0;
  47. uint8_t M1comm[3][57] = { // 0 - stop, 1 - bal, 2 - jobb; időzítés; lépés száma
  48.   {1,50,50, 2,50,100, 0,50,50, 1,50,100, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 1,50,50, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 1,50,200, 2,50,100, 0,50,25, 2,50,100},
  49.   {1,25,100, 2,25,100, 1,25,100, 2,25,50, 0,25,50, 2,25,50, 0,25,50, 2,25,50, 0,25,50, 2,25,50, 0,25,50, 2,25,50, 0,25,50, 2,25,50, 0,25,50, 2,25,50, 1,25,50, 2,25,50, 1,25,50},
  50.   {1,9,100, 1,9,200, 2,10,100, 0,5,200, 0,5,200, 2,20,200, 1,5,100, 2,20,20, 1,10,50, 1,10,50, 2,20,50, 1,5,100, 2,20,50, 1,10,100, 2,20,50, 1,10,100, 2,20,50, 0,10,100, 0,20,100}
  51. };
  52. uint8_t M2comm[3][57] = {
  53.   {2,50,200, 0,50,20, 2,50,50, 0,50,50, 2,50,50, 1,50,100, 2,50,50, 1,50,200, 2,50,50, 1,50,100, 2,50,550, 0,50,150, 2,50,50, 0,50,150, 2,50,50, 1,50,150, 2,50,50, 0,50,50, 2,50,50},
  54.   {1,25,50, 2,25,50, 1,25,100, 2,5,20, 0,25,50, 2,5,20, 0,25,150, 2,5,20, 0,25,150, 2,5,20, 0,25,50, 2,5,20, 0,25,50, 2,5,20, 0,25,50, 2,50,20, 1,25,100, 2,50,20, 1,25,100},
  55.   {0,10,250, 0,10,250, 0,10,250, 2,20,200, 1,5,100, 2,20,50, 1,5,100, 2,20,20, 0,1,50, 2,10,50, 0,5,200, 2,20,50, 1,5,200, 2,10,100, 1,10,50, 2,10,10, 1,10,200, 2,10,10, 1,20,200}
  56. };
  57. uint8_t M3comm[3][57] = {
  58.   {1,20,250, 1,20,250, 0,20,250, 2,20,50, 1,20,250, 1,20,250, 1,20,250, 0,20,250, 2,20,100, 1,20,250, 2,20,100, 0,20,250, 2,20,100, 1,20,250, 2,20,100, 1,20,250, 2,20,100, 0,20,250, 2,20,150},
  59.   {1,10,250, 2,10,250, 1,10,250, 2,10,250, 0,10,250, 2,10,20, 0,10,250, 2,10,20, 0,10,250, 2,10,20, 0,10,250, 2,10,20, 0,10,250, 2,10,20, 0,10,250, 2,10,20, 1,10,250, 2,10,20, 1,10,250},
  60.   {0,1,250, 0,1,250, 0,1,250, 0,1,250, 0,1,250, 2,20,200, 0,5,100, 2,20,200, 1,5,50, 1,10,50, 2,20,200, 0,5,100, 2,20,200, 1,10,100, 2,20,200, 1,5,100, 2,20,200, 0,10,100, 2,20,200}
  61. };
  62. uint8_t M4comm[3][57] = {
  63.   {1,50,10, 2,50,100, 0,50,10, 2,50,50, 1,50,5, 2,50,50, 0,50,5, 2,50,50, 1,50,10, 2,50,50, 0,50,10, 2,50,50, 1,50,10, 2,50,50, 0,50,10, 2,50,50, 1,50,10, 2,50,50, 0,50,10},
  64.   {1,50,100, 2,50,200, 1,50,100, 2,50,200, 0,50,50, 2,50,200, 0,50,50, 2,50,200, 0,50,50, 2,50,200, 0,50,50, 2,50,200, 0,50,50, 2,50,200, 0,50,50, 2,50,200, 1,50,100, 2,50,200, 1,50,100},
  65.   {1,10,50, 2,200,200, 1,5,100, 2,200,200, 0,5,100, 2,200,200, 0,5,100, 2,200,200, 1,10,50, 1,100,50, 2,200,200, 0,5,100, 2,200,200, 1,10,100, 2,200,200, 1,10,100, 2,200,200, 0,10,100, 2,200,200}
  66. };
  67. uint8_t M5comm[3][57] = {
  68.   {1,50,200, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 0,50,50, 2,50,100, 0,50,100},
  69.   {1,25,100, 2,22,200, 1,25,100, 2,25,200, 0,25,50, 2,25,20, 0,25,50, 2,25,20, 0,25,50, 2,25,20, 0,25,50, 2,25,20, 0,25,50, 2,25,20, 0,25,50, 2,25,20, 1,25,100, 2,25,20, 1,25,100},
  70. //  {1,10,50, 2,20,200, 1,5,100, 2,20,200, 0,5,100, 2,20,200, 0,5,100, 2,20,200, 1,10,50, 1,10,50, 2,10,200, 0,5,100, 2,20,200, 1,10,100, 2,20,200, 1,10,100, 2,20,200, 0,10,100, 2,20,200}
  71.   {1,1,200, 2,10,250, 2,10,250, 2,20,20, 0,5,100, 2,20,20, 0,5,100, 2,20,20, 1,10,50, 1,5,50, 2,10,200, 0,5,100, 2,20,200, 0,10,100, 2,20,200, 1,10,100, 2,20,200, 0,10,100, 2,20,200}
  72. };
  73.  
  74. bool M1run=false, M2run=false, M3run=false, M4run=false, M5run=false;
  75. bool M1btrun=false, M2btrun=false, M3btrun=false, M4btrun=false, M5btrun=false;
  76.  
  77. void setup() {
  78.   Serial.begin(115200);
  79.   Serial1.begin(9600);
  80.   delay(500);
  81.   myservo.attach(13);
  82.   lcd.begin(16,2);   // lcd alaphelyzetbe állítás
  83.   lcd.clear();
  84.   lcd.print("Start ...");
  85.   int tickEvent = t.every(1, tikkel, 1 );
  86.   pinMode(M1AIrany, OUTPUT);
  87.   pinMode(M1BIrany, OUTPUT);
  88.   pinMode(M1Lepes, OUTPUT);
  89.   pinMode(M2Irany, OUTPUT);
  90.   pinMode(M2Lepes, OUTPUT);
  91.   pinMode(M3Irany, OUTPUT);
  92.   pinMode(M3Lepes, OUTPUT);
  93.   pinMode(M4Irany, OUTPUT);
  94.   pinMode(M4Lepes, OUTPUT);
  95.   pinMode(M5Irany, OUTPUT);
  96.   pinMode(M5Lepes, OUTPUT);
  97.   pinMode(A7,INPUT);
  98.   digitalWrite(M1Lepes,HIGH);
  99.   digitalWrite(M2Lepes,HIGH);
  100.   digitalWrite(M3Lepes,HIGH);
  101.   digitalWrite(M4Lepes,HIGH);
  102.   digitalWrite(M5Lepes,HIGH);
  103.   irrecv.enableIRIn(); // Start the receiver
  104.   Serial.println("Enabled IRin");
  105.  
  106. //  Serial1.print("AT+VERSION\r\n");
  107.  
  108.   lcd.setCursor(15,0);
  109.   lcd.print("+");
  110.   digitalWrite(M1AIrany,LOW);
  111.   digitalWrite(M1BIrany,HIGH);
  112.   digitalWrite(M2Irany,LOW);
  113.   digitalWrite(M3Irany,LOW);
  114.   digitalWrite(M4Irany,LOW);
  115.   digitalWrite(M5Irany,LOW);
  116.   lcd.setCursor(0,0);
  117.   lcd.print("Lepes:    ");
  118.   lcd.setCursor(7,0);
  119.   lcd.print(lepes);
  120. }
  121.  
  122. void tikkel() {
  123. //M1clong=2, M1cc=0
  124.   if (M1run==true) {    // M1 motor fut-e
  125.     //M1del,M1Delc    
  126.     M1delc++;   // késleltetés számláló
  127.     if (M1delc==M1del) { // Várakozás vége
  128.       M1delc=0;   // késleltetés számláló alaphelyzet
  129.       M1stepOne();    // M1 motor lépés
  130.       M1stepnum--;    // Lépés számláló csökkentése
  131.       if (M1stepnum==0) { // Megvolt az összes lépés
  132.         M1commread();   // Következő lépés parancs beolvasása
  133.       }
  134.     }
  135.   }
  136.   if (M2run==true) {
  137.     M2delc++;
  138.     if (M2delc==M2del) { // Várakozás vége
  139.       M2delc=0;
  140.       M2stepOne();
  141.       M2stepnum--;    // Lépés számláló csökkentése
  142.       if (M2stepnum==0) { // Megvolt az összes lépés
  143.         M2commread();
  144.       }
  145.     }
  146.   }
  147.   if (M3run==true) {
  148.     M3delc++;
  149.     if (M3delc==M3del) { // Várakozás vége
  150.       M3delc=0;
  151.       M3stepOne();
  152.       M3stepnum--;    // Lépés számláló csökkentése
  153.       if (M3stepnum==0) { // Megvolt az összes lépés
  154.         M3commread();
  155.       }
  156.     }
  157.   }
  158. /*
  159.   if (M4run==true) {
  160.     M4delc++;
  161.     if (M4delc==M4del) { // Várakozás vége
  162.       M4delc=0;
  163.       M4stepOne();
  164.       M4stepnum--;    // Lépés számláló csökkentése
  165.       if (M4stepnum==0) { // Megvolt az összes lépés
  166.         M4commread();
  167.       }
  168.     }
  169.   }
  170. */  
  171.   if (M5run==true) {
  172.     M5delc++;
  173.     if (M5delc==M5del) { // Várakozás vége
  174.       M5delc=0;
  175.       M5stepOne();
  176.       M5stepnum--;    // Lépés számláló csökkentése
  177.       if (M5stepnum==0) { // Megvolt az összes lépés
  178.         M5commread();
  179.       }
  180.     }
  181.   }
  182. }
  183.  
  184. void M1commread() {
  185.   if (M1cc<M1clong) {
  186.     Serial.print("M1 ");
  187.     Serial.print(M1cc);
  188.     M1dir=M1comm[prg][M1cc++];          
  189.     M1del=M1comm[prg][M1cc++];          
  190.     M1stepnum=M1comm[prg][M1cc++];
  191.     Serial.print(" ");
  192.     Serial.print(M1dir);
  193.     Serial.print(" ");
  194.     Serial.print(M1del);
  195.     Serial.print(" ");
  196.     Serial.print(M1stepnum);
  197.     Serial.print(" ");
  198.     Serial.println(M1cc);
  199.   } else {
  200.     M1run=false;    // Ha a parancs számláló vége, nincs lépés
  201.     Serial.println("M1 STOP");
  202.   }
  203. }
  204.  
  205. void M2commread() {
  206.   if (M2cc<M2clong) {
  207.     Serial.print("M2 ");
  208.     Serial.println(M2cc);
  209.     M2dir=M2comm[prg][M2cc++];          
  210.     M2del=M2comm[prg][M2cc++];          
  211.     M2stepnum=M2comm[prg][M2cc++];
  212.   } else {
  213.     M2run=false;    // Ha a parancs számláló vége, nincs lépés
  214.     Serial.println("M2 STOP");
  215.   }
  216. }
  217.  
  218. void M3commread() {
  219.   if (M3cc<M3clong) {
  220.     Serial.print("M3 ");
  221.     Serial.println(M3cc);
  222.     M3dir=M3comm[prg][M3cc++];          
  223.     M3del=M3comm[prg][M3cc++];          
  224.     M3stepnum=M3comm[prg][M3cc++];
  225.   } else {
  226.     M3run=false;    // Ha a parancs számláló vége, nincs lépés
  227.     Serial.println("M3 STOP");
  228.   }
  229. }
  230.  
  231. void M4commread() {
  232.   if (M4cc<M4clong) {
  233.     Serial.print("M4 ");
  234.     Serial.println(M4cc);
  235.     M4dir=M4comm[prg][M4cc++];          
  236.     M4del=M4comm[prg][M4cc++];          
  237.     M4stepnum=M4comm[prg][M4cc++];
  238.   } else {
  239.     M4run=false;    // Ha a parancs számláló vége, nincs lépés
  240.     Serial.println("M4 STOP");
  241.   }
  242. }
  243.  
  244. void M5commread() {
  245.   if (M5cc<M5clong) {
  246.     Serial.print("M5 ");
  247.     Serial.println(M5cc);
  248.     M5dir=M5comm[prg][M5cc++];          
  249.     M5del=M5comm[prg][M5cc++];          
  250.     M5stepnum=M5comm[prg][M5cc++];
  251.   } else {
  252.     M5run=false;    // Ha a parancs számláló vége, nincs lépés
  253.     Serial.println("M5 STOP");
  254.   }
  255. }
  256.  
  257. void ArmOn() {
  258. //  M1step(260,1);
  259.   M1step(520,1);
  260.   M3step(1240,1);
  261.   M5step(200,0);
  262.   M2step(240,0);
  263.   M4step(50,0);
  264. }
  265.  
  266. void ArmOff() {
  267.   M4step(50,1);
  268.   M2step(240,1);
  269.   M5step(200,1);
  270.   M3step(1240,0);
  271.   M1step(520,0);
  272. //  M1step(260,0);
  273. }
  274.  
  275. void Zeropos() {
  276.   if (M1pos!=0) {
  277.     M1step(abs(M1pos),(M1pos>0));
  278.   }
  279.   if (M3pos!=0) {
  280.     M3step(abs(M3pos),(M3pos>0));
  281.   }
  282.   if (M5pos!=0) {
  283.     M5step(abs(M5pos),(M5pos>0));
  284.   }
  285.   if (M2pos!=0) {
  286.     M2step(abs(M2pos),(M2pos>0));
  287.   }
  288.   if (M4pos!=0) {
  289.     M4step(abs(M4pos),(M4pos>0));
  290.   }
  291. }
  292.  
  293. void M1stepOne() {
  294.   if (M1dir==0) {
  295.     if (M1pos<M1max) {
  296.       M1pos++;
  297.       digitalWrite(M1AIrany,HIGH);
  298.       digitalWrite(M1BIrany,LOW);
  299.       digitalWrite(M1Lepes,LOW);
  300.       delayMicroseconds(10);
  301.       digitalWrite(M1Lepes,HIGH);
  302.     }
  303.   }
  304.   if (M1dir==1) {
  305.     if (M1pos>M1min) {
  306.       M1pos--;
  307.       digitalWrite(M1AIrany,LOW);
  308.       digitalWrite(M1BIrany,HIGH);
  309.       digitalWrite(M1Lepes,LOW);
  310.       delayMicroseconds(10);
  311.       digitalWrite(M1Lepes,HIGH);
  312.     }
  313.   }
  314. }
  315.  
  316. void M2stepOne() {
  317.   if (M2dir==0) {
  318.     if (M2pos<M2max) {
  319.       M2pos++;
  320.       digitalWrite(M2Irany,HIGH);
  321.       digitalWrite(M2Lepes,LOW);
  322.       delayMicroseconds(100);
  323.       digitalWrite(M2Lepes,HIGH);
  324.     }
  325.   }
  326.   if (M2dir==1) {
  327.     if (M2pos>M2min) {
  328.       M2pos--;
  329.       digitalWrite(M2Irany,LOW);
  330.       digitalWrite(M2Lepes,LOW);
  331.       delayMicroseconds(100);
  332.       digitalWrite(M2Lepes,HIGH);
  333.     }
  334.   }
  335. }
  336.  
  337. void M3stepOne() {
  338.   if (M3dir==0) {
  339.     if (M3pos<M3max) {
  340.       M3pos++;
  341.       digitalWrite(M3Irany,HIGH);
  342.       digitalWrite(M3Lepes,LOW);
  343.       delayMicroseconds(100);
  344.       digitalWrite(M3Lepes,HIGH);
  345.     }
  346.   }
  347.   if (M3dir==1) {
  348.     if (M3pos>M3min) {
  349.       M3pos--;
  350.       digitalWrite(M3Irany,LOW);
  351.       digitalWrite(M3Lepes,LOW);
  352.       delayMicroseconds(100);
  353.       digitalWrite(M3Lepes,HIGH);
  354.     }
  355.   }
  356. }
  357.  
  358. void M4stepOne() {
  359.   if (M4dir==0) {
  360.     if (M4pos<M4max) {
  361.       M4pos++;
  362.       digitalWrite(M4Irany,HIGH);
  363.       digitalWrite(M4Lepes,LOW);
  364.       delayMicroseconds(100);
  365.       digitalWrite(M4Lepes,HIGH);
  366.     }
  367.   }
  368.   if (M4dir==1) {
  369.     if (M4pos>M4min) {
  370.       M4pos--;
  371.       digitalWrite(M4Irany,LOW);
  372.       digitalWrite(M4Lepes,LOW);
  373.       delayMicroseconds(100);
  374.       digitalWrite(M4Lepes,HIGH);
  375.     }
  376.   }
  377. }
  378.  
  379. void M5stepOne() {
  380.   if (M5dir==0) {
  381.     if (M5pos<M5max) {
  382.       M5pos++;
  383.       digitalWrite(M5Irany,HIGH);
  384.       digitalWrite(M5Lepes,LOW);
  385.       delayMicroseconds(100);
  386.       digitalWrite(M5Lepes,HIGH);
  387.     }
  388.   }
  389.   if (M5dir==1) {
  390.     if (M5pos>M5min) {
  391.       M5pos--;
  392.       digitalWrite(M5Irany,LOW);
  393.       digitalWrite(M5Lepes,LOW);
  394.       delayMicroseconds(100);
  395.       digitalWrite(M5Lepes,HIGH);
  396.     }
  397.   }
  398. }
  399.  
  400.  
  401. void M1step(int stepnum, bool dir) {
  402.   Serial.print(" M1 dir:");
  403.   Serial.print(dir);
  404.   Serial.print(" pos: ");
  405.   Serial.print(M1pos);
  406.   for (uint16_t i=1;i<=stepnum;i++) {
  407.     if (dir==0) {
  408.       if (M1pos<M1max) {
  409.         M1pos++;
  410.         digitalWrite(M1AIrany,HIGH);
  411.         digitalWrite(M1BIrany,LOW);
  412.         digitalWrite(M1Lepes,LOW);
  413.         delay(5);
  414.         digitalWrite(M1Lepes,HIGH);
  415.         delay(5);
  416.       }
  417.     } else {
  418.       if (M1pos>M1min) {
  419.         M1pos--;
  420.         digitalWrite(M1AIrany,LOW);
  421.         digitalWrite(M1BIrany,HIGH);
  422.         digitalWrite(M1Lepes,LOW);
  423.         delay(5);
  424.         digitalWrite(M1Lepes,HIGH);
  425.         delay(5);
  426.       }
  427.     }
  428.   }
  429.   Serial.print(" ");
  430.   Serial.println(M1pos);
  431. }
  432.  
  433. void M2step(int stepnum, bool dir) {
  434.   Serial.print(" M2 dir:");
  435.   Serial.print(dir);
  436.   Serial.print(" pos: ");
  437.   Serial.print(M2pos);
  438.   for (uint16_t i=1;i<=stepnum;i++) {
  439.     if (dir==0) {
  440.       if (M2pos<M2max) {
  441.         M2pos++;
  442.         digitalWrite(M2Irany,HIGH);
  443.         digitalWrite(M2Lepes,LOW);
  444.         delay(5);
  445.         digitalWrite(M2Lepes,HIGH);
  446.         delay(5);
  447.       }
  448.     } else {
  449.       if (M2pos>M2min) {
  450.         M2pos--;
  451.         digitalWrite(M2Irany,LOW);
  452.         digitalWrite(M2Lepes,LOW);
  453.         delay(5);
  454.         digitalWrite(M2Lepes,HIGH);
  455.         delay(5);
  456.       }
  457.     }
  458.   }
  459.   Serial.print(" ");
  460.   Serial.println(M2pos);
  461. }
  462.  
  463. void M3step(int stepnum, bool dir) {
  464.   Serial.print(" M3 dir:");
  465.   Serial.print(dir);
  466.   Serial.print(" pos: ");
  467.   Serial.print(M3pos);
  468.   for (uint16_t i=1;i<=stepnum;i++) {
  469.     if (dir==0) {
  470.       if (M3pos<M3max) {
  471.         M3pos++;
  472.         digitalWrite(M3Irany,HIGH);
  473.         digitalWrite(M3Lepes,LOW);
  474.         delay(1);
  475.         digitalWrite(M3Lepes,HIGH);
  476.         delay(1);
  477.       }
  478.     } else {
  479.       if (M3pos>M3min) {
  480.         M3pos--;
  481.         digitalWrite(M3Irany,LOW);
  482.         digitalWrite(M3Lepes,LOW);
  483.         delay(1);
  484.         digitalWrite(M3Lepes,HIGH);
  485.         delay(1);
  486.       }
  487.     }
  488.   }
  489.   Serial.print(" ");
  490.   Serial.println(M3pos);
  491. }
  492.  
  493. void M4step(int stepnum, bool dir) {
  494.   Serial.print(" M4 dir:");
  495.   Serial.print(dir);
  496.   Serial.print(" pos: ");
  497.   Serial.print(M4pos);
  498.   for (uint16_t i=1;i<=stepnum;i++) {
  499.     if (dir==0) {
  500.       if (M4pos<M4max) {
  501.         M4pos++;
  502.         digitalWrite(M4Irany,HIGH);
  503.         digitalWrite(M4Lepes,LOW);
  504.         delay(1);
  505.         digitalWrite(M4Lepes,HIGH);
  506.         delay(1);
  507.       }
  508.     } else {
  509.       if (M4pos>M4min) {
  510.         M4pos--;
  511.         digitalWrite(M4Irany,LOW);
  512.         digitalWrite(M4Lepes,LOW);
  513.         delay(1);
  514.         digitalWrite(M4Lepes,HIGH);
  515.         delay(1);
  516.       }
  517.     }
  518.   }
  519.   Serial.print(" ");
  520.   Serial.println(M4pos);
  521. }
  522.  
  523. void M5step(int stepnum, bool dir) {
  524.   Serial.print(" M5 dir:");
  525.   Serial.print(dir);
  526.   Serial.print(" pos: ");
  527.   Serial.print(M5pos);
  528.   for (uint16_t i=1;i<=stepnum;i++) {
  529.     if (dir==0) {
  530.       if (M5pos<M5max) {
  531.         M5pos++;
  532.         digitalWrite(M5Irany,HIGH);
  533.         digitalWrite(M5Lepes,LOW);
  534.         delay(10);
  535.         digitalWrite(M5Lepes,HIGH);
  536.         delay(10);
  537.       }
  538.     } else {
  539.       if (M5pos>M5min) {
  540.         M5pos--;
  541.         digitalWrite(M5Irany,LOW);
  542.         digitalWrite(M5Lepes,LOW);
  543.         delay(10);
  544.         digitalWrite(M5Lepes,HIGH);
  545.         delay(10);
  546.       }
  547.     }
  548.   }
  549.   Serial.print(" ");
  550.   Serial.println(M5pos);
  551. }
  552.  
  553. void loop() {
  554.   byte dataInb;
  555.   t.update();
  556.   if (Serial1.available() > 0) {
  557.     //dataIn = Serial1.readString();  // Read the data as string
  558.     dataInb = Serial1.read();  // Read the data as string
  559.     sbuffer[sbc++]=dataInb;
  560.     if (dataInb==120) {
  561.       if (sbc==2) {
  562.         if (sbuffer[0]==83) {
  563.           Serial.println("Save");
  564.         }
  565.         if (sbuffer[0]==76) {
  566.           Serial.println("Run");
  567.         }
  568.         if (sbuffer[0]==75) {
  569.           Serial.println("Pause");
  570.         }
  571.         if (sbuffer[0]==82) {
  572.           Serial.println("Reset");
  573.         }
  574.       }
  575.       if (sbuffer[0]==77) {
  576.         Serial.print("Motor ");
  577.         Serial.println(sbuffer[1]-48);
  578.         motor=sbuffer[1]-48;
  579.         valuecalc();
  580.       }
  581.       sbc=0;
  582.     }
  583.   }  
  584.   if (M1btrun==true) M1btmove();
  585.   if (M2btrun==true) M2btmove();
  586.   if (M3btrun==true) M3btmove();
  587.   if (M4btrun==true) M4btmove();
  588.   if (M5btrun==true) M5btmove();
  589.  
  590.   if (irrecv.decode(&results)) {
  591.     IRValue=results.value;
  592.     IRkiertekel(IRValue);
  593.     IRgogo(IRcode);
  594.     irrecv.resume(); // Receive the next value
  595.   }
  596. }
  597.  
  598. void valuecalc() {
  599.   uint8_t sz=0,i ;
  600.   for ( i=1;i<=10;i++) {
  601.     if (sbuffer[i]==120 or sbuffer[i]==46) {
  602.       sz=i-1;
  603.       break;
  604.     }
  605.   }
  606.   for (i=2;i<=sz;i++) {
  607.   }
  608.   if (sz==2) {
  609.     value=sbuffer[2]-48;
  610.   }
  611.   if (sz==3) {
  612.     value=(sbuffer[2]-48)*10;
  613.     value=value+sbuffer[3]-48;
  614.   }
  615.   if (sz==4) {
  616.     value=(sbuffer[2]-48)*100;
  617.     value=value+(sbuffer[3]-48)*10;
  618.     value=value+sbuffer[4]-48;
  619.   }
  620.   Serial.print("Value ");
  621.   Serial.println(value);
  622.   if (motor==1) {
  623.     M1btrun=true;
  624.     M1dest=value*10-600;
  625.   }
  626.   if (motor==2) {
  627.     M2btrun=true;
  628.     M2dest=value*10-800;
  629.   }
  630.   if (motor==3) {
  631.     M3btrun=true;
  632.     M3dest=value*10-1400;
  633.   }
  634.   if (motor==4) {
  635.     M4btrun=true;
  636.     M4dest=value-50;
  637.   }
  638.   if (motor==5) {
  639.     M5btrun=true;
  640.     M5dest=value*10-250;
  641.   }
  642.   if (motor==6) {
  643.     myservo.write(value);
  644.   }
  645.   //servopoz();
  646. }
  647.  
  648. void servopoz() {
  649.   for (int i=0;i<=10;i++) {
  650.     digitalWrite(13,HIGH);
  651.     delayMicroseconds(value*1000);
  652.     digitalWrite(13,LOW);
  653.     delay(20);
  654.   }
  655. }
  656.  
  657. void M1btmove() {
  658.   M1dir=0;
  659.   if (M1pos>M1dest) M1dir=1;
  660.   M1stepOne();
  661.   delay(10);
  662.   if (M1pos==M1dest) M1btrun=false;
  663. }
  664.  
  665. void M2btmove() {
  666.   M2dir=0;
  667.   if (M2pos>M2dest) M2dir=1;
  668.   M2stepOne();
  669.   delay(5);
  670.   if (M2pos==M2dest) M2btrun=false;
  671. }
  672.  
  673. void M3btmove() {
  674.   M3dir=0;
  675.   if (M3pos>M3dest) M3dir=1;
  676. /*  Serial.print("M3dest:");
  677.   Serial.print(M3dest);
  678.   Serial.print(" M3pos:");
  679.   Serial.print(M3pos);
  680.   Serial.print(" M3dir:");
  681.   Serial.println(M3dir); */
  682.   M3stepOne();
  683.   delay(5);
  684.   if (M3pos==M3dest)  M3btrun=false;
  685. }
  686.  
  687. void M4btmove() {
  688.   M4dir=0;
  689.   if (M4pos>M4dest) M4dir=1;
  690.   M4stepOne();
  691.   delay(10);
  692.   if (M4pos==M4dest) M4btrun=false;
  693. }
  694.  
  695. void M5btmove() {
  696.   M5dir=0;
  697.   if (M5pos>M5dest) M5dir=1;
  698.   M5stepOne();
  699.   delay(5);
  700.   if (M5pos==M5dest) M5btrun=false;
  701. }
  702.  
  703. void IRgogo (byte i) {
  704.   if (i==10)  lepes=10;
  705.   if (i==11)  lepes=100;
  706.   if (i==12)  lepes=200;
  707.   lcd.setCursor(0,0);
  708.   lcd.print("Lepes:    ");
  709.   lcd.setCursor(7,0);
  710.   lcd.print(lepes);
  711.   if (i==13) { // balra
  712.     lcd.setCursor(15,0);
  713.     lcd.print("-");
  714.     digitalWrite(M1AIrany,HIGH);
  715.     digitalWrite(M1BIrany,LOW);
  716.     digitalWrite(M2Irany,HIGH);
  717.     digitalWrite(M3Irany,HIGH);
  718.     digitalWrite(M4Irany,HIGH);
  719.     digitalWrite(M5Irany,HIGH);
  720.   }
  721.   if (i==14) { // jobbra
  722.     lcd.setCursor(15,0);
  723.     lcd.print("+");
  724.     digitalWrite(M1AIrany,LOW);
  725.     digitalWrite(M1BIrany,HIGH);
  726.     digitalWrite(M2Irany,LOW);
  727.     digitalWrite(M3Irany,LOW);
  728.     digitalWrite(M4Irany,LOW);
  729.     digitalWrite(M5Irany,LOW);
  730.   }
  731.   if (i==1)  M1step(lepes,!digitalRead(M1AIrany));
  732.   if (i==2)  M2step(lepes,!digitalRead(M2Irany));
  733.   if (i==3)  M3step(lepes,!digitalRead(M3Irany));
  734.   if (i==4)  M4step(lepes,!digitalRead(M4Irany));
  735.   if (i==5)  M5step(lepes,!digitalRead(M5Irany));
  736.   if (i==15)  Zeropos(); // EQ
  737.   if (i==16) {  // << Start prg1
  738.     prg=0;
  739.     prgstart();
  740.   }
  741.   if (i==17) {  // >> Start prg2
  742.     prg=1;
  743.     prgstart();
  744.   }
  745.   if (i==18) {  // >I Start prg3
  746.     prg=2;
  747.     prgstart();
  748.   }
  749.   if (i==19)  ArmOff(); // CH-
  750.   if (i==21)  ArmOn();  // CH+
  751.   if (i==20) { // CH pozíció törlés
  752.     M1pos=0;
  753.     M2pos=0;
  754.     M3pos=0;
  755.     M4pos=0;
  756.     M5pos=0;
  757.   }
  758. }
  759.  
  760. void prgstart() {
  761.   if (M1run==true or M2run==true or M3run==true or M4run==true or M5run==true ) {
  762.     M1run=false;
  763.     M2run=false;
  764.     M3run=false;
  765.     M4run=false;
  766.     M5run=false;
  767.     Zeropos();  // Alaphelyzetbe állás
  768.   }
  769.   M1cc=0;
  770.   M2cc=0;
  771.   M3cc=0;
  772.   M4cc=0;
  773.   M5cc=0;
  774.   M1commread();
  775.   M2commread();
  776.   M3commread();
  777.   M4commread();
  778.   M5commread();
  779.   M1run=true;
  780.   M2run=true;
  781.   M3run=true;
  782.   //M4run=true;
  783.   M5run=true;
  784. }
  785.  
  786. void IRkiertekel(long i) {
  787.   //Serial.print(i);
  788.   IRcode=0;
  789.   switch(i){
  790.     case 16738455:
  791.       IRcode=10;
  792.       Serial.println("0");
  793.       break;
  794.     case 16753245:
  795.       IRcode=19;
  796.       Serial.println("CH-");
  797.       break;
  798.     case 16736925:
  799.       IRcode=20;
  800.       Serial.println("CH");
  801.       break;
  802.     case 16769565:
  803.       IRcode=21;
  804.       Serial.println("CH+");
  805.       break;
  806.     case 16761405:
  807.       IRcode=18;
  808.       Serial.println(">I");
  809.       break;
  810.     case 16720605:
  811.       IRcode=16;
  812.       Serial.println("<<");
  813.       break;
  814.     case 16712445:
  815.       IRcode=17;
  816.       Serial.println(">>");
  817.       break;
  818.     case 16769055:
  819.       IRcode=13;
  820.       Serial.println("-");
  821.       break;
  822.     case 16754775:
  823.       IRcode=14;
  824.       Serial.println("+");
  825.       break;
  826.     case 16748655:
  827.       IRcode=15;
  828.       Serial.println("EQ");
  829.       break;
  830.     case 16724175:          //1
  831.       IRcode=1;
  832.       Serial.println("1");
  833.       break;
  834.     case 16718055:          // 2
  835.       IRcode=2;
  836.       Serial.println("2");
  837.       break;
  838.     case 16743045:          // 3
  839.       IRcode=3;
  840.       Serial.println("3");
  841.       break;
  842.     case 16716015:          // 4
  843.       IRcode=4;
  844.       Serial.println("4");
  845.       break;
  846.     case 16726215:          // 5
  847.       IRcode=5;
  848.       Serial.println("5");
  849.       break;
  850.     case 16734885:          // 6
  851.       IRcode=6;
  852.       Serial.println("6");
  853.       break;
  854.     case 16728765:          // 7
  855.       IRcode=7;
  856.       Serial.println("7");
  857.       break;
  858.     case 16730805:          // 8
  859.       IRcode=8;
  860.       Serial.println("8");
  861.       break;
  862.     case 16732845:          // 9
  863.       IRcode=9;
  864.       Serial.println("9");
  865.       break;
  866.     case 16750695:         // 100+
  867.       IRcode=11;
  868.       Serial.println("100+");
  869.       break;
  870.     case 16756815:         // 200+
  871.       IRcode=12;
  872.       Serial.println("200+");
  873.       break;
  874.   }
  875. }
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!