devbox
爱喝兽奶帝天荒
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

esp32舵机机械臂_esp32 机器人控制

作者：爱喝兽奶帝天荒 | 2024-08-11 00:09:11

赞

踩

esp32 机器人控制

前言

参考【太极创客】零基础入门学用Arduino 第二部分 meArm机械臂 合辑_哔哩哔哩_bilibili

主要是使用esp32控制四个舵机，学会使用串口通讯以及蓝牙通讯。

代码部分

  1. #include <ESP32Servo.h>
  2. #include <Arduino.h>
  3. #include <BluetoothSerial.h>
  4.  
  5.  
  6. #define SERVO_base   2   //电机接口
  7. #define SERVO_rArm   4
  8. #define SERVO_fArm   5
  9. #define SERVO_claw   18
  10. #define MAX_WIDTH   2500 //脉宽范围50ms~2500ms
  11. #define MIN_WIDTH   500
  12.  
  13. //存储电机极限值(const指定该数值为常量,常量数值在程序运行中不能改变)
  14. const int baseMin = 0;
  15. const int baseMax = 180;
  16. const int rArmMin = 30;
  17. const int rArmMax = 180;
  18. const int fArmMin = 50;
  19. const int fArmMax = 180;
  20. const int clawMin = 70;
  21. const int clawMax = 100;
  22. int DSD = 15; //Default Servo Delay (默认电机运动延迟时间)
  23. int moveStep = 3;  //手柄模式步进角度
  24. bool mode = 0; //手柄与命令模式 0手柄 1命令
  25. // 定义 servo 对象
  26. Servo base, rArm, fArm, claw;  //建立4个电机对象  底座、大臂、小臂、夹子
  27. BluetoothSerial SerialBT;  //蓝牙对象
  28. void reportStatus();  //舵机状态信息
  29. void servoMove(char servoName, int toPos, int servoDelay);  //电机移动
  30. void armCmd(char serialCmd);   //命令模式
  31. void armJoy(char serialCmd); //手柄模式
  32. void armIniPos();   //初始化电机角度
  33.  
  34. void setup()
  35. {
  36.   Serial.begin(9600); //开启串口
  37.   // 分配硬件定时器
  38.   ESP32PWM::allocateTimer(0);
  39.   // 设置频率
  40.   base.setPeriodHertz(50);
  41.   rArm.setPeriodHertz(50);
  42.   fArm.setPeriodHertz(50);
  43.   claw.setPeriodHertz(50);
  44.   // 关联 servo 对象与 GPIO 引脚，设置脉宽范围
  45.   base.attach(SERVO_base, MIN_WIDTH, MAX_WIDTH);
  46.   rArm.attach(SERVO_rArm, MIN_WIDTH, MAX_WIDTH);
  47.   fArm.attach(SERVO_fArm, MIN_WIDTH, MAX_WIDTH);
  48.   claw.attach(SERVO_claw, MIN_WIDTH, MAX_WIDTH);
  49.   //初始化电机角度
  50.   base.write(90); 
  51.   delay(10);
  52.   fArm.write(90); 
  53.   delay(10);
  54.   rArm.write(90); 
  55.   delay(10);
  56.   claw.write(90);  
  57.   delay(10); 
  58.  
  59.   reportStatus();//输出当前电机角度信息
  60.   Serial.begin(9600);
  61.   SerialBT.begin("eh"); // 如果没有参数传入则默认是蓝牙名称是: "ESP32"
  62.   Serial.printf("BT initial ok and ready to pair. \r\n");
  63. }
  64.  
  65. void loop()
  66. {
  67.     if (Serial.available()>0) {  
  68.       char serialCmd = Serial.read();
  69.       delay(10);
  70.       SerialBT.write(serialCmd);
  71.       delay(10);
  72.       if( mode == 1 ){
  73.         armCmd(serialCmd); //指令模式
  74.       } else {
  75.         armJoy(serialCmd); //手柄模式
  76.       }
  77.     }
  78.     
  79.     if (SerialBT.available())
  80.     {
  81.         //Serial.write(SerialBT.read());
  82.         char serialCmd = SerialBT.read();
  83.         Serial.print("  ");
  84.         Serial.write(serialCmd);
  85.         Serial.println("  ");
  86.         if( mode == 1 ){
  87.           armCmd(serialCmd); //指令模式
  88.         } else {
  89.           armJoy(serialCmd); //手柄模式
  90.         }
  91.     }
  92.  
  93. }
  94.  
  95. void armJoy(char serialCmd) //手柄模式
  96. {
  97.   int baseJoyPos;
  98.   int rArmJoyPos;
  99.   int fArmJoyPos;
  100.   int clawJoyPos;
  101.   switch(serialCmd)
  102.   {
  103.     case 'a':
  104.       Serial.println("Received Command: a");
  105.       baseJoyPos = base.read()+moveStep;
  106.       servoMove('b',baseJoyPos,DSD);
  107.       break;
  108.     case 'd':
  109.       Serial.println("Received Command: d");
  110.       baseJoyPos = base.read()-moveStep;
  111.       servoMove('b',baseJoyPos,DSD);
  112.       break;
  113.     case 'w':
  114.       Serial.println("Received Command: w");
  115.       baseJoyPos = rArm.read()+moveStep;
  116.       servoMove('r',baseJoyPos,DSD);
  117.       break;
  118.     case 's':
  119.       Serial.println("Received Command: s");
  120.       baseJoyPos = rArm.read()-moveStep;
  121.       servoMove('r',baseJoyPos,DSD);
  122.       break;
  123.     case '8':
  124.       Serial.println("Received Command: 8");
  125.       baseJoyPos = fArm.read()+moveStep;
  126.       servoMove('f',baseJoyPos,DSD);
  127.       break;
  128.     case '5':
  129.     Serial.println("Received Command: 5");
  130.     baseJoyPos = fArm.read()-moveStep;
  131.     servoMove('f',baseJoyPos,DSD);
  132.     break;
  133.     case '4':
  134.     Serial.println("Received Command: 4");
  135.     baseJoyPos = claw.read()+moveStep;
  136.     servoMove('c',baseJoyPos,DSD);
  137.     break;
  138.     case '6':
  139.     Serial.println("Received Command: 6");
  140.     baseJoyPos = claw.read()-moveStep;
  141.     servoMove('c',baseJoyPos,DSD);
  142.     break;
  143.      case 'm' :   //切换至指令模式 
  144.       mode = 1; 
  145.       Serial.println("Command: Switch to Instruction Mode.");
  146.       break;
  147.     case 'o':  
  148.       reportStatus();
  149.       break;
  150.     case 'i':  
  151.       armIniPos();
  152.       break;
  153.     default:
  154.       Serial.println("Unknown Command.");
  155.       return;
  156.       
  157.   }
  158. }
  159.  
  160. void armCmd(char serialCmd)   //命令模式
  161. {
  162.     if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){
  163.     int servoData = Serial.parseInt();
  164.     servoMove(serialCmd, servoData, DSD);  // 机械臂舵机运行函数（参数：舵机名，目标角度，延迟/速度）
  165.   } else {
  166.     switch(serialCmd){    
  167.       case 'o':  // 输出舵机状态信息
  168.         reportStatus();
  169.         break;
  170.       case 'i':
  171.         armIniPos();  
  172.         break;
  173.       case 'm' :   //切换至手柄模式 
  174.         mode = 0; 
  175.         Serial.println("Command: Switch to Joy-Stick Mode.");
  176.         break; 
  177.       default:  //未知指令反馈
  178.         Serial.println("Unknown Command.");
  179.     }
  180.   } 
  181. }
  182.  
  183.  
  184. void armIniPos(){   //初始化电机角度
  185.   Serial.println("+Command: Restore Initial Position.");
  186.   int robotIniPosArray[4][3] = {
  187.     {'b', 90, DSD},
  188.     {'r', 90, DSD},
  189.     {'f', 90, DSD},
  190.     {'c', 90, DSD} 
  191.   };   
  192.  
  193.   for (int i = 0; i < 4; i++){
  194.     servoMove(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);
  195.   }
  196. }
  197.  
  198. void servoMove(char servoName, int toPos, int servoDelay)  //电机移动
  199. {
  200.   //串口监视器输出接收指令信息
  201.   Serial.println("");
  202.   Serial.print("+Command: Servo ");
  203.   Serial.print(servoName);
  204.   Serial.print(" to ");
  205.   Serial.print(toPos);
  206.   Serial.print(" at servoDelay value ");
  207.   Serial.print(servoDelay);
  208.   Serial.println(".");
  209.   Serial.println("");  
  210.  
  211.   int fromPos; //建立变量，存储电机起始运动角度值
  212.   switch(servoName){
  213.     case 'b':
  214.       if(toPos >= baseMin && toPos <= baseMax){
  215.         fromPos = base.read();  // 获取当前电机角度值用于“电机运动起始角度值”
  216.         if (fromPos <= toPos){  //如果“起始角度值”小于“目标角度值”
  217.               for (int i=fromPos; i<=toPos; i++){
  218.                 base.write(i);
  219.                 delay (servoDelay);
  220.                 //Serial.println(base.read());
  221.             }
  222.             }  else { //否则“起始角度值”大于“目标角度值”
  223.               for (int i=fromPos; i>=toPos; i--){
  224.                 base.write(i);
  225.                 delay (servoDelay);
  226.                 //Serial.println(base.read());
  227.               }
  228.             
  229.           }
  230.         break;
  231.       } else {
  232.         Serial.println("+Warning: Base Servo Value Out Of Limit!");
  233.         return;
  234.       }
  235.  
  236.     case 'c':
  237.       if(toPos >= clawMin && toPos <= clawMax){
  238.         fromPos = claw.read();  // 获取当前电机角度值用于“电机运动起始角度值”
  239.         if (fromPos <= toPos){  //如果“起始角度值”小于“目标角度值”
  240.               for (int i=fromPos; i<=toPos; i++){
  241.                 claw.write(i);
  242.                 delay (servoDelay);
  243.                 //Serial.println(claw.read());
  244.             }
  245.             }  else { //否则“起始角度值”大于“目标角度值”
  246.               for (int i=fromPos; i>=toPos; i--){
  247.                 claw.write(i);
  248.                 delay (servoDelay);
  249.                // Serial.println(claw.read());
  250.               }
  251.             
  252.           }
  253.         break;
  254.       } else {
  255.         Serial.println("+Warning: Claw Servo Value Out Of Limit!");
  256.         return;        
  257.       }
  258.  
  259.     case 'f':
  260.       if(toPos >= fArmMin && toPos <= fArmMax){
  261.         fromPos = fArm.read();  // 获取当前电机角度值用于“电机运动起始角度值”
  262.         if (fromPos <= toPos){  //如果“起始角度值”小于“目标角度值”
  263.               for (int i=fromPos; i<=toPos; i++){
  264.                 fArm.write(i);
  265.                 delay (servoDelay);
  266.                 //Serial.println(fArm.read());
  267.             }
  268.             }  else { //否则“起始角度值”大于“目标角度值”
  269.               for (int i=fromPos; i>=toPos; i--){
  270.                 fArm.write(i);
  271.                 delay (servoDelay);
  272.                 //Serial.println(fArm.read());
  273.               }
  274.             
  275.           }
  276.         break;
  277.       } else {
  278.         Serial.println("+Warning: fArm Servo Value Out Of Limit!");
  279.         return;
  280.       }
  281.          
  282.     case 'r':
  283.       if(toPos >= rArmMin && toPos <= rArmMax){
  284.         fromPos = rArm.read();  // 获取当前电机角度值用于“电机运动起始角度值”
  285.         if (fromPos <= toPos){  //如果“起始角度值”小于“目标角度值”
  286.               for (int i=fromPos; i<=toPos; i++){
  287.                 rArm.write(i);
  288.                 delay (servoDelay);
  289.                 //Serial.println(rArm.read());
  290.             }
  291.             }  else { //否则“起始角度值”大于“目标角度值”
  292.               for (int i=fromPos; i>=toPos; i--){
  293.                 rArm.write(i);
  294.                 delay (servoDelay);
  295.                 //Serial.println(rArm.read());
  296.               }
  297.             
  298.           }
  299.         break;
  300.       } else {
  301.         Serial.println("+Warning: rArm Servo Value Out Of Limit!");
  302.         return;
  303.       }      
  304.   }
  305.  
  306. }
  307.  
  308. void reportStatus(){  //舵机状态信息
  309.   Serial.println("");
  310.   Serial.println("");
  311.   Serial.println("+ Robot-Arm Status Report +");
  312.   Serial.print("Claw Position: "); Serial.println(claw.read());
  313.   Serial.print("Base Position: "); Serial.println(base.read());
  314.   Serial.print("Rear  Arm Position:"); Serial.println(rArm.read());
  315.   Serial.print("Front Arm Position:"); Serial.println(fArm.read());
  316.   Serial.println("++++++++++++++++++++++++++");
  317.   Serial.println("");
  318. }

本文内容由网友自发贡献，转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号