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主題。