# the 'arduino_sketches/easyEEZYbotARM_Arduino_Communication' folder must be uploaded
# to the arduino.
# This example shows the basis for moving the arduino servos. It does not link the 'kinematic model' together
# with the Arduino. For an example of this please see example5_connect_kinematic_model_to_EEZYbotARM.py
# The servos are plugged into the following pin positions on the PCA9685 board:
# Servo_q1 -> EzzyBot base (q1) -> PCA9685 pin 1
# Servo_q2 -> Main arm (q2) -> PCA9685 pin 2
# Servo_q3 -> Horarm (q3) -> PCA9685 pin 3
# Servo_EE -> End Effector (EE) -> PCA9685 pin 0
from easyEEZYbotARM.serial_communication import arduinoController
# Insert your Arduino serial port here
myArduino = arduinoController(port="COM3")
# Create some test data.
# The angles used for this test data are 'raw' servo angles (i.e. not calibrated)
# against the kinematic model for the EEZYbotARM. This example moves the EzzyBot base (q1)
testData = []
testData.append(myArduino.composeMessage(servoAngle_q1=90,
servoAngle_q2=90,
servoAngle_q3=90,
servoAngle_EE=10))
testData.append(myArduino.composeMessage(servoAngle_q1=90,
servoAngle_q2=90,
servoAngle_q3=90,
servoAngle_EE=90))
def __init__(self, arduino_port = "/dev/ttyUSB0"):
self.arduino = arduinoController(port=arduino_port)
self.arduino.openSerialPort()
self.arm = EEZYbotARM_Mk2(initial_q1=0, initial_q2=90, initial_q3=-90)
self.homePos = [0, 110, -140]
self.return_home()
# Import EEZYbotARM library
from easyEEZYbotARM.kinematic_model import EEZYbotARM_Mk2
from easyEEZYbotARM.serial_communication import arduinoController
import time
import easyEEZYbotARM
import cv2 as cv
from imutils.video import VideoStream
# Insert your Arduino serial port here to initialise the arduino controller
myArduino = arduinoController(port="/dev/ttyUSB0")
myArduino.openSerialPort()
import numpy as np
# Initialise kinematic model with initial joint angles (home position)
robot = EEZYbotARM_Mk2(
initial_q1=0, initial_q2=90, initial_q3=-90)
# Plot it
#myVirtualRobotArm.plot()
# Define end effector open and closed angle
servoAngle_EE_closed = 15
servoAngle_EE_open = 90
# Calculate the current servo angles
servoAngle_q1, servoAngle_q2, servoAngle_q3 = robot.map_kinematicsToServoAngles()
print("Initial = , ", servoAngle_q1, ", ", servoAngle_q2, ", ", servoAngle_q3)
# a1 = 0
# a2 = 45
# a3 = -45
# x, y, z = robot.forwardKinematics(q1=a1, q2=a2, q3=a3)