API tools faq
paste
Szerelo

4D robot arm with RC servo

Szerelo
Aug 21st, 2020 (edited)
2,080
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 11.41 KB | None | 0 0
raw download clone embed print report
  1. // 4D robotarm
  2. // v 0.9    Nano Bluetooth
  3. // Need application!
  4.  
  5. #include <EEPROM.h>
  6. #include <Wire.h>
  7. #include <LiquidCrystal_I2C.h>
  8. #include <Servo.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.  
  16.  
  17. uint8_t sbc=0, motor; // sbc - serial buffer counter
  18. uint8_t sbuffer[100];  // serial buffer
  19. int lepes=10;
  20. long counter=0, count1=2000;
  21. uint8_t  M1dest=90, M2dest=90, M3dest=90, M4dest=90; // destination position
  22. uint8_t  M1pos=90, M2pos=90, M3pos=90, M4pos=90;     // actual pos
  23. uint8_t value, stepcounter=0, stepcounter1=0;
  24. bool RUN=false; // RUN status
  25. uint8_t steps[5][100] = {};
  26. uint8_t servoMM[5][2]={};
  27.  
  28. void setup() {
  29.   pinMode(A1,INPUT_PULLUP);
  30.   Serial.begin(9600);
  31.   servo1.attach(6);
  32.   servo2.attach(11);
  33.   servo3.attach(10);
  34.   servo4.attach(9);
  35.   lcd.begin(16,2);   // lcd init
  36.   lcd.clear();
  37.   lcd.print("Start ...");
  38.   servoMM[1][0]=EEPROM.read(1);   // servo1 min
  39.   servoMM[1][1]=EEPROM.read(2);   // servo1 max
  40.   servoMM[2][0]=EEPROM.read(3);
  41.   servoMM[2][1]=EEPROM.read(4);
  42.   servoMM[3][0]=EEPROM.read(5);
  43.   servoMM[3][1]=EEPROM.read(6);
  44.   servoMM[4][0]=EEPROM.read(7);
  45.   servoMM[4][1]=EEPROM.read(8);
  46.   if (digitalRead(A1)==LOW) {
  47.     lcd.setCursor(0,0);
  48.     lcd.print("Setting     ");
  49.     minMaxSet();
  50.   }
  51. }
  52.  
  53.  
  54. void loop() {
  55.   byte dataInb;
  56.   if (Serial.available() > 0) {
  57.     dataInb = Serial.read();  // Read the data as byte
  58.     sbuffer[sbc++]=dataInb;
  59.     if (dataInb==120) {   //  120 - x, the end marker
  60.       if (sbc==2) {   // If serial data counter 2, check the first char. The second char is end marker
  61.         if (sbuffer[0]==65) {   // A - slow
  62.           count1=8000;
  63.           counter=0;
  64.           lcd.setCursor(0,0);
  65.           lcd.print("SLOW            ");
  66.         }
  67.         if (sbuffer[0]==66) {   // B - normal
  68.           count1=2000;
  69.           counter=0;
  70.           lcd.setCursor(0,0);
  71.           lcd.print("NORMAL          ");
  72.         }
  73.         if (sbuffer[0]==67) {   // C - fast
  74.           count1=1;
  75.           counter=0;
  76.           lcd.setCursor(0,0);
  77.           lcd.print("FAST            ");
  78.         }
  79.         if (sbuffer[0]==83) {   // S - Save
  80.           lcd.clear();
  81.           lcd.print(M1dest);
  82.           lcd.print(" ");
  83.           lcd.print(M2dest);
  84.           lcd.print(" ");
  85.           lcd.print(M3dest);
  86.           lcd.print(" ");
  87.           lcd.print(M4dest);
  88.           lcd.setCursor(0,1);
  89.           lcd.print(stepcounter);
  90.           steps[1][stepcounter] = M1dest;
  91.           steps[2][stepcounter] = M2dest;
  92.           steps[3][stepcounter] = M3dest;
  93.           steps[4][stepcounter] = M4dest;
  94.           stepcounter++;
  95.           if (stepcounter>=100) stepcounter=99;
  96.           lcd.setCursor(0,0);
  97.           lcd.print("SAVE:  ");
  98.           lcd.setCursor(0,7);
  99.           lcd.print(stepcounter);
  100.           lcd.print("  ");
  101.         }
  102.         if (sbuffer[0]==76) {   // L - Run
  103.           if (stepcounter1>=stepcounter) stepcounter1=0;  // Ha végig ment a program, akkor a lépésszámláló nullázódik
  104.           if (stepcounter>0) RUN=true;   // Van-e valamilyen érték a lépéstárolóban
  105.           lcd.setCursor(0,0);
  106.           lcd.print("RUN             ");
  107.         }
  108.         if (sbuffer[0]==75) {   // K - Pause
  109.           RUN=false;
  110.           lcd.setCursor(0,0);
  111.           lcd.print("PAUSE           ");
  112.         }
  113.         if (sbuffer[0]==82) {   // R - Reset
  114.           RUN=false;
  115.           stepcounter=0;
  116.           stepcounter1=0;
  117.           M1dest=M1pos;
  118.           M2dest=M2pos;
  119.           M3dest=M3pos;
  120.           M4dest=M4pos;
  121.           lcd.setCursor(0,0);
  122.           lcd.print("RESET           ");
  123.         }
  124.       }
  125.       if (sbuffer[0]==77 and RUN==false) {  // M - motor, csak akkor vesz figyelembe új értéket, ha áll
  126.         motor=sbuffer[1]-48;  // Selected motor number. second char the position value
  127.         valuecalc();
  128.         lcd.setCursor(0,0);
  129.         lcd.print("Motor:  ");
  130.         lcd.setCursor(0,7);
  131.         lcd.print(motor);
  132.         lcd.print("  ");
  133.       }
  134.       sbc=0;
  135.     }
  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(1000);
  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.   while (digitalRead(A1)==LOW);
  237.   lcd.setCursor(0,0);
  238.   lcd.print("Servo1 min:");
  239.   while (digitalRead(A1)==HIGH) {
  240.     value=map(analogRead(A0),0,1024,0,180);
  241.     servo1.write(value);
  242.     lcd.setCursor(12,0);
  243.     lcd.print(value);
  244.     lcd.print("  ");
  245.   }
  246.   while (digitalRead(A1)==LOW);
  247.   servoMM[1][0]=value;
  248.   EEPROM.write(1,value);  // servo1 min
  249.   lcd.setCursor(0,0);
  250.   lcd.print("Servo1 max:");
  251.   while (digitalRead(A1)==HIGH) {
  252.     value=map(analogRead(A0),0,1024,0,180);
  253.     servo1.write(value);
  254.     lcd.setCursor(12,0);
  255.     lcd.print(value);
  256.     lcd.print("  ");
  257.   }
  258.   while (digitalRead(A1)==LOW);
  259.   servoMM[1][1]=value;
  260.   EEPROM.write(2,value);  // servo1 max
  261.   lcd.setCursor(0,0);
  262.   lcd.print("Servo1 test: ");
  263.   while (digitalRead(A1)==HIGH) {
  264.     value=map(analogRead(A0),0,1024,servoMM[1][0],servoMM[1][1]);
  265.     servo1.write(value);
  266.     lcd.setCursor(13,0);
  267.     lcd.print(value);
  268.     lcd.print("  ");
  269.     lcd.setCursor(0,1);
  270.     lcd.print(servoMM[1][0]);
  271.     lcd.print("  ");
  272.     lcd.print(servoMM[1][1]);
  273.     lcd.print("  ");
  274.   }
  275.   while (digitalRead(A1)==LOW);
  276.   lcd.clear();
  277.   lcd.print("Servo2 min:");
  278. // SERVO2
  279.   while (digitalRead(A1)==HIGH) {
  280.     value=map(analogRead(A0),0,1024,0,180);
  281.     servo2.write(value);
  282.     lcd.setCursor(12,0);
  283.     lcd.print(value);
  284.     lcd.print("  ");
  285.   }
  286.   while (digitalRead(A1)==LOW);
  287.   servoMM[2][0]=value;
  288.   EEPROM.write(3,value);  // servo1 min
  289.   lcd.setCursor(0,0);
  290.   lcd.print("Servo2 max:");
  291.   while (digitalRead(A1)==HIGH) {
  292.     value=map(analogRead(A0),0,1024,0,180);
  293.     servo2.write(value);
  294.     lcd.setCursor(12,0);
  295.     lcd.print(value);
  296.     lcd.print("  ");
  297.   }
  298.   while (digitalRead(A1)==LOW);
  299.   servoMM[2][1]=value;
  300.   EEPROM.write(4,value);  // servo1 max
  301.   lcd.setCursor(0,0);
  302.   lcd.print("Servo2 test: ");
  303.   while (digitalRead(A1)==HIGH) {
  304.     value=map(analogRead(A0),0,1024,servoMM[2][0],servoMM[2][1]);
  305.     servo2.write(value);
  306.     lcd.setCursor(13,0);
  307.     lcd.print(value);
  308.     lcd.print("  ");
  309.     lcd.setCursor(0,1);
  310.     lcd.print(servoMM[2][0]);
  311.     lcd.print("  ");
  312.     lcd.print(servoMM[2][1]);
  313.     lcd.print("  ");
  314.   }
  315.   while (digitalRead(A1)==LOW);
  316.   lcd.clear();
  317.   lcd.print("Servo3 min:");
  318. // SERVO3
  319.   while (digitalRead(A1)==HIGH) {
  320.     value=map(analogRead(A0),0,1024,0,180);
  321.     servo3.write(value);
  322.     lcd.setCursor(12,0);
  323.     lcd.print(value);
  324.     lcd.print("  ");
  325.   }
  326.   while (digitalRead(A1)==LOW);
  327.   servoMM[3][0]=value;
  328.   EEPROM.write(5,value);  // servo1 min
  329.   lcd.setCursor(0,0);
  330.   lcd.print("Servo3 max:");
  331.   while (digitalRead(A1)==HIGH) {
  332.     value=map(analogRead(A0),0,1024,0,180);
  333.     servo3.write(value);
  334.     lcd.setCursor(12,0);
  335.     lcd.print(value);
  336.     lcd.print("  ");
  337.   }
  338.   while (digitalRead(A1)==LOW);
  339.   servoMM[3][1]=value;
  340.   EEPROM.write(6,value);  // servo1 max
  341.   lcd.setCursor(0,0);
  342.   lcd.print("Servo3 test: ");
  343.   while (digitalRead(A1)==HIGH) {
  344.     value=map(analogRead(A0),0,1024,servoMM[3][0],servoMM[3][1]);
  345.     servo3.write(value);
  346.     lcd.setCursor(13,0);
  347.     lcd.print(value);
  348.     lcd.print("  ");
  349.     lcd.setCursor(0,1);
  350.     lcd.print(servoMM[3][0]);
  351.     lcd.print("  ");
  352.     lcd.print(servoMM[3][1]);
  353.     lcd.print("  ");
  354.   }
  355.   while (digitalRead(A1)==LOW);
  356.   lcd.clear();
  357.   lcd.print("Servo4 min:");
  358. // SERVO4
  359.   while (digitalRead(A1)==HIGH) {
  360.     value=map(analogRead(A0),0,1024,0,180);
  361.     servo4.write(value);
  362.     lcd.setCursor(12,0);
  363.     lcd.print(value);
  364.     lcd.print("  ");
  365.   }
  366.   while (digitalRead(A1)==LOW);
  367.   servoMM[4][0]=value;
  368.   EEPROM.write(7,value);  // servo1 min
  369.   lcd.setCursor(0,0);
  370.   lcd.print("Servo4 max:");
  371.   while (digitalRead(A1)==HIGH) {
  372.     value=map(analogRead(A0),0,1024,0,180);
  373.     servo4.write(value);
  374.     lcd.setCursor(12,0);
  375.     lcd.print(value);
  376.     lcd.print("  ");
  377.   }
  378.   while (digitalRead(A1)==LOW);
  379.   servoMM[4][1]=value;
  380.   EEPROM.write(8,value);  // servo1 max
  381.   lcd.setCursor(0,0);
  382.   lcd.print("Servo4 test: ");
  383.   while (digitalRead(A1)==HIGH) {
  384.     value=map(analogRead(A0),0,1024,servoMM[4][0],servoMM[4][1]);
  385.     servo4.write(value);
  386.     lcd.setCursor(13,0);
  387.     lcd.print(value);
  388.     lcd.print("  ");
  389.     lcd.setCursor(0,1);
  390.     lcd.print(servoMM[4][0]);
  391.     lcd.print("  ");
  392.     lcd.print(servoMM[4][1]);
  393.     lcd.print("  ");
  394.   }
  395.   while (digitalRead(A1)==LOW);
  396.   lcd.setCursor(0,0);
  397.   lcd.print("Setting OK.");
  398.   delay(500);
  399. }
  400.  
  401.  
  402. //END
Add Comment
Please, Sign In to add comment
Public Pastes
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!