更多创客作品,请关注笔者网站园丁鸟,搜集全球极具创意,且有价值的创客作品
ROS机器人知识请关注,diegorobot
业余时间完成的一款在线统计过程分析工具SPC,及SPC知识分享网站qdo
ROS Arduino Bridge本质上是上位机通过串口发送控制命令来实现对Arduino的控制,所以我们要实现4驱的控制,我们也必须的修改通讯部分
1.Arduino firmware修改
1.1 command.h
在此文件中增加4驱所需的命令及宏定义
#ifndef COMMANDS_H
#define COMMANDS_H
#define ANALOG_READ 'a'
#define GET_BAUDRATE 'b'
#define PIN_MODE 'c'
#define DIGITAL_READ 'd'
#define READ_ENCODERS 'e'
#define MOTOR_SPEEDS 'm'
#define PING 'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE 's'
#define SERVO_READ 't'
#define UPDATE_PID 'u'
#define DIGITAL_WRITE 'w'
#define ANALOG_WRITE 'x'
#define LEFT 0
#define RIGHT 1
#define LEFT_H 2 //新增
#define RIGHT_H 3 //新增
#define READ_PIDOUT 'f'
#define READ_PIDIN 'i'
#define READ_MPU6050 'g'
#endif
1.2 RosArduinoBridge-diego.ino
此文件是主程序,由于此文件代码比较多,故这里只介绍新增部分代码,完整的代码请见github
在runCommand()函数中修改在4驱模式下读取pidin 的代码
case READ_PIDIN:
Serial.print(readPidIn(LEFT));
Serial.print(" ");
#ifdef L298P
Serial.println(readPidIn(RIGHT));
#endif
#ifdef L298P_4WD
Serial.print(readPidIn(RIGHT));
Serial.print(" ");
Serial.print(readPidIn(LEFT_H));
Serial.print(" ");
Serial.println(readPidIn(RIGHT_H));
#endif
break;
在runCommand()函数中修改在4驱模式下读取pidout 的代码
case READ_PIDOUT:
Serial.print(readPidOut(LEFT));
Serial.print(" ");
#ifdef L298P
Serial.println(readPidOut(RIGHT));
#endif
#ifdef L298P_4WD
Serial.print(readPidOut(RIGHT));
Serial.print(" ");
Serial.print(readPidOut(LEFT_H));
Serial.print(" ");
Serial.println(readPidOut(RIGHT_H));
#endif
break;
在runCommand()函数中修改在4驱模式下读取编码器数据的代码
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
#ifdef L298P
Serial.println(readEncoder(RIGHT));
#endif
#ifdef L298P_4WD
Serial.print(readEncoder(RIGHT));
Serial.print(" ");
Serial.print(readEncoder(LEFT_H));
Serial.print(" ");
Serial.println(readEncoder(RIGHT_H));
#endif
break;
在runCommand()函数中修改在4驱模式下设置马达速度的代码
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
#ifdef L298P
setMotorSpeeds(0, 0);
#endif
#ifdef L298P_4WD
setMotorSpeeds(0, 0, 0, 0);
#endif
moving = 0;
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
#ifdef L298P_4WD
leftPID_h.TargetTicksPerFrame = arg1;
rightPID_h.TargetTicksPerFrame = arg2;
#endif
Serial.println("OK");
break;
在runCommand()函数中修改在4驱模式下更新PID参数的代码
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
left_Kp = pid_args[0];
left_Kd = pid_args[1];
left_Ki = pid_args[2];
left_Ko = pid_args[3];
right_Kp = pid_args[4];
right_Kd = pid_args[5];
right_Ki = pid_args[6];
right_Ko = pid_args[7];
#ifdef L298P_4WD
left_h_Kp = pid_args[0];
left_h_Kd = pid_args[1];
left_h_Ki = pid_args[2];
left_h_Ko = pid_args[3];
right_h_Kp = pid_args[4];
right_h_Kd = pid_args[5];
right_h_Ki = pid_args[6];
right_h_Ko = pid_args[7];
#endif
Serial.println("OK");
break;
在setup()函数中设置在4驱模式下新增的pin对应的寄存器的操作
void setup() {
Serial.begin(BAUDRATE);
#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);
#ifdef L298P_4WD
DDRD &= ~(1 << LEFT_H_ENC_PIN_A);
DDRD &= ~(1 << LEFT_H_ENC_PIN_B);
DDRC &= ~(1 << RIGHT_H_ENC_PIN_A);
DDRC &= ~(1 << RIGHT_H_ENC_PIN_B);
#endif
//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);
#ifdef L298P_4WD
PORTD &= ~(1 << LEFT_H_ENC_PIN_A);
PORTD &= ~(1 << LEFT_H_ENC_PIN_B);
PORTC &= ~(1 << RIGHT_H_ENC_PIN_A);
PORTC &= ~(1 << RIGHT_H_ENC_PIN_B);
#endif
// 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);
#ifdef L298P_4WD
// tell pin change mask to listen to left encoder pins
PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B) | (1 << LEFT_H_ENC_PIN_A) | (1 << LEFT_H_ENC_PIN_B);
// tell pin change mask to listen to right encoder pins
PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B) | (1 << RIGHT_H_ENC_PIN_A) | (1 << RIGHT_H_ENC_PIN_B);
#endif
// 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++) {
servosPos[i]=90;
}
servodriver.begin();
servodriver.setPWMFreq(50);
#endif
}
在loop()函数中修改在4驱模式下自动停止的逻辑
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
;
#ifdef L298P
setMotorSpeeds(0, 0);
#endif
#ifdef L298P_4WD
setMotorSpeeds(0, 0, 0, 0);
#endif
moving = 0;
}
#endif
2.上位机代码修改
上位机的代码修改主要是针对arduino_driver.py和base_controller.py的修改。
2.1 arduino_driver.py修改
修改def get_pidin(self):使其支持4个电机的pidin读取
def get_pidin(self):
values = self.execute_array('i')
print("pidin_raw_data: "+str(values))
if len(values) not in [2,4]:
print "pidin was not 2 or 4 for 4wd"
raise SerialException
return None
else:
return values
修改def get_pidout(self):使其支持4个电机的pidout读取
def get_pidout(self):
values = self.execute_array('f')
print("pidout_raw_data: "+str(values))
if len(values) not in [2,4]:
print "pidout was not 2 or 4 for 4wd"
raise SerialException
return None
else:
return values
修改def get_encoder_counts(self):使其支持4个电机的编码器数据读取
def get_encoder_counts(self):
values = self.execute_array('e')
if len(values) not in [2,4]:
print "Encoder count was not 2 or 4 for 4wd"
raise SerialException
return None
else:
修改完后,将arduino的固件更新,就可以启动4驱底盘控制了,要注意的时候要同时打开上位机和arduino上的4驱开关,如果是使用2驱的代码,也要记得关掉4驱的开关。
3.启动4驱底盘控制
现在执行如下命令可以启动4驱底盘控制
roslaunch diego_nav diego_arduino_run.launch
现在我们可以通过配套的ROS APP连接Diego#进行控制
首先创建一个新的ROS机器人连接,设置相应的ROS Master ip,和cmd_vel主题。