API tools faq
paste
Advertisement
Szerelo

4D robot arm with RC servo - potentiometer versions

Szerelo
Aug 28th, 2020
2,057
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 12.41 KB | None | 0 0
raw download clone embed print report
  1. // 4D robotarm
  2. // v 0.1 Beta-    Nano Poti
  3.  
  4. #include <EEPROM.h>
  5. #include <Wire.h>
  6. #include <LiquidCrystal_I2C.h>
  7. #include <Servo.h>
  8. #include <Rotary.h>
  9. Servo servo1;  // create servo object to control a servo
  10. Servo servo2;
  11. Servo servo3;
  12. Servo servo4;
  13.  
  14. LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);  // Set the LCD I2C address
  15. Rotary r = Rotary(2, 3);            // define rotary encoder and pins
  16.  
  17. #define Gomb 5   //  Nyomógomb
  18. #define RGomb 4  //  Enkóder gombja
  19.  
  20. unsigned char result;
  21. uint8_t sbc=0, motor; // sbc - serial buffer counter
  22. uint8_t sbuffer[100];  // serial buffer
  23. long counter=0, count1=1;
  24. uint8_t  M1dest=90, M2dest=90, M3dest=90, M4dest=90; // destination position
  25. uint8_t  M1pos=90, M2pos=90, M3pos=90, M4pos=90;     // actual pos
  26. uint8_t value, stepcounter=0, stepcounter1=0, maxstepcounter=0;
  27. bool RUN=false; // RUN status
  28. uint8_t steps[5][100] = {};
  29. uint8_t servoMM[5][2]={};
  30. byte value1, value2, value3, value4;
  31. byte sp=2;    // Speed, 1-Slow, 2-Normal, 3-Fast
  32. String sps="Normal";
  33. bool menu=false;
  34. byte menupos=0;
  35.  
  36. const char string_0[] PROGMEM = "    Continue    ";
  37. const char string_1[] PROGMEM = "      Reset     ";
  38. const char string_2[] PROGMEM = "      Stop      ";
  39. const char string_3[] PROGMEM = "    RUN Manual  ";
  40. const char string_4[] PROGMEM = "    RUN Auto    ";
  41. const char string_5[] PROGMEM = "      Slow      ";
  42. const char string_6[] PROGMEM = "     Normal     ";
  43. const char string_7[] PROGMEM = "      Fast      ";
  44. const char *const stable[] PROGMEM = {string_0, string_1, string_2, string_3, string_4, string_5, string_6, string_7};
  45. char buffer[17];
  46.  
  47.  
  48. void setup() {
  49.   pinMode(Gomb,INPUT_PULLUP);
  50.   pinMode(RGomb,INPUT_PULLUP);
  51.   Serial.begin(115200);
  52.   r.begin(true);
  53.   servo1.attach(6);
  54.   servo2.attach(11);
  55.   servo3.attach(10);
  56.   servo4.attach(9);
  57.   lcd.begin(16,2);   // lcd init
  58.   servoMM[1][0]=EEPROM.read(1);   // servo1 min
  59.   servoMM[1][1]=EEPROM.read(2);   // servo1 max
  60.   servoMM[2][0]=EEPROM.read(3);
  61.   servoMM[2][1]=EEPROM.read(4);
  62.   servoMM[3][0]=EEPROM.read(5);
  63.   servoMM[3][1]=EEPROM.read(6);
  64.   servoMM[4][0]=EEPROM.read(7);
  65.   servoMM[4][1]=EEPROM.read(8);
  66.   if (digitalRead(Gomb)==LOW) {
  67.     lcd.setCursor(0,0);
  68.     lcd.print("Setting     ");
  69.     minMaxSet();
  70.   }
  71.   count1=1;
  72.   if (sp==1) count1=20;
  73.   if (sp==2) count1=10;
  74.   kiir1();
  75. }
  76.  
  77. void kiir1() {
  78.   lcd.clear();
  79.   lcd.print("Manual position");
  80.   lcd.setCursor(0,1);
  81.   lcd.print("Step:    ");
  82.   lcd.setCursor(6,1);
  83.   lcd.print(stepcounter+1);
  84.   sps="Normal";
  85.   if (sp==1) sps="Slow  ";
  86.   if (sp==3) sps="Fast  ";
  87.   lcd.setCursor(10,1);
  88.   lcd.print(sps);
  89. }
  90.  
  91. void menukiir() {
  92.   //long asd;
  93.   lcd.clear();
  94.   lcd.print("      Menu      ");
  95.   lcd.setCursor(0,1);
  96.   strcpy_P(buffer, (char *)pgm_read_word(&(stable[menupos])));
  97.   lcd.print(buffer);
  98. }
  99.  
  100. void loop() {
  101.   result = r.process();
  102.   if (result == DIR_CW or result == DIR_CCW) {   // enkoder mozgás, lcd LED bekapcsolása
  103.     //lcd.noBacklight();
  104.   }
  105.   if (result==DIR_CW and RUN==false and menu==true) {
  106.     menupos++;
  107.     if (menupos>=8) menupos=7;    
  108.     menukiir();
  109.   }
  110.   if (result==DIR_CCW and RUN==false and menu==true) {
  111.     menupos--;
  112.     if (menupos==255) menupos=0;    
  113.     menukiir();
  114.   }
  115.   if (digitalRead(Gomb)==LOW and menu==false) {   // Save position
  116.     steps[1][stepcounter] = M1dest;
  117.     steps[2][stepcounter] = M2dest;
  118.     steps[3][stepcounter] = M3dest;
  119.     steps[4][stepcounter] = M4dest;
  120.     stepcounter++;
  121.     maxstepcounter=stepcounter;
  122.     if (stepcounter>=99) stepcounter=98;
  123.     kiir1();
  124.     delay(500);
  125.   }
  126.   if (digitalRead(RGomb)==LOW and menu==false) {
  127.     RUN=false;
  128.     menu=true;
  129.     menukiir();
  130.   }
  131.   if (RUN==false) {
  132.     M1dest=map(analogRead(A0),0,1024,servoMM[1][0],servoMM[1][1]);
  133.     M2dest=map(analogRead(A1),0,1024,servoMM[2][0],servoMM[2][1]);
  134.     M3dest=map(analogRead(A2),0,1024,servoMM[3][0],servoMM[3][1]);
  135.     M4dest=map(analogRead(A3),0,1024,servoMM[4][0],servoMM[4][1]);
  136.   }
  137.   counter++;
  138.   if (counter>=count1) {
  139.     counter=0;
  140.     if (M1dest>M1pos) {
  141.       M1pos++;
  142.       servo1.write(M1pos);
  143.     }
  144.     if (M1dest<M1pos) {
  145.       M1pos--;
  146.       servo1.write(M1pos);
  147.     }
  148.     if (M2dest>M2pos) {
  149.       M2pos++;
  150.       servo2.write(M2pos);
  151.     }
  152.     if (M2dest<M2pos) {
  153.       M2pos--;
  154.       servo2.write(M2pos);
  155.     }
  156.     if (M3dest>M3pos) {
  157.       M3pos++;
  158.       servo3.write(M3pos);
  159.     }
  160.     if (M3dest<M3pos) {
  161.       M3pos--;
  162.       servo3.write(M3pos);
  163.     }
  164.     if (M4dest>M4pos) {
  165.       M4pos++;
  166.       servo4.write(M4pos);
  167.     }
  168.     if (M4dest<M4pos) {
  169.       M4pos--;
  170.       servo4.write(M4pos);
  171.     }
  172.     if (M1dest==M1pos and M2dest==M2pos and M3dest==M3pos and M4dest==M4pos and RUN==true) {   // Ha mind a 4 motor a pozícióján van
  173.       if (stepcounter1<stepcounter) {   // Ha a futás közbeni lépés számláló kissebb, mint a lépés tároló, akkor a következő lépésre lép
  174.         M1dest = steps[1][stepcounter1];
  175.         M2dest = steps[2][stepcounter1];
  176.         M3dest = steps[3][stepcounter1];
  177.         M4dest = steps[4][stepcounter1];
  178.         lcd.clear();
  179.         lcd.print(M1dest);
  180.         lcd.print(" ");
  181.         lcd.print(M2dest);
  182.         lcd.print(" ");
  183.         lcd.print(M3dest);
  184.         lcd.print(" ");
  185.         lcd.print(M4dest);
  186.         lcd.setCursor(0,1);
  187.         lcd.print(stepcounter1);
  188.         delay(500);
  189.         stepcounter1++;  // Futás közbeni lépésszámláló növelése    
  190.       }
  191.       else {  // A futás közbeni lépés számláló nagyobb, mint a lépés tároló, nincs több lépés, futás vége
  192.         RUN=false;
  193.       }
  194.     }
  195.   }
  196.   //delay(10);
  197. }
  198.  
  199. void valuecalc() {
  200.   uint8_t sz=0,i ;
  201.   for ( i=1;i<=10;i++) {  // Serial buffer check
  202.     if (sbuffer[i]==120 or sbuffer[i]==46) {  // If x or ., end the checking
  203.       sz=i-1;   // Serial buffel -1, chunk the end marker
  204.       break;
  205.     }
  206.   }
  207.   //for (i=2;i<=sz;i++) {
  208.   //}
  209.   if (sz==2) {  // If the serial buffer size 3 char (exp. M18 - M - motor, 1 - motor number, 8 - position value), then the data size only 1 char
  210.     value=sbuffer[2]-48;
  211.   }
  212.   if (sz==3) {  // If the serial buffer size 4 char (exp. M247 - M - motor, 2 - motor number, 47 - position value), then the data size 2 char
  213.     value=(sbuffer[2]-48)*10;
  214.     value=value+sbuffer[3]-48;
  215.   }
  216.   if (sz==4) {  // If the serial buffer size 5 char (exp. M4152 - M - motor, 4 - motor number, 152 - position value), then the data size 3 char
  217.     value=(sbuffer[2]-48)*100;
  218.     value=value+(sbuffer[3]-48)*10;
  219.     value=value+sbuffer[4]-48;
  220.   }
  221.   if (motor==1) {
  222.     M1dest=map(value,0,180,servoMM[1][0],servoMM[1][1]);
  223.   }
  224.   if (motor==2) {
  225.     M2dest=map(value,0,180,servoMM[2][0],servoMM[2][1]);
  226.   }
  227.   if (motor==3) {
  228.     M3dest=map(value,0,180,servoMM[3][0],servoMM[3][1]);
  229.   }
  230.   if (motor==4) {
  231.     M4dest=map(value,0,180,servoMM[4][0],servoMM[4][1]);
  232.   }
  233. }
  234.  
  235. void minMaxSet () {
  236.   delay(500);
  237.   while (digitalRead(Gomb)==LOW);
  238.   lcd.setCursor(0,0);
  239.   lcd.print("Min:  ");
  240.   delay(500);
  241.   while (digitalRead(Gomb)==HIGH) {
  242.     value1=map(analogRead(A0),0,1024,0,180);
  243.     value2=map(analogRead(A1),0,1024,0,180);
  244.     value3=map(analogRead(A2),0,1024,0,180);
  245.     value4=map(analogRead(A3),0,1024,0,180);
  246.     servo1.write(value1);
  247.     servo2.write(value2);
  248.     servo3.write(value3);
  249.     servo4.write(value4);
  250.     lcd.setCursor(6,0);
  251.     lcd.print(value1);
  252.     lcd.print("  ");
  253.     lcd.setCursor(12,0);
  254.     lcd.print(value2);
  255.     lcd.print("  ");
  256.     lcd.setCursor(6,1);
  257.     lcd.print(value3);
  258.     lcd.print("  ");
  259.     lcd.setCursor(12,1);
  260.     lcd.print(value4);
  261.     lcd.print("  ");
  262.   }
  263.   delay(500);
  264.   while (digitalRead(Gomb)==LOW);
  265.   servoMM[1][0]=value1;
  266.   servoMM[2][0]=value2;
  267.   servoMM[3][0]=value3;
  268.   servoMM[4][0]=value4;
  269.   EEPROM.write(1,value1);  // servo1 min
  270.   EEPROM.write(3,value2);
  271.   EEPROM.write(5,value3);
  272.   EEPROM.write(6,value4);
  273.   lcd.clear();
  274.   lcd.print("Max:");
  275.   delay(500);
  276.   while (digitalRead(Gomb)==HIGH) {
  277.     value1=map(analogRead(A0),0,1024,0,180);
  278.     value2=map(analogRead(A1),0,1024,0,180);
  279.     value3=map(analogRead(A2),0,1024,0,180);
  280.     value4=map(analogRead(A3),0,1024,0,180);
  281.     servo1.write(value1);
  282.     servo2.write(value2);
  283.     servo3.write(value3);
  284.     servo4.write(value4);
  285.     lcd.setCursor(6,0);
  286.     lcd.print(value1);
  287.     lcd.print("  ");
  288.     lcd.setCursor(12,0);
  289.     lcd.print(value2);
  290.     lcd.print("  ");
  291.     lcd.setCursor(6,1);
  292.     lcd.print(value3);
  293.     lcd.print("  ");
  294.     lcd.setCursor(12,1);
  295.     lcd.print(value4);
  296.     lcd.print("  ");
  297.   }
  298.   delay(500);
  299.   while (digitalRead(Gomb)==LOW);
  300.   servoMM[1][1]=value1;
  301.   servoMM[2][1]=value2;
  302.   servoMM[3][1]=value3;
  303.   servoMM[4][1]=value4;
  304.   EEPROM.write(2,value1);  // servo1 max
  305.   EEPROM.write(4,value2);
  306.   EEPROM.write(6,value3);
  307.   EEPROM.write(8,value4);
  308.   lcd.clear();
  309.   lcd.print("Test: ");
  310.   delay(500);
  311.   while (digitalRead(Gomb)==HIGH) {
  312.     value1=map(analogRead(A0),0,1024,servoMM[1][0],servoMM[1][1]);
  313.     value2=map(analogRead(A1),0,1024,servoMM[2][0],servoMM[2][1]);
  314.     value3=map(analogRead(A2),0,1024,servoMM[3][0],servoMM[3][1]);
  315.     value4=map(analogRead(A3),0,1024,servoMM[4][0],servoMM[4][1]);
  316.     servo1.write(value1);
  317.     servo2.write(value2);
  318.     servo3.write(value3);
  319.     servo4.write(value4);
  320.     lcd.setCursor(6,0);
  321.     lcd.print(value1);
  322.     lcd.print("  ");
  323.     lcd.setCursor(12,0);
  324.     lcd.print(value2);
  325.     lcd.print("  ");
  326.     lcd.setCursor(6,1);
  327.     lcd.print(value3);
  328.     lcd.print("  ");
  329.     lcd.setCursor(12,1);
  330.     lcd.print(value4);
  331.     lcd.print("  ");
  332.   }
  333.   delay(500);
  334.   while (digitalRead(Gomb)==LOW);
  335.   lcd.clear();
  336.   lcd.print("Setting OK.");
  337.   delay(500);
  338. }
  339.  
  340. void BTserial () {
  341.   byte dataInb;
  342.   if (Serial.available() > 0) {
  343.     dataInb = Serial.read();  // Read the data as string
  344.     sbuffer[sbc++]=dataInb;
  345.     if (dataInb==120) {   //  120 - x, the end marker
  346.       if (sbc==2) {   // If serial data counter 2, check the first char. The second char is end marker
  347.         if (sbuffer[0]==65) {   // A - slow
  348.           count1=8000;
  349.           counter=0;
  350.           lcd.setCursor(0,0);
  351.           lcd.print("SLOW            ");
  352.         }
  353.         if (sbuffer[0]==66) {   // B - normal
  354.           count1=2000;
  355.           counter=0;
  356.           lcd.setCursor(0,0);
  357.           lcd.print("NORMAL          ");
  358.         }
  359.         if (sbuffer[0]==67) {   // C - fast
  360.           count1=1;
  361.           counter=0;
  362.           lcd.setCursor(0,0);
  363.           lcd.print("FAST            ");
  364.         }
  365.         if (sbuffer[0]==83) {   // S - Save
  366.           lcd.clear();
  367.           lcd.print(M1dest);
  368.           lcd.print(" ");
  369.           lcd.print(M2dest);
  370.           lcd.print(" ");
  371.           lcd.print(M3dest);
  372.           lcd.print(" ");
  373.           lcd.print(M4dest);
  374.           lcd.setCursor(0,1);
  375.           lcd.print(stepcounter);
  376.           steps[1][stepcounter] = M1dest;
  377.           steps[2][stepcounter] = M2dest;
  378.           steps[3][stepcounter] = M3dest;
  379.           steps[4][stepcounter] = M4dest;
  380.           stepcounter++;
  381.           if (stepcounter>=100) stepcounter=99;
  382.           lcd.setCursor(0,0);
  383.           lcd.print("SAVE:  ");
  384.           lcd.setCursor(0,7);
  385.           lcd.print(stepcounter);
  386.           lcd.print("  ");
  387.         }
  388.         if (sbuffer[0]==76) {   // L - Run
  389.           if (stepcounter1>=stepcounter) stepcounter1=0;  // Ha végig ment a program, akkor a lépésszámláló nullázódik
  390.           if (stepcounter>0) RUN=true;   // Van-e valamilyen érték a lépéstárolóban
  391.           lcd.setCursor(0,0);
  392.           lcd.print("RUN             ");
  393.         }
  394.         if (sbuffer[0]==75) {   // K - Pause
  395.           RUN=false;
  396.           lcd.setCursor(0,0);
  397.           lcd.print("PAUSE           ");
  398.         }
  399.         if (sbuffer[0]==82) {   // R - Reset
  400.           RUN=false;
  401.           stepcounter=0;
  402.           stepcounter1=0;
  403.           M1dest=M1pos;
  404.           M2dest=M2pos;
  405.           M3dest=M3pos;
  406.           M4dest=M4pos;
  407.           lcd.setCursor(0,0);
  408.           lcd.print("RESET           ");
  409.         }
  410.       }
  411.       if (sbuffer[0]==77 and RUN==false) {  // M - motor, csak akkor vesz figyelembe új értéket, ha áll
  412.         motor=sbuffer[1]-48;  // Selected motor number. second char the position value
  413.         valuecalc();
  414.         lcd.setCursor(0,0);
  415.         lcd.print("Motor:  ");
  416.         lcd.setCursor(0,7);
  417.         lcd.print(motor);
  418.         lcd.print("  ");
  419.       }
  420.       sbc=0;
  421.     }
  422.   }  // Serial END
  423.  
  424. }
  425.  
  426. //END
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!