daily pastebin goal
64%
SHARE
TWEET

Untitled

a guest Jan 18th, 2019 71 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*##############################################
  2.                        _ _                  _
  3.                       | | |                | |
  4.    _ __   ___  ___  __| | | ___  __ _  __ _| |_ ___
  5.   | '_ \ / _ \/ _ \/ _` | |/ _ \/ _` |/ _` | __/ _ \
  6.   | | | |  __/  __/ (_| | |  __/ (_| | (_| | ||  __/
  7.   |_| |_|\___|\___|\__,_|_|\___|\__, |\__,_|\__\___|
  8.                                __/ |
  9.                               |___/
  10.    made by: Needlegate
  11.    needlegate@gmail.com
  12.    2019-01-05
  13.  
  14.    for Arduino ATmega 2560
  15. */
  16. #include "Motors.h"
  17. #include "Program.h"
  18. #include <Braccio.h>
  19. #include <Servo.h>
  20. #define CLS "\033[2J"
  21. Servo base;
  22. Servo shoulder;
  23. Servo elbow;
  24. Servo wrist_rot;
  25. Servo wrist_ver;
  26. Servo gripper;
  27. Motors Robot;
  28. Program Prog;
  29. int x_axis = A0;            //joystick pin
  30. int y_axis = A1;            // joystick pin
  31. int x_pos;
  32. int y_pos;
  33. byte axis_choice = 0;       //manual mode
  34. int button1 = 30;
  35. int button2 = 34;
  36. int button3 = 38;
  37. bool bt1_Tick;              //high or low every loop
  38. bool bt2_Tick;              //      --II--
  39. bool bt3_Tick;              //      --II--
  40. bool lastBt1;
  41. long last_bt1_highFlank;
  42. long last_bt2_highFlank;
  43. long last_bt3_highFlank;
  44. bool lastBt2;
  45. bool lastBt3;
  46. byte lvl_one_ptr = 1;
  47. byte final_lvl_1 = 0;
  48. byte lvl_two_ptr = 0;
  49. byte last_lvl_two_ptr;        //special for manual menu
  50. byte final_lvl_2 = 0;
  51. byte lvl_three_ptr = 0;
  52. byte final_lvl_3 = 0;
  53.  
  54. byte program_amount;
  55. byte program_choice;
  56. bool program_running = false;
  57. byte program_nodes;
  58. byte current_node;
  59.  
  60. int lastCheck;
  61. byte tick = 0;
  62. long last_tick = 0;
  63.  
  64.  
  65. void setup() {
  66.   Serial.begin(57600);
  67.  
  68.  
  69.   Serial.println("##################################################################");
  70.   Serial.println("################ NEEDLEGATE ROBOT TOOL 1.0 #######################");
  71.   Serial.println("##################################################################");
  72.   Serial.println();
  73.   Serial.println("robot starting up, stay away..");
  74.   //  Braccio.begin(-70);
  75.   pinMode(button1, INPUT);
  76.   pinMode(button2, INPUT);
  77.   pinMode(button3, INPUT);
  78. }
  79.  
  80. void loop() {
  81.   bt1_Tick = false;
  82.   bt2_Tick = false;
  83.   bt3_Tick = false;
  84.   check_button_highFlank();
  85.  
  86.   menus();
  87.   if (!program_running) {
  88.     if (final_lvl_1 == 1) { // manual mode
  89.       manual();
  90.       move_robot();
  91.     }
  92.     if (final_lvl_1 == 2) { //programming
  93.       manual();
  94.       move_robot();
  95.       menu_logic();
  96.     }
  97.     if (final_lvl_1 == 3) { // load program
  98.       menu_logic();
  99.  
  100.     }
  101.     if (final_lvl_1 == 4) { //Erase memory
  102.       print_menu();
  103.       menu_logic();
  104.     }
  105.  
  106.     if (update_screen()) { // only updates if something changed (not perfect)
  107.       print_screen();
  108.     }
  109.  
  110.   } else {
  111.     menu_logic();
  112.     if (program_running) {
  113.       if ( run_program(program_choice, program_nodes)) {
  114.         Serial.println("program done");
  115.         delay(1000);
  116.       } else {
  117.         print_screen();
  118.         Serial.print("running next node: ");
  119.         Serial.println(program_nodes);
  120.       }
  121.     }
  122.  
  123.   }
  124. }
  125. boolean run_program(byte program, byte nodes) {
  126.  
  127.   if (current_node > nodes) {
  128.     program_running = false;
  129.     Serial.println("program completed");
  130.     return true;
  131.   }
  132.  
  133.   bool moving_to_node = true;
  134.   byte actual_pos[7];
  135.   byte goal_pos[7];
  136.   for (int i = 1; i < 7; i++) {
  137.     actual_pos[i] = Robot.get_motor(i); // get actual pos
  138.     goal_pos[i] = Prog.get_axis(program, current_node, i); //error <<... node 255?
  139.  
  140.   }
  141.   byte completed = 0;
  142.   for (int i = 1; i < 7; i++) {
  143.     if (actual_pos[i] > goal_pos[i]) {
  144.       Robot.decrement(i);
  145.     } else if (actual_pos[i] < goal_pos[i]) {
  146.       Robot.increment(i);
  147.     } else {
  148.       completed++;
  149.     }
  150.   }
  151.   /// just 1 degree per lap
  152.  
  153.  
  154.   if (completed >= 6) {
  155.     current_node++;
  156.   }
  157.  
  158.   moving_to_node = false;
  159.  
  160.   // move_auto(m1, m2, m3, m4, m5, m6);
  161.   return false;
  162. }
  163. boolean update_screen() {
  164.   long this_tick = millis();
  165.   if (this_tick - last_tick > 1000) {
  166.     tick += 1;
  167.     last_tick = this_tick;
  168.   }
  169.   int a = digitalRead(button1);
  170.   int b = digitalRead(button2);
  171.   int c = digitalRead(button3);
  172.   int tempx = analogRead(x_axis);
  173.   int tempy = analogRead(y_axis);
  174.   tempx = map(tempx, 0, 1023, 0, 100);
  175.   tempy = map(tempy, 0, 1023, 0, 100);
  176.   int r = 0;
  177.   for (int i = 1; i < 7; i++) {
  178.     int R =  Robot.get_motor(i);
  179.     r = r + R;
  180.   }
  181.   int checkSum = a + b + c + r + tick;
  182.   if (checkSum != lastCheck) {
  183.     lastCheck = a + b + c + r + tick;
  184.     return true;
  185.   }
  186.   lastCheck = a + b + c + r + tick;
  187.   return false;
  188. }
  189. void manual() {     /// manually run robot. menu 1 /2
  190.  
  191.   x_pos = analogRead(x_axis);
  192.   y_pos = analogRead(y_axis);
  193.   long currentMillis = millis();
  194.  
  195.   if (y_pos > 490 && y_pos < 520) { //return if joystick is untouched
  196.     return;
  197.   }
  198.   boolean increment;
  199.  
  200.   if (y_pos > 520) {    // determine if we want to increment or not
  201.     increment = true;
  202.   } else if (y_pos < 490) {
  203.     increment = false;
  204.   }
  205.  
  206.   if (increment) {         //increment robot pos for axis of choice
  207.     int value = map(y_pos, 521, 1023, 1, 3);
  208.     long thisMove = millis();
  209.     long lastMove = Robot.get_lastMove(axis_choice);
  210.     boolean timeToMove = false;
  211.     if (thisMove - lastMove > 50 / value) {
  212.       if (Robot.increment(axis_choice)) {
  213.         //Serial.println(Robot.get_motor(axis_choice));
  214.       }
  215.     }
  216.   }
  217.  
  218.   if (!increment) {         //decrement robot pos for axis of choice
  219.     int value = map(y_pos, 490, 0, 1, 3);
  220.     long thisMove = millis();
  221.     long lastMove = Robot.get_lastMove(axis_choice);
  222.     boolean timeToMove = false;
  223.     if (thisMove - lastMove > 50 / value) {
  224.       if (Robot.decrement(axis_choice)) {
  225.         ;
  226.       }
  227.     }
  228.   }
  229.  
  230. }
  231. boolean save_node() {  //send the axis positions from Robot class to Prog class
  232.   byte temp_axis[6];
  233.   for (int i = 1; i < 7; i++) {
  234.     temp_axis[i - 1] = Robot.get_motor(i);
  235.   }
  236.   if (Prog.set_node(temp_axis[0], temp_axis[1], temp_axis[2], temp_axis[3],
  237.                     temp_axis[4], temp_axis[5])) {
  238.     return true;
  239.   } else {
  240.  
  241.     return false;
  242.   }
  243. }
  244. boolean save_program() { // let programmer class know that the program is done
  245.   Prog.set_end();
  246.  
  247. }
  248.  
  249. void move_robot() {
  250.   byte m1 = Robot.get_motor(1);
  251.   byte m2 = Robot.get_motor(2);
  252.   byte m3 = Robot.get_motor(3);
  253.   byte m4 = Robot.get_motor(4);
  254.   byte m5 = Robot.get_motor(5);
  255.   byte m6 = Robot.get_motor(6);
  256.   Braccio.ServoMovement(3, m1, m2, m3, m4, m5, m6);
  257.   //delay(5);;
  258.  
  259. }
  260.  
  261. void menus() {
  262.   ////////////return button 3
  263.   if (bt3_Tick) { //return 1 level
  264.     if (lvl_one_ptr > 0 && lvl_two_ptr == 0 && lvl_three_ptr == 0) {
  265.       final_lvl_1 = 0;
  266.     }
  267.     else if (lvl_two_ptr > 0 && lvl_three_ptr == 0) {
  268.       lvl_two_ptr = 0;    //return to level 1
  269.       final_lvl_2 = 0;
  270.     } else if (lvl_three_ptr > 0) {
  271.       lvl_three_ptr = 0; //return to level 2
  272.       final_lvl_3 = 0;
  273.     }
  274.   }
  275.  
  276.   ////////cycle menu level - button 2
  277.   if (bt2_Tick) { //cycle menus
  278.     if (final_lvl_2 > 0) { //we are in level 3
  279.       lvl_three_ptr = (lvl_three_ptr + 1) % 4;
  280.       if (lvl_three_ptr == 0) //make sure we dont get zero
  281.         lvl_three_ptr = 1;
  282.       /////////////////////////////////////////////
  283.     } else if (final_lvl_1 > 0 && final_lvl_3 == 0) { // lvl 2
  284.       if (final_lvl_1 == 3) {
  285.         lvl_two_ptr = (lvl_two_ptr + 1) % 4;
  286.         if (lvl_two_ptr == 0) //make sure we dont get zero
  287.           lvl_two_ptr = 1;
  288.       } else if (final_lvl_1 == 2) { // special - create program
  289.         axis_choice = (axis_choice + 1) % 7;
  290.         if (axis_choice == 0) {
  291.           axis_choice = 1;
  292.         }
  293.       }
  294.       ///////// end special create menu
  295.       else if (final_lvl_1 == 1) { //special - manual menu
  296.         axis_choice = (axis_choice + 1) % 7;
  297.         if (axis_choice == 0) {
  298.           axis_choice = 1;
  299.         }
  300.       }
  301.       ////// end special manual menu
  302.     } else {
  303.       lvl_one_ptr = (lvl_one_ptr + 1) % 5;        //lvl 1
  304.       if (lvl_one_ptr == 0)
  305.         lvl_one_ptr = 1;
  306.     }
  307.   }
  308.   //////chose menu - button 1
  309.   if (bt1_Tick) {  //select button 1
  310.     byte temp;
  311.  
  312.     if (lvl_three_ptr > 0) {
  313.       final_lvl_3 = lvl_three_ptr;
  314.  
  315.     } else if (lvl_two_ptr > 0 && lvl_three_ptr == 0) {
  316.       final_lvl_2 = lvl_two_ptr;
  317.  
  318.     } else if (lvl_one_ptr > 0 && lvl_two_ptr == 0 && lvl_three_ptr == 0) {
  319.       final_lvl_1 = lvl_one_ptr;
  320.       if (final_lvl_1 == 3) {                  //special for loading menu
  321.         program_amount = 0;
  322.         bool check_program = true;
  323.         while (check_program) {
  324.           temp = Prog.get_program_list();
  325.           if (temp == 254) {
  326.             program_amount++;
  327.           }
  328.           if (temp == 0) {
  329.             check_program = false;
  330.           }
  331.         }
  332.       }
  333.     }
  334.   }
  335.  
  336. }
  337.  
  338.  
  339. void print_screen() {
  340.   delay(5);
  341.   Serial.print("\033[2J"); // clear screen
  342.   // print_logo();
  343.   print_axis();
  344.   Serial.println("");
  345.   Serial.print(lvl_one_ptr); Serial.println(final_lvl_1);
  346.   Serial.print(lvl_two_ptr); Serial.println(final_lvl_2);
  347.   Serial.print(lvl_three_ptr); Serial.println(final_lvl_3);
  348.   print_menu();
  349.  
  350. }
  351. void print_menu() {
  352.   char arrow = '<';
  353.   char spear = '-';
  354.   Serial.println();
  355.   ///main menu
  356.   if (final_lvl_1 == 0) {
  357.     Serial.println("__MAIN MENU__");
  358.     Serial.print("* manual drive   ");
  359.     if (lvl_one_ptr == 1) {
  360.       Serial.print(arrow);
  361.       Serial.print(spear); Serial.println(spear);
  362.     } else {
  363.       Serial.println();
  364.     }
  365.     Serial.print("* create program ");
  366.     if (lvl_one_ptr == 2) {
  367.       Serial.print(arrow);
  368.       Serial.print(spear); Serial.println(spear);
  369.     } else {
  370.       Serial.println();
  371.     }
  372.     Serial.print("* load program   ");
  373.     if (lvl_one_ptr == 3) {
  374.       Serial.print(arrow);
  375.       Serial.print(spear); Serial.println(spear);
  376.     } else {
  377.       Serial.println();
  378.     }
  379.     Serial.print("* format EEPROM   ");
  380.     if (lvl_one_ptr == 4) {
  381.       Serial.print(arrow);
  382.       Serial.print(spear); Serial.println(spear);
  383.     } else {
  384.       Serial.println();
  385.     }
  386.   }
  387.   // level 2 manual menu
  388.   //manual menu
  389.   if (final_lvl_2 == 0 && final_lvl_1 == 1) {
  390.     Serial.println("__MANUAL MENU__");
  391.     Serial.println("manual drive");
  392.     Serial.print("axis choosed: "); Serial.println(axis_choice);
  393.  
  394.   }
  395.   // create program menu
  396.   if (final_lvl_2 == 0 && final_lvl_1 == 2) {
  397.     Serial.println("_Create Program_");
  398.     Serial.println("manual drive - save node by clicking bt1");
  399.     Serial.println(", hold bt1 2 secs to save program");
  400.     Serial.print("axis choosed: "); Serial.println(axis_choice);
  401.   }
  402.   // list programs om EEPROM
  403.   if (final_lvl_2 == 0 && final_lvl_1 == 3) {
  404.     Serial.print("number of saved programs: ");
  405.     Serial.println(program_amount);
  406.     Serial.print("load program: "); Serial.println(program_choice);
  407.   }
  408.   /// erase EEPROM menu
  409.   if (final_lvl_2 == 0 && final_lvl_1 == 4) {
  410.     Serial.print(CLS);
  411.     Serial.println("WARNING");
  412.     Serial.println("this will erase all stored data on EEPROM");
  413.     Serial.println("press 1 to ERASE or press 3 to go back to menu");
  414.     Serial.println("continue?");
  415.     delay(100);
  416.   }
  417. }
  418. ////////////////////////
  419. ////LOGIC//////////////
  420. //////////////////////
  421. void menu_logic() {
  422.   byte temp = digitalRead(button1);   /// for reset for save button
  423.   long currentMillis = millis();
  424.   /// Erase menu
  425.   if (final_lvl_2 == 0 && final_lvl_1 == 4) {
  426.     bool choice = false;
  427.     while (!choice) {
  428.  
  429.       if (get_bt3_highFlank()) {
  430.         final_lvl_1 = 0;
  431.         choice = true;
  432.       }
  433.       if (get_bt1_highFlank()) {
  434.         final_lvl_1 = 0;
  435.         Prog.format_EEPROM();
  436.         program_amount = 0;
  437.         choice = true;
  438.       }
  439.     }
  440.   }
  441.   /// programming menu
  442.   if (final_lvl_1 == 2 && lvl_one_ptr == 2) { //prog menu
  443.     if (bt1_Tick) {     ///set node
  444.       if (save_node()) {
  445.         Serial.println("saving node in program");
  446.       } else {
  447.         Serial.println("failed to save node");
  448.       }
  449.  
  450.       delay(300);
  451.     } else if (temp == 1 && currentMillis - last_bt1_highFlank > 2000 && currentMillis - last_bt1_highFlank < 3000) {
  452.       Serial.println("Saving program");
  453.       Prog.set_end();
  454.       last_bt1_highFlank = millis();
  455.       final_lvl_1 == 0;
  456.       lvl_one_ptr == 0;
  457.     }
  458.  
  459.   }
  460.  
  461.   /// load program
  462.   if (final_lvl_1 == 3 && lvl_one_ptr == 3) {
  463.     if (bt2_Tick) {
  464.       program_choice = (program_choice + 1) % ( program_amount + 1);
  465.       if (program_choice == 0) {
  466.         program_choice = 1;
  467.       }
  468.     }
  469.     if (bt1_Tick && program_choice != 0) {
  470.       program_nodes =  Prog.get_program_node_amount(program_choice);
  471.  
  472.       program_running = true;
  473.       current_node = 2;                           ///<<<<----------2 because of error in program class
  474.  
  475.     }
  476.     if (bt3_Tick) {
  477.       program_nodes = 0;
  478.       program_running = false;
  479.       program_choice = 0;
  480.     }
  481.   }
  482.  
  483. }
  484. void print_axis() {
  485.   Serial.println("axis angles:");
  486.   Serial.print("M1"); Serial.print("  M2");
  487.   Serial.print("  M3"); Serial.print("   M4"); Serial.print("   M5");
  488.   Serial.println("  M6");
  489.   Serial.print(Robot.get_motor(1)); Serial.print("  ");
  490.   Serial.print(Robot.get_motor(2)); Serial.print("  ");
  491.   Serial.print(Robot.get_motor(3)); Serial.print("  ");
  492.   Serial.print(Robot.get_motor(4)); Serial.print("  ");
  493.   Serial.print(Robot.get_motor(5)); Serial.print("  ");
  494.   Serial.print(Robot.get_motor(6)); Serial.println("  ");
  495. }
  496. void print_logo() {
  497.   Serial.print("\033[2J"); // clear screen
  498.   Serial.println("##################################################################");
  499.   Serial.println("################ NEEDLEGATE ROBOT TOOL 1.0 #######################");
  500.   Serial.println("##################################################################");
  501.   Serial.println();
  502.   delay(1000);
  503. }
  504. boolean get_bt1_lowFlank() {
  505.   bool b1 = digitalRead(button1);
  506.   // delay(5);;
  507.   if (!b1 && lastBt1) { // low flank
  508.     lastBt1 = b1;
  509.     return true;
  510.   }
  511.   lastBt1 = b1;
  512.   return false;
  513. }
  514. boolean get_bt1_highFlank() {
  515.   bool b1 = digitalRead(button1);
  516.   // delay(5);;
  517.   if (b1 && !lastBt1) { // high flank
  518.     lastBt1 = b1;
  519.     last_bt1_highFlank = millis();
  520.     return true;
  521.   }
  522.   lastBt1 = b1;
  523.   return false;
  524. }
  525. boolean get_bt2_highFlank() {
  526.   bool b2 = digitalRead(button2);
  527.   // delay(5);;
  528.   if (b2 && !lastBt2) { // high flank
  529.     lastBt2 = b2;
  530.     return true;
  531.   }
  532.   lastBt2 = b2;
  533.   return false;
  534. }
  535. boolean get_bt3_highFlank() {
  536.   bool b3 = digitalRead(button3);
  537.   // delay(5);;
  538.   if (b3 && !lastBt3) { // high flank
  539.     lastBt3 = b3;
  540.     return true;
  541.   }
  542.   lastBt3 = b3;
  543.   return false;
  544. }
  545. void check_button_highFlank() {
  546.   if (get_bt1_highFlank()) {
  547.     last_bt1_highFlank = millis();
  548.     bt1_Tick = true;
  549.   }
  550.   if (get_bt2_highFlank()) {
  551.     last_bt2_highFlank = millis();
  552.     bt2_Tick = true;
  553.   }
  554.   if (get_bt3_highFlank()) {
  555.     last_bt3_highFlank = millis();
  556.     bt3_Tick = true;
  557.   }
  558. }
RAW Paste Data
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
 
Top