O controlo dos motores no chefbot é efectuado pela interação dos seguintes nodes:
twist_to_motors.py, que subscreve o tópico cmd_vel, e publica os tópicos left e right target velocities
pid_velocity.py, com duas instâncias, left e right, que subscrevem os tópicos das respectivas target velocities, e posição dos encoders, e publicam os tópicos com a velocidade (pwm) a aplicar aos motores.
launchpad_node, que subscreve os tópicos com a velocidade a aplicar nos motores, left e right, e publica a posição dos respectivos
O launchpad_node é o responsável pela comunicação com o software que é executado no microcontrolador. No meu caso um Arduino mega.
O Microcontrolador cumpre as seguintes tarefas:
- Comunicar com computador (launchpad_node).
- Ler a posição dos encoders associados aos motores
- Actuar na direção e velocidade dos motores (via ponte h)
O loop do programa a correr no arduino é o seguinte
Read_From_Serial(); Update_Time(); Update_Encoders(); Update_Ultra_Sonic(); Update_Motors(); Update_MPU6050();
As funções relevantes para o controlo dos motores são as seguintes
- Read_From_Serial()
- Update_Encoders()
- Update_Motors()
Existe ainda outro node relacionado com o assunto, o teleop.
Leitura dos encoders no chefbot
Os encoders dos motores esquerdo e direito são do tipo quadrature e cada um tem dois pinos de output.
O microcontrolador tem para cada encoder uma função associada a um pino com capacidade de interromper o programa e chamar a função que processa o estado dos dois pinos e actualiza uma variável com a posição do encoder.
A posição do encoder é comunicada no loop pela função Update_Encoders().
O launchpad_node, recebe as posições dos encoders e publica os valores nos respectivos tópicos.
Os tópicos com a posição dos encoders são subscritos pelas respectivas instâncias do node pid_velocity.py, que fecha o circuito ao calcular o valor adequado da potência a enviar aos motores, face a velocidade real calculada com base na posição dos encoders e a velocidade desejada, que recebe pela subscrição dos respectivos tópicos.
Direcção e velocidade dos motores no chefbot
A direção e velocidade dos motores no chefbot são calculadas no computador e após a sua comunicação, aplicadas nos motores pelo microcontrolador através de uma ponte H.
Existem portanto dois dispositivos intervenientes no processo de controlo da direção e velocidade dos motores no chefbot.
No computador tem relevo os nodes twist_to_motors.py e e em em especial as duas instâncias do node pid_velocity.py.
No microcontrolador tem especial relevo as funções moveLeftMotor() e moveRightMotor(), que funcionam da mesma forma, ao usar o sinal do valor do pwm para definir a direção, e o seu valor absoluto como potência.
Cálculo da direção e velocidade dos motores no chefbot
O cálculo da direção e velocidade dos motores no chefbot é efectuado no computador pelo node twist_to_motors.py e os dois nodes pid_velocity.py.
Numa primeira fase, no computador, o node twist_to_motors.py recebe no tópico adequado uma mensagem com a indicação da velocidade linear e velocidade angular, desejada, calcula a velocidade de cada roda e publica-as no respectivo tópico.
Numa segunda fase, os dois nodes pid_velocity.py, recebem as mensagens com a velocidade desejada para cada uma das rodas, e a posição dos encoders.
Controlo PID da velocidade no chefbot
Os nodes que efectuam o controlo PID da velocidade no chefbot são duas instâncias do script pid_velocity.py que o valor da potência a aplicar nos motores em função da velocidade desejada e efectiva.
O forma como o node implementa o controlo da velocidade parece implicar um fluxo contínuo de mensagens nos tópicos que definem as velocidades desejadas para cada roda (o output do node twist_to_motors.py).
spin
while not rospy.is_shutdown(): self.spinOnce() self.r.sleep()
spinOnce
# only do the loop if we've recently recieved a target velocity message while not rospy.is_shutdown() and self.ticks_since_target < self.timeout_ticks: self.calcVelocity() self.doPid() self.pub_motor.publish(self.motor) self.r.sleep() self.ticks_since_target += 1 if self.ticks_since_target == self.timeout_ticks: self.pub_motor.publish(0)
Aplicação da direção e velocidade dos motores no chefbot
A aplicação de uma nova velocidade e direção nos motores acontece após a recepção da mensagem durante a execução da função Read_From_Serial() que indica a velocidade dos motores, os respectivos valores são guardados na seguinte função:
void Set_Speed() {
motor_left_speed = Messenger_Handler.readLong();
motor_right_speed = Messenger_Handler.readLong();
Os valores são usados pela seguinte função:
void Update_Motors() {
moveRightMotor(motor_right_speed);
moveLeftMotor(motor_left_speed);
Por último, os valores são aplicados na ponte h da seguinte forma:
void moveLeftMotor(float leftServoValue) {
if (leftServoValue > 0) {
digitalWrite(A_2,LOW);
digitalWrite(B_2,HIGH);
analogWrite(PWM_2,leftServoValue);
} else if(leftServoValue < 0) {
digitalWrite(A_2,HIGH);
digitalWrite(B_2,LOW);
analogWrite(PWM_2,abs(leftServoValue));
} else if(leftServoValue == 0) {
digitalWrite(A_2,HIGH);
digitalWrite(B_2,HIGH);
}
Se o valor da velocidade for maior que zero, o motor roda para a frente com a velocidade indicada,
Se o valor da velocidade for menor que zero, o motor roda para traz com o valor absoluto da velocidade indicada,
Se o valor da velocidade for igual a zero, o motor para.