赞
踩
参考【太极创客】零基础入门学用Arduino 第二部分 meArm机械臂 合辑_哔哩哔哩_bilibili
主要是使用esp32控制四个舵机,学会使用串口通讯以及蓝牙通讯。
- #include <ESP32Servo.h>
- #include <Arduino.h>
- #include <BluetoothSerial.h>
-
-
- #define SERVO_base 2 //电机接口
- #define SERVO_rArm 4
- #define SERVO_fArm 5
- #define SERVO_claw 18
- #define MAX_WIDTH 2500 //脉宽范围50ms~2500ms
- #define MIN_WIDTH 500
-
- //存储电机极限值(const指定该数值为常量,常量数值在程序运行中不能改变)
- const int baseMin = 0;
- const int baseMax = 180;
- const int rArmMin = 30;
- const int rArmMax = 180;
- const int fArmMin = 50;
- const int fArmMax = 180;
- const int clawMin = 70;
- const int clawMax = 100;
- int DSD = 15; //Default Servo Delay (默认电机运动延迟时间)
- int moveStep = 3; //手柄模式步进角度
- bool mode = 0; //手柄与命令模式 0手柄 1命令
- // 定义 servo 对象
- Servo base, rArm, fArm, claw; //建立4个电机对象 底座、大臂、小臂、夹子
- BluetoothSerial SerialBT; //蓝牙对象
- void reportStatus(); //舵机状态信息
- void servoMove(char servoName, int toPos, int servoDelay); //电机移动
- void armCmd(char serialCmd); //命令模式
- void armJoy(char serialCmd); //手柄模式
- void armIniPos(); //初始化电机角度
-
- void setup()
- {
- Serial.begin(9600); //开启串口
- // 分配硬件定时器
- ESP32PWM::allocateTimer(0);
- // 设置频率
- base.setPeriodHertz(50);
- rArm.setPeriodHertz(50);
- fArm.setPeriodHertz(50);
- claw.setPeriodHertz(50);
- // 关联 servo 对象与 GPIO 引脚,设置脉宽范围
- base.attach(SERVO_base, MIN_WIDTH, MAX_WIDTH);
- rArm.attach(SERVO_rArm, MIN_WIDTH, MAX_WIDTH);
- fArm.attach(SERVO_fArm, MIN_WIDTH, MAX_WIDTH);
- claw.attach(SERVO_claw, MIN_WIDTH, MAX_WIDTH);
- //初始化电机角度
- base.write(90);
- delay(10);
- fArm.write(90);
- delay(10);
- rArm.write(90);
- delay(10);
- claw.write(90);
- delay(10);
-
- reportStatus();//输出当前电机角度信息
- Serial.begin(9600);
- SerialBT.begin("eh"); // 如果没有参数传入则默认是蓝牙名称是: "ESP32"
- Serial.printf("BT initial ok and ready to pair. \r\n");
- }
-
- void loop()
- {
- if (Serial.available()>0) {
- char serialCmd = Serial.read();
- delay(10);
- SerialBT.write(serialCmd);
- delay(10);
- if( mode == 1 ){
- armCmd(serialCmd); //指令模式
- } else {
- armJoy(serialCmd); //手柄模式
- }
- }
-
- if (SerialBT.available())
- {
- //Serial.write(SerialBT.read());
- char serialCmd = SerialBT.read();
- Serial.print(" ");
- Serial.write(serialCmd);
- Serial.println(" ");
- if( mode == 1 ){
- armCmd(serialCmd); //指令模式
- } else {
- armJoy(serialCmd); //手柄模式
- }
- }
-
- }
-
- void armJoy(char serialCmd) //手柄模式
- {
- int baseJoyPos;
- int rArmJoyPos;
- int fArmJoyPos;
- int clawJoyPos;
- switch(serialCmd)
- {
- case 'a':
- Serial.println("Received Command: a");
- baseJoyPos = base.read()+moveStep;
- servoMove('b',baseJoyPos,DSD);
- break;
- case 'd':
- Serial.println("Received Command: d");
- baseJoyPos = base.read()-moveStep;
- servoMove('b',baseJoyPos,DSD);
- break;
- case 'w':
- Serial.println("Received Command: w");
- baseJoyPos = rArm.read()+moveStep;
- servoMove('r',baseJoyPos,DSD);
- break;
- case 's':
- Serial.println("Received Command: s");
- baseJoyPos = rArm.read()-moveStep;
- servoMove('r',baseJoyPos,DSD);
- break;
- case '8':
- Serial.println("Received Command: 8");
- baseJoyPos = fArm.read()+moveStep;
- servoMove('f',baseJoyPos,DSD);
- break;
- case '5':
- Serial.println("Received Command: 5");
- baseJoyPos = fArm.read()-moveStep;
- servoMove('f',baseJoyPos,DSD);
- break;
- case '4':
- Serial.println("Received Command: 4");
- baseJoyPos = claw.read()+moveStep;
- servoMove('c',baseJoyPos,DSD);
- break;
- case '6':
- Serial.println("Received Command: 6");
- baseJoyPos = claw.read()-moveStep;
- servoMove('c',baseJoyPos,DSD);
- break;
- case 'm' : //切换至指令模式
- mode = 1;
- Serial.println("Command: Switch to Instruction Mode.");
- break;
- case 'o':
- reportStatus();
- break;
- case 'i':
- armIniPos();
- break;
- default:
- Serial.println("Unknown Command.");
- return;
-
- }
- }
-
- void armCmd(char serialCmd) //命令模式
- {
- if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){
- int servoData = Serial.parseInt();
- servoMove(serialCmd, servoData, DSD); // 机械臂舵机运行函数(参数:舵机名,目标角度,延迟/速度)
- } else {
- switch(serialCmd){
- case 'o': // 输出舵机状态信息
- reportStatus();
- break;
- case 'i':
- armIniPos();
- break;
- case 'm' : //切换至手柄模式
- mode = 0;
- Serial.println("Command: Switch to Joy-Stick Mode.");
- break;
- default: //未知指令反馈
- Serial.println("Unknown Command.");
- }
- }
- }
-
-
- void armIniPos(){ //初始化电机角度
- Serial.println("+Command: Restore Initial Position.");
- int robotIniPosArray[4][3] = {
- {'b', 90, DSD},
- {'r', 90, DSD},
- {'f', 90, DSD},
- {'c', 90, DSD}
- };
-
- for (int i = 0; i < 4; i++){
- servoMove(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);
- }
- }
-
- void servoMove(char servoName, int toPos, int servoDelay) //电机移动
- {
- //串口监视器输出接收指令信息
- Serial.println("");
- Serial.print("+Command: Servo ");
- Serial.print(servoName);
- Serial.print(" to ");
- Serial.print(toPos);
- Serial.print(" at servoDelay value ");
- Serial.print(servoDelay);
- Serial.println(".");
- Serial.println("");
-
- int fromPos; //建立变量,存储电机起始运动角度值
- switch(servoName){
- case 'b':
- if(toPos >= baseMin && toPos <= baseMax){
- fromPos = base.read(); // 获取当前电机角度值用于“电机运动起始角度值”
- if (fromPos <= toPos){ //如果“起始角度值”小于“目标角度值”
- for (int i=fromPos; i<=toPos; i++){
- base.write(i);
- delay (servoDelay);
- //Serial.println(base.read());
- }
- } else { //否则“起始角度值”大于“目标角度值”
- for (int i=fromPos; i>=toPos; i--){
- base.write(i);
- delay (servoDelay);
- //Serial.println(base.read());
- }
-
- }
- break;
- } else {
- Serial.println("+Warning: Base Servo Value Out Of Limit!");
- return;
- }
-
- case 'c':
- if(toPos >= clawMin && toPos <= clawMax){
- fromPos = claw.read(); // 获取当前电机角度值用于“电机运动起始角度值”
- if (fromPos <= toPos){ //如果“起始角度值”小于“目标角度值”
- for (int i=fromPos; i<=toPos; i++){
- claw.write(i);
- delay (servoDelay);
- //Serial.println(claw.read());
- }
- } else { //否则“起始角度值”大于“目标角度值”
- for (int i=fromPos; i>=toPos; i--){
- claw.write(i);
- delay (servoDelay);
- // Serial.println(claw.read());
- }
-
- }
- break;
- } else {
- Serial.println("+Warning: Claw Servo Value Out Of Limit!");
- return;
- }
-
- case 'f':
- if(toPos >= fArmMin && toPos <= fArmMax){
- fromPos = fArm.read(); // 获取当前电机角度值用于“电机运动起始角度值”
- if (fromPos <= toPos){ //如果“起始角度值”小于“目标角度值”
- for (int i=fromPos; i<=toPos; i++){
- fArm.write(i);
- delay (servoDelay);
- //Serial.println(fArm.read());
- }
- } else { //否则“起始角度值”大于“目标角度值”
- for (int i=fromPos; i>=toPos; i--){
- fArm.write(i);
- delay (servoDelay);
- //Serial.println(fArm.read());
- }
-
- }
- break;
- } else {
- Serial.println("+Warning: fArm Servo Value Out Of Limit!");
- return;
- }
-
- case 'r':
- if(toPos >= rArmMin && toPos <= rArmMax){
- fromPos = rArm.read(); // 获取当前电机角度值用于“电机运动起始角度值”
- if (fromPos <= toPos){ //如果“起始角度值”小于“目标角度值”
- for (int i=fromPos; i<=toPos; i++){
- rArm.write(i);
- delay (servoDelay);
- //Serial.println(rArm.read());
- }
- } else { //否则“起始角度值”大于“目标角度值”
- for (int i=fromPos; i>=toPos; i--){
- rArm.write(i);
- delay (servoDelay);
- //Serial.println(rArm.read());
- }
-
- }
- break;
- } else {
- Serial.println("+Warning: rArm Servo Value Out Of Limit!");
- return;
- }
- }
-
- }
-
- void reportStatus(){ //舵机状态信息
- Serial.println("");
- Serial.println("");
- Serial.println("+ Robot-Arm Status Report +");
- Serial.print("Claw Position: "); Serial.println(claw.read());
- Serial.print("Base Position: "); Serial.println(base.read());
- Serial.print("Rear Arm Position:"); Serial.println(rArm.read());
- Serial.print("Front Arm Position:"); Serial.println(fArm.read());
- Serial.println("++++++++++++++++++++++++++");
- Serial.println("");
- }
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。