设为首页收藏官网网盘国家资源立人课堂 今天是: 2022-08-03    美好的一天,从现在开始投稿

立人创客乐享社区

 找回密码
 立即注册
立人创客乐享社区 首页 机器智能 查看内容

基于intel Curie的开源机械臂

2020-5-3 22:31| 发布者: 信息中心| 查看: 30| 评论: 0|原作者: 刘老师

摘要: 基于intel Curie的机械臂具有动作记忆功能,通过记录每个点的位置,依次去回放动作,达到对机械臂编程的作用。机械臂的是用方法是: 1.DC及USB均上电,上电后并成功启动后LED会闪一下白光。2.成功启动后手拧电位器可 ...

基于intel Curie的机械臂具有动作记忆功能,通过记录每个点的位置,依次去回放动作,达到对机械臂编程的作用。机械臂的是用方法是:
1.DC及USB均上电,上电后并成功启动后LED会闪一下白光。
2.成功启动后手拧电位器可以看到机械抓的开合。
喜欢就支持下这个项目吧: http://www.kenrobot.com/arduinocn/vote  参赛编号13
试教步骤

1.    掰动机械臂到需要的位置后轻触一下按键,等待LED闪烁两次红色表示该位置已记录完毕。
2.    依上述方法掰动几次机械臂完成录入时长按按键1秒以上,松开按键后LED闪烁两次蓝色表示录入结束开始自动执行记录动作程序。
3.    需要停止重新录入动作时,长按按键1秒以上,松开按键后LED闪烁两次蓝色表示可以重新录入动作。

目前,机械臂的模型和代码都还在优化阶段,等成熟后,计划可以推出机械臂套件出售,下面是目前已经修改的模型特征
目前SW的工程文件我已经上传,大家可以提出自己的宝贵意见,如果有更好的制作方案,欢迎提出交流。


机械臂视频:



模型stl文件: ARM_PRINTABLE.zip (423.23 KB, 下载次数: 255)

  1. #include <CurieEEPROM.h>
  2. #include <Servo.h>
  3.   
  4. Servo myservo0,myservo1,myservo2,myservo3,myservo4;  // create servo object to control a servo
  5.                 // twelve servo objects can be created on most boards

  6. int addr = 0;
  7. int pos = 0;    // variable to store the servo position
  8. int read_Servo0, read_Servo1, read_Servo2, read_Servo3, read_Servo4;
  9. bool bottom = false;
  10. bool bottom_pressed = false;
  11. int min_0, max_0, min_1, max_1, min_2, max_2, min_3, max_3;
  12. //int R_PIN, G_PIN, B_PIN;
  13. #define R_PIN  4
  14. #define G_PIN  5
  15. #define B_PIN  6

  16. bool eeprom_end()
  17. {
  18.         if (EEPROM.read(addr) == 6666) {
  19.                 delay(100);
  20.                 return false;
  21.         }
  22.         else
  23.                 return true;
  24. }

  25. bool match_servo()
  26. {
  27.         int read_Servo0_p, read_Servo1_p, read_Servo2_p, read_Servo3_p, read_Servo4_p;

  28.         read_Servo0_p = analogRead(A0);
  29.         if (read_Servo0_p > max_0)
  30.                 read_Servo0_p = max_0;
  31.         if (read_Servo0_p < min_0)
  32.                 read_Servo0_p = min_0;
  33.         read_Servo0_p = map(read_Servo0_p, min_0, max_0, 611, 2389);

  34.         read_Servo1_p = analogRead(A1);
  35.         if (read_Servo1_p > max_1)
  36.                 read_Servo1_p = max_1;
  37.         if (read_Servo1_p < min_1)
  38.                 read_Servo1_p = min_1;
  39.         read_Servo1_p = map(read_Servo1_p, min_1, max_1, 611, 2389);

  40.         read_Servo2_p = analogRead(A2);
  41.         if (read_Servo2_p > max_2)
  42.                 read_Servo2_p = max_2;
  43.         if (read_Servo2_p < min_2)
  44.                 read_Servo2_p = min_2;
  45.         read_Servo2_p = map(read_Servo2_p, min_2, max_2, 611, 2389);

  46.         read_Servo3_p = analogRead(A3);
  47.         if (read_Servo3_p > max_3)
  48.                 read_Servo3_p = max_3;
  49.         if (read_Servo3_p < min_3)
  50.                 read_Servo3_p = min_3;
  51.         read_Servo3_p = map(read_Servo3_p, min_3, max_3, 611, 2389);

  52.         read_Servo4_p = analogRead(A4);
  53.         if (read_Servo4_p > 1024)
  54.                 read_Servo4_p = 1024;
  55.         if (read_Servo4_p < 0)
  56.                 read_Servo4_p = 0;
  57.         read_Servo4_p = map(read_Servo4_p, 0, 1024, 1222, 1722);

  58.         Serial.print("read_Servo_p:");
  59.         Serial.print(read_Servo0_p);
  60.         Serial.print(":");
  61.         Serial.print(read_Servo1_p);
  62.         Serial.print(":");
  63.         Serial.print(read_Servo2_p);
  64.         Serial.print(":");
  65.         Serial.print(read_Servo3_p);
  66.         Serial.print(":");
  67.         Serial.print(read_Servo4_p);
  68.         Serial.print("\n");


  69.         if (abs(read_Servo0_p - read_Servo0) > 20) {
  70.                 //Serial.print(read_Servo0_p - read_Servo0);
  71.                 Serial.println("!0");
  72.                 return false;
  73.         }
  74.         else if (abs(read_Servo1_p - read_Servo1_p) > 20) {
  75.                 Serial.println("!1");
  76.                 return false;
  77.         }
  78.         else if (abs(read_Servo2_p - read_Servo2_p) > 20) {
  79.                 Serial.println("!2");
  80.                 return false;
  81.         }
  82.         else if (abs(read_Servo3_p - read_Servo3_p) > 20) {
  83.                 Serial.println("!3");
  84.                 return false;
  85.         }
  86.         else if (abs(read_Servo4_p - read_Servo4_p) > 20) {
  87.                 Serial.println("!4");
  88.                 return false;
  89.         }
  90.         else if (abs(EEPROM.read(addr - 5) - read_Servo0) > 20) {
  91.                 /*Serial.print(":");
  92.                 Serial.print(EEPROM.read(addr - 5) - read_Servo0);
  93.                 Serial.print("\n");*/
  94.                 Serial.println("!5");
  95.                 return false;
  96.         }
  97.         else if (abs(EEPROM.read(addr - 4) - read_Servo1) > 20) {
  98.                 Serial.println("!6");
  99.                 return false;
  100.         }
  101.         else if (abs(EEPROM.read(addr - 3) - read_Servo2) > 20) {
  102.                 Serial.println("!7");
  103.                 return false;
  104.         }
  105.         else if (abs(EEPROM.read(addr - 2) - read_Servo3) > 20) {
  106.                 Serial.println("!8");
  107.                 return false;
  108.         }
  109.         else if (abs(EEPROM.read(addr - 1) - read_Servo4) > 20) {
  110.                 Serial.println("!9");
  111.                 return false;
  112.         }
  113.         else
  114.                 return true;
  115. }

  116. void get_Servo()
  117. {
  118.         read_Servo0 = analogRead(A0);
  119.         if (read_Servo0 > max_0)
  120.                 read_Servo0 = max_0;
  121.         if (read_Servo0 < min_0)
  122.                 read_Servo0 = min_0;
  123.         read_Servo0 = map(read_Servo0, min_0, max_0, 611, 2389);


  124.         read_Servo1 = analogRead(A1);
  125.         if (read_Servo1 > max_1)
  126.                 read_Servo1 = max_1;
  127.         if (read_Servo1 < min_1)
  128.                 read_Servo1 = min_1;
  129.         read_Servo1 = map(read_Servo1, min_1, max_1, 611, 2389);


  130.         read_Servo2 = analogRead(A2);
  131.         if (read_Servo2 > max_2)
  132.                 read_Servo2 = max_2;
  133.         if (read_Servo2 < min_2)
  134.                 read_Servo2 = min_2;
  135.         read_Servo2 = map(read_Servo2, min_2, max_2, 611, 2389);


  136.         read_Servo3 = analogRead(A3);
  137.         if (read_Servo3 > max_3)
  138.                 read_Servo3 = max_3;
  139.         if (read_Servo3 < min_3)
  140.                 read_Servo3 = min_3;
  141.         read_Servo3 = map(read_Servo3, min_3, max_3, 611, 2389);


  142.         read_Servo4 = analogRead(A4);
  143.         if (read_Servo4 > 1024)
  144.                 read_Servo4 = 1024;
  145.         if (read_Servo4 < 0)
  146.                 read_Servo4 = 0;
  147.         read_Servo4 = map(read_Servo4, 0, 1024, 1222, 1722);


  148.         EEPROM.write(addr, read_Servo0);
  149.         delay(100);
  150.         addr++;
  151.         EEPROM.write(addr, read_Servo1);
  152.         delay(100);
  153.         addr++;
  154.         EEPROM.write(addr, read_Servo2);
  155.         delay(100);
  156.         addr++;
  157.         EEPROM.write(addr, read_Servo3);
  158.         delay(100);
  159.         addr++;
  160.         EEPROM.write(addr, read_Servo4);
  161.         delay(100);
  162.         addr++;

  163.         Serial.print("read_Servo:");
  164.         Serial.print(read_Servo0);
  165.         Serial.print(":");
  166.         Serial.print(read_Servo1);
  167.         Serial.print(":");
  168.         Serial.print(read_Servo2);
  169.         Serial.print(":");
  170.         Serial.print(read_Servo3);
  171.         Serial.print(":");
  172.         Serial.print(read_Servo4);
  173.         Serial.print("\n");
  174. }

  175. void write_eeprom_end()
  176. {
  177.         EEPROM.write(addr, 6666);
  178.         delay(100);
  179. }

  180. void read_eeprom()
  181. {
  182.         for (int i = 0; i < 512; i)
  183.         {
  184.                 int value = EEPROM.read(i);
  185.                 delay(100);
  186.                 Serial.print(value);
  187.                 if ((i+1) % 5 == 0)
  188.                         Serial.print("\n");
  189.                 else
  190.                         Serial.print(":");
  191.                 if (value != 6666)
  192.                         i++;
  193.                 else
  194.                         i = 512;
  195.         }
  196.         Serial.print("\n");
  197. }

  198. void get_Servo4()
  199. {
  200.         pos = analogRead(A4);
  201.         myservo4.writeMicroseconds(map(pos, 0, 1024, 1222, 1722));
  202. }

  203. void servo_detach()
  204. {
  205.         myservo0.detach();
  206.         myservo1.detach();
  207.         myservo2.detach();
  208.         myservo3.detach();
  209.         //myservo4.detach();
  210. }

  211. void servo_attach()
  212. {
  213.         myservo0.attach(8);
  214.         myservo1.attach(9);
  215.         myservo2.attach(10);
  216.         myservo3.attach(11);
  217.         //myservo4.attach(12);
  218. }

  219. void bottom_press()
  220. {
  221.         long now_time = millis();
  222.          
  223.         //detachInterrupt(digitalPinToInterrupt(UPS_INT_PIN));
  224.         detachInterrupt(7);

  225.         delay(5);
  226.         if (digitalRead(7) == LOW)
  227.         {
  228.                 bottom = !bottom;
  229.                 Serial.println(bottom);
  230.                 unsigned long value;

  231.                 while (digitalRead(7) == LOW);

  232.                 if (millis() - now_time >= 1000)
  233.                 {
  234.                         bottom_pressed = !bottom_pressed;
  235.                         Serial.println("LONG PRESS!");
  236.                         digitalWrite(B_PIN, LOW);
  237.                         delay(500);
  238.                         digitalWrite(B_PIN, HIGH);
  239.                         delay(500);
  240.                         digitalWrite(B_PIN, LOW);
  241.                         delay(500);
  242.                         digitalWrite(B_PIN, HIGH);
  243.                         delay(500);
  244.                         if (bottom_pressed)
  245.                         {
  246.                                 write_eeprom_end();
  247.                                 read_eeprom();
  248.                                 addr = 0;

  249.                                 servo_attach();

  250.                                  
  251.                         }
  252.                         /*else
  253.                         {
  254.                                 digitalWrite(G_PIN, LOW);
  255.                                 delay(500);
  256.                                 digitalWrite(G_PIN, HIGH);
  257.                                 delay(500);
  258.                                 digitalWrite(G_PIN, LOW);
  259.                                 delay(500);
  260.                                 digitalWrite(G_PIN, HIGH);
  261.                                 delay(500);
  262.                         }*/
  263.                         //read_eeprom();
  264.                         //addr = 0;
  265.                 }
  266.                 else
  267.                 {
  268.                         get_Servo();
  269.                         while (!match_servo())
  270.                         {
  271.                                 addr -= 5;
  272.                                 get_Servo();
  273.                         }
  274.                         Serial.println("EEPROM WRITE END");
  275.                         Serial.print(EEPROM.read(addr - 5));
  276.                         Serial.print(":");
  277.                         Serial.print(EEPROM.read(addr - 4));
  278.                         Serial.print(":");
  279.                         Serial.print(EEPROM.read(addr - 3));
  280.                         Serial.print(":");
  281.                         Serial.print(EEPROM.read(addr - 2));
  282.                         Serial.print(":");
  283.                         Serial.println(EEPROM.read(addr - 1));
  284.                         digitalWrite(R_PIN, LOW);
  285.                         delay(500);
  286.                         digitalWrite(R_PIN, HIGH);
  287.                         delay(500);
  288.                         digitalWrite(R_PIN, LOW);
  289.                         delay(500);
  290.                         digitalWrite(R_PIN, HIGH);
  291.                         delay(500);
  292.                 }
  293.         }
  294.         attachInterrupt(7, bottom_press, FALLING);
  295. }

  296. void get_analog()
  297. {
  298.         for (pos = 10; pos <= 170; pos += 10) // goes from 0 degrees to 180 degrees
  299.         {                                  // in steps of 1 degree
  300.                 myservo0.write(pos);
  301.                 myservo1.write(pos);
  302.                 myservo2.write(pos);
  303.                 myservo3.write(pos);
  304.                 myservo4.write(map(pos, 10, 170, 65*11, 110*11));// tell servo to go to position in variable 'pos'
  305.                 delay(500);                       // waits 15ms for the servo to reach the position
  306.                 if (pos == 10 || pos == 170)
  307.                 {
  308.                         Serial.print(analogRead(A0));
  309.                         Serial.print(" : ");
  310.                         Serial.print(analogRead(A1));
  311.                         Serial.print(" : ");
  312.                         Serial.print(analogRead(A2));
  313.                         Serial.print(" : ");
  314.                         Serial.print(analogRead(A3));
  315.                         Serial.print("\n");
  316.                 }
  317.         }
  318.         for (pos = 170; pos >= 10; pos -= 10)     // goes from 180 degrees to 0 degrees
  319.         {
  320.                 myservo0.write(pos);
  321.                 myservo1.write(pos);
  322.                 myservo2.write(pos);
  323.                 myservo3.write(pos);
  324.                 //if(pos >= 65 && pos <= 110)
  325.                 myservo4.write(map(pos, 10, 170, 65*11, 110*11));// tell servo to go to position in variable 'pos'
  326.                 delay(500);                       // waits 15ms for the servo to reach the position
  327.                 if (pos == 10 || pos == 170)
  328.                 {
  329.                         Serial.print(analogRead(A0));
  330.                         Serial.print(" : ");
  331.                         Serial.print(analogRead(A1));
  332.                         Serial.print(" : ");
  333.                         Serial.print(analogRead(A2));
  334.                         Serial.print(" : ");
  335.                         Serial.print(analogRead(A3));
  336.                         Serial.print("\n");
  337.                 }
  338.         }
  339. }

  340. void process_servo()
  341. {
  342.         int servo_pos_0, servo_pos_1, servo_pos_2, servo_pos_3, servo_pos_4;
  343.         int servo_des_0, servo_des_1, servo_des_2, servo_des_3, servo_des_4;
  344.         int servo_move_0, servo_move_1, servo_move_2, servo_move_3, servo_move_4;
  345.         int rate_0, rate_1, rate_2, rate_3, rate_4, servo_move_max, rate_time;

  346.         servo_pos_0 = read_Servo0;
  347.         servo_pos_1 = read_Servo1;
  348.         servo_pos_2 = read_Servo2;
  349.         servo_pos_3 = read_Servo3;
  350.         servo_pos_4 = read_Servo4;

  351.          
  352.         myservo0.writeMicroseconds(servo_pos_0);
  353.         myservo1.writeMicroseconds(servo_pos_1);
  354.         myservo2.writeMicroseconds(servo_pos_2);
  355.         myservo3.writeMicroseconds(servo_pos_3);
  356.         myservo4.writeMicroseconds(servo_pos_4);

  357.         servo_des_0 = EEPROM.read(addr);
  358.         //delay(100);
  359.         addr++;
  360.         servo_des_1 = EEPROM.read(addr);
  361.         //delay(100);
  362.         addr++;
  363.         servo_des_2 = EEPROM.read(addr);
  364.         //delay(100);
  365.         addr++;
  366.         servo_des_3 = EEPROM.read(addr);
  367.         //delay(100);
  368.         addr++;
  369.         servo_des_4 = EEPROM.read(addr);
  370.         //delay(100);
  371.         addr++;

  372.         servo_move_0 = servo_des_0 - servo_pos_0;
  373.         servo_move_1 = servo_des_1 - servo_pos_1;
  374.         servo_move_2 = servo_des_2 - servo_pos_2;
  375.         servo_move_3 = servo_des_3 - servo_pos_3;
  376.         servo_move_4 = servo_des_4 - servo_pos_4;

  377.         servo_move_max = max(abs(servo_move_0), max(abs(servo_move_1), max(abs(servo_move_2), max(abs(servo_move_3), abs(servo_move_4)))));

  378.         rate_0 = servo_move_0 * 50 / servo_move_max;
  379.         rate_1 = servo_move_1 * 50 / servo_move_max;
  380.         rate_2 = servo_move_2 * 50 / servo_move_max;
  381.         rate_3 = servo_move_3 * 50 / servo_move_max;
  382.         rate_4 = servo_move_4 * 50 / servo_move_max;

  383.         /*Serial.print("Servo_rate:");
  384.         Serial.print(rate_0);
  385.         Serial.print(" : ");
  386.         Serial.print(rate_1);
  387.         Serial.print(" : ");
  388.         Serial.print(rate_2);
  389.         Serial.print(" : ");
  390.         Serial.print(rate_3);
  391.         Serial.print(" : ");
  392.         Serial.print(rate_4);
  393.         Serial.print("\n");*/

  394.         rate_time = servo_move_max;
  395.         //rate_num = 5;
  396.         //int rate_num = 0;
  397.         while (rate_time)
  398.         {
  399.                 if (rate_time < 50)
  400.                 {
  401.                         servo_pos_0 = servo_des_0;
  402.                         servo_pos_1 = servo_des_1;
  403.                         servo_pos_2 = servo_des_2;
  404.                         servo_pos_3 = servo_des_3;
  405.                         servo_pos_4 = servo_des_4;
  406.                         rate_time = 0;
  407.                 }
  408.                 else
  409.                 {
  410.                         servo_pos_0 += rate_0;
  411.                         servo_pos_1 += rate_1;
  412.                         servo_pos_2 += rate_2;
  413.                         servo_pos_3 += rate_3;
  414.                         servo_pos_4 += rate_4;
  415.                         rate_time -= 50;
  416.                 }
  417.                 //rate_num++;
  418.                 /*Serial.print("rate_time:");
  419.                 Serial.print(rate_time);
  420.                 Serial.print("Servo_test:");
  421.                 Serial.print(servo_pos_0);
  422.                 Serial.print(" : ");
  423.                 Serial.print(servo_pos_1);
  424.                 Serial.print(" : ");
  425.                 Serial.print(servo_pos_2);
  426.                 Serial.print(" : ");
  427.                 Serial.print(servo_pos_3);
  428.                 Serial.print(" : ");
  429.                 Serial.print(servo_pos_4);
  430.                 Serial.print(" : ");
  431.                 Serial.print(rate_num);
  432.                 Serial.print("\n");*/

  433.                 myservo0.write(servo_pos_0);
  434.                 myservo1.write(servo_pos_1);
  435.                 myservo2.write(servo_pos_2);
  436.                 myservo3.write(servo_pos_3);
  437.                 myservo4.write(servo_pos_4);

  438.                 delay(40);
  439.         }

  440.         /*Serial.print(servo_pos_0);
  441.         Serial.print(" : ");
  442.         Serial.print(servo_pos_1);
  443.         Serial.print(" : ");
  444.         Serial.print(servo_pos_2);
  445.         Serial.print(" : ");
  446.         Serial.print(servo_pos_3);
  447.         Serial.print(" : ");
  448.         Serial.print(servo_pos_4);
  449.         Serial.print("\n");*/

  450.         read_Servo0 = servo_des_0;
  451.         read_Servo1 = servo_des_1;
  452.         read_Servo2 = servo_des_2;
  453.         read_Servo3 = servo_des_3;
  454.         read_Servo4 = servo_des_4;
  455. }

  456. void config_init()
  457. {
  458.         min_0 = 177;
  459.         min_1 = 178;
  460.         min_2 = 194;
  461.         min_3 = 172;
  462.         max_0 = 660;
  463.         max_1 = 668;
  464.         max_2 = 704;
  465.         max_3 = 650;
  466.         pinMode(A0, INPUT);
  467.         pinMode(A1, INPUT);
  468.         pinMode(A2, INPUT);
  469.         pinMode(A3, INPUT);
  470.         pinMode(A4, INPUT);
  471.         pinMode(7, INPUT_PULLUP);
  472.         pinMode(R_PIN, OUTPUT);
  473.         pinMode(G_PIN, OUTPUT);
  474.         pinMode(B_PIN, OUTPUT);
  475.         digitalWrite(R_PIN, HIGH);
  476.         digitalWrite(G_PIN, HIGH);
  477.         digitalWrite(B_PIN, HIGH);
  478.         attachInterrupt(7, bottom_press, FALLING);
  479.         myservo0.attach(8);
  480.         myservo1.attach(9);
  481.         myservo2.attach(10);
  482.         myservo3.attach(11);
  483.         myservo4.attach(12);
  484. }

  485. void setup()
  486. {
  487.         config_init();
  488.         Serial.begin(9600);
  489. }
  490.   
  491. void loop()
  492. {
  493.         if (bottom_pressed)
  494.         {
  495.                 if (eeprom_end())
  496.                         process_servo();
  497.                 else
  498.                         addr = 0;
  499.         }
  500.         else
  501.         {
  502.                 servo_detach();

  503.                 get_Servo4();
  504.                 delay(50);
  505.         }
  506. }




  507.   //for(pos = 10; pos <= 170; pos += 1) // goes from 0 degrees to 180 degrees
  508.   //{                                  // in steps of 1 degree
  509.   //  myservo0.write(90);
  510.   //  myservo1.write(90);
  511.   //  myservo2.write(90);
  512.   //  myservo3.write(90);
  513.   //  //if(pos >= 65 && pos <= 110)
  514.   //  myservo4.write(map(pos,10,170,65,110));// tell servo to go to position in variable 'pos'
  515.   //  delay(20);                       // waits 15ms for the servo to reach the position
  516.   //}
  517.   //for(pos = 170; pos>=10; pos -= 1)     // goes from 180 degrees to 0 degrees
  518.   //{                                
  519.   //  myservo0.write(90);
  520.   //  myservo1.write(90);
  521.   //  myservo2.write(90);
  522.   //  myservo3.write(90);
  523.   //  //if(pos >= 65 && pos <= 110)
  524.   //  myservo4.write(map(pos,10,170,65,110));// tell servo to go to position in variable 'pos'
  525.   //  delay(20);                       // waits 15ms for the servo to reach the position
  526.   //}
复制代码



路过

雷人

握手

鲜花

鸡蛋
发表评论

最新评论

Loading...
展开
返回顶部