Piloter un servomoteur avec le contrôleur T’REX

Un des caractéristiques du contrôleur T’REX est qu’il peut contrôler jusqu’à 6 servomoteurs (4 si on utilise 2 encodeurs). Pour cela, on envoie la commande de position de la même manière que le contrôle des moteurs.

 

Câblage

La carte T’REX propose 6 emplacements où connecter de servomoteurs. Le servomoteur à l’index 0 est est réalité situé au 3ème emplacement, vu que les 2 premiers sont réservés en priorité aux encodeurs.

La batterie utilisée pour l’alimentation étant de 7.2v, la carte a dû être configurée en mode Low Voltage. Ce mode implique que la broche d’alimentation des servomoteurs n’est plus de +6V, mais devient la tension de la batterie. La majorité des servomoteurs ne supportent pas une tension si élevée. Dans mon cas, j’utilise un TowerPro MG995 qui accepte une tension d’entrée située entre 4.8V et 6V. La solution consiste à connecter le fil d’alimentation sur la borne +5v de la carte T’REX. Seuls les fils de commande et la masse seront connectés sur les emplacements destinés au servomoteurs.


 

Le code

Une fois de plus, je reprends le code écrit précédemment sur l’envoi de données au contrôleur T’REX.
Cette fois, on contrôle uniquement la position du servomoteur, en la faisant varier de 1000 à 2000. Toutes les 500ms, la valeur va s’incrémenter de 100.

#!/usr/bin/python3
# -*- coding: utf-8 -*-

from __future__ import division # Pour division float
from pprint import pprint
import pigpio
import time

# Cablage
TREX_ADDRESS = 0x07
SDA=2 # Pin no 3
SCL=3 # Pin no 5

# Connexion de pigpiod
pigpio = pigpio.pi()

def trex_command():
	'''
	Envoi une requete de commande au T'REX
	'''
	
	# Verification de la connexion de pigpiod
	if not pigpio.connected:
		exit(0)
	
	# Preparation de la requete Bit Bang
	pigpio.bb_i2c_open(SDA, SCL, baud=100000)
	
	# Preparation des donnees
	right_motor_bytes = right_motor_speed.to_bytes(2, byteorder='big', signed=True)
	left_motor_bytes = left_motor_speed.to_bytes(2, byteorder='big', signed=True)
	servo_0_bytes = servo.to_bytes(2, byteorder='big');
	servo_1_bytes = (0).to_bytes(2, byteorder='big');
	servo_2_bytes = (0).to_bytes(2, byteorder='big');
	servo_3_bytes = (0).to_bytes(2, byteorder='big');
	servo_4_bytes = (0).to_bytes(2, byteorder='big');
	servo_5_bytes = (0).to_bytes(2, byteorder='big');
	impact_bytes = (50).to_bytes(2, byteorder='big');
	battery_bytes = (550).to_bytes(2, byteorder='big');

	# Envoi de la requete
	pigpio.bb_i2c_zip(SDA, [
		4, TREX_ADDRESS,							# Set I2C adress to TREX_ADDRESS
		2,											# Start condition
		7, 27,										# Write 27 bytes of data
		0x0F,										# 1. Start byte
		0x06,										# 2. PWM frequency -> 122Hz
		left_motor_bytes[0], left_motor_bytes[1],	# 3-4. Left motor speed
		0x00,										# 5 Left motor brake
		right_motor_bytes[0], right_motor_bytes[1],	# 6-7. Right motor speed
		0x00,										# 8. Right motor brake
		servo_0_bytes[0], servo_0_bytes[1],			# 9-10. Servo 0 position
		servo_1_bytes[0], servo_1_bytes[1],			# 11-12. Servo 1 position
		servo_2_bytes[0], servo_2_bytes[1],			# 13-14. Servo 2 position
		servo_3_bytes[0], servo_3_bytes[1],			# 15-16. Servo 3 position
		servo_4_bytes[0], servo_4_bytes[1],			# 17-18. Servo 4 position
		servo_5_bytes[0], servo_5_bytes[1],			# 19-20. Servo 5 position
		0x32,										# 21. Accelerometer de-vibrate -> 50
		impact_bytes[0], impact_bytes[1],			# 22-23. Impact sensitivity
		battery_bytes[0], battery_bytes[1],			# 24-25. Low battery
		TREX_ADDRESS,								# 26. I2C address
		0x00,										# 27. Clock frequency
		3,											# Stop condition
		0											# No more commands
	])
	
	# Fin de la requete Bit Bang	
	pigpio.bb_i2c_close(SDA)
		
	
if __name__ == '__main__':
	left_motor_speed = 0
	right_motor_speed = 0
	servo = 0
	
	# Nouvelle position du servo toutes les 500 ms
	for i in range(1000,2100, 100):
		servo = i
		trex_command()
		time.sleep(0.5)
		
	# Deconnexion de pigpiod
	pigpio.stop()

Laisser un commentaire

Votre adresse de messagerie ne sera pas publiée. Les champs obligatoires sont indiqués avec *