ROS機器人Diego 1#製作(十四)機械臂的控制---arduino驅動
今天從淘寶上訂購的兩個機械臂終於到貨了,從這篇開始逐步講解ROS機械臂的控制,這兩個機械臂又花掉了800塊,配的MG99R模擬舵機,如果配數字舵機每個機械臂要555元,價格還是有點小貴。機械臂安裝好後如下圖
本來有一塊通過串列埠控制24路舵機控制的模組,但由於樹莓派的串列埠已經被藍芽所使用,現在藍芽還要接藍芽音箱,所以只好在找其他的舵機控制版,最後找到這款2路電機,和16路舵機控制模組,可以直接插在arduino上,通過ardunio的i2c介面控制舵機,同時通過arduino的D6、D7、D11、D12口控制電機,正好滿足需求,而且其還留了兩排arduino引腳插孔,可以自己焊接上兩排插針,做擴充套件板用,這塊板子淘寶價格是58元。
下圖是裝完機械臂後的Diego 1#的圖片,看到是挺酷的
1.修改電機驅動程式碼
因為更換了電機驅動板,所以先要更改一下電機驅動的程式碼,其實很簡單,只需要修改DualL298PMotorShield_h中介面中其中一個電機的控制對應arduino引腳,由原來的4和5改為11和12就可以了,程式碼如下:
#ifndef DualL298PMotorShield_h
#define DualL298PMotorShield_h
#include <Arduino.h>
class DualL298PMotorShield
{
public:
// CONSTRUCTORS
DualL298PMotorShield(); // Default pin selection.
DualL298PMotorShield(unsigned char M1DIR, unsigned char M1PWM,
unsigned char M2DIR, unsigned char M2PWM); // User-defined pin selection.
// PUBLIC METHODS
void init(); // Initialize TIMER 1, set the PWM to 20kHZ.
void setM1Speed(int speed); // Set speed for M1.
void setM2Speed(int speed); // Set speed for M2.
void setSpeeds(int m1Speed, int m2Speed); // Set speed for both M1 and M2.
private:
//static const unsigned char _M1DIR = 5;
static const unsigned char _M1DIR = 12;
static const unsigned char _M2DIR = 7;
//static const unsigned char _M1PWM = 4;
static const unsigned char _M1PWM = 11;
static const unsigned char _M2PWM = 6;
};
#endif
2.修改碼盤資料讀取的程式碼
由於這款舵機控制模組是通過arduino 的I2C介面控制舵機,即arduino上的A4,A5。我們之前有一個電機碼盤的資料是通過A4,A5讀取的,所以修改到其他其他的模擬量介面,我在這裡用了A2,A3。只需要修改encoder_driver.h對應介面引腳即可,同時要保證實際的連線也要做相應的調整,程式碼如下:
#ifdef ARDUINO_ENC_COUNTER
//below can be changed, but should be PORTD pins;
//otherwise additional changes in the code are required
#define LEFT_ENC_PIN_A PD2 //pin 2
#define LEFT_ENC_PIN_B PD3 //pin 3
//below can be changed, but should be PORTC pins
//#define RIGHT_ENC_PIN_A PC4 //pin A4
#define RIGHT_ENC_PIN_A PC2 //pin A2
//#define RIGHT_ENC_PIN_B PC5 //pin A5
#define RIGHT_ENC_PIN_B PC3 //pin A3
#endif
long readEncoder(int i);
void resetEncoder(int i);
void resetEncoders();
3.舵機控制
修改servos.h
//定義12 個舵機
#define N_SERVOS 12
//Servo servos [N_SERVOS];
//byte servoPins [N_SERVOS] = {3, 5};//由於本身沒有使用arduino的PWM引腳控制舵機,所以這行註釋
short servoslastpos [N_SERVOS];//0~180//記錄舵機當前的位置
修改ROSArduinoBridge-diego.uno的void setup()的servo初始化程式碼。
void setup() {
Serial.begin(BAUDRATE);
// Initialize the motor controller if used */
#ifdef USE_BASE
#ifdef ARDUINO_ENC_COUNTER
//set as inputs
DDRD &= ~(1 << LEFT_ENC_PIN_A);
DDRD &= ~(1 << LEFT_ENC_PIN_B);
DDRC &= ~(1 << RIGHT_ENC_PIN_A);
DDRC &= ~(1 << RIGHT_ENC_PIN_B);
//enable pull up resistors
PORTD |= (1 << LEFT_ENC_PIN_A);
PORTD |= (1 << LEFT_ENC_PIN_B);
PORTC |= (1 << RIGHT_ENC_PIN_A);
PORTC |= (1 << RIGHT_ENC_PIN_B);
// tell pin change mask to listen to left encoder pins
PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B);
// tell pin change mask to listen to right encoder pins
PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B);
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
initMotorController();
resetPID();
#endif
/* Attach servos if used */
#ifdef USE_SERVOS
//int i;
//for (i = 0; i < N_SERVOS; i++) {
//servos[i].attach(servoPins[i]);
//}
int i;
for (i = 0; i < N_SERVOS; i++) {
servosPos[i]=90;
}
servodriver.begin();
servodriver.setPWMFreq(50);
#endif
}
修改頭部的巨集定義,啟用servo
#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
//#undef USE_SERVOS // Disable use of PWM servos
修改頭部的巨集定義,增加servo的引用和ServoDriver的控制變數servodriver
#ifdef USE_SERVOS
//#include <Servo.h>
#include "servos.h"
#include <Wire.h>
#include <ServoDriver.h>
#define SERVOMIN 102 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 512 // this is the 'maximum' pulse length count (out of 4096)
ServoDriver servodriver = ServoDriver();
#endif
修改runCommand舵機控制部分程式碼如下
#ifdef USE_SERVOS
case SERVO_WRITE:
//servos[arg1].write(arg2);
servodriver.setPWM(arg1,0,short(SERVOMIN+arg2*(SERVOMAX-SERVOMIN)/180));//將舵機我這0~180轉換為舵機控制脈衝長度
servosPos[arg1]=short(arg2);
Serial.println("OK");
break;
case SERVO_READ:
//Serial.println(servos[arg1].read());
Serial.println(servosPos[arg1]);
break;
#endif