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.
01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 | #!/usr/bin/python3# -*- coding: utf-8 -*-from __future__ import division # Pour division floatfrom pprint import pprintimport pigpioimport time# CablageTREX_ADDRESS = 0x07SDA=2 # Pin no 3SCL=3 # Pin no 5# Connexion de pigpiodpigpio = 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() |
