Csit.selu.edu



2 DOF Robotic ArmZachary Guy5/9/19Spring 2019 – ET 494Advisor: Dr. Mohammad SaadehInstructor: Dr. Mohammad SaadehAbstractThis project builds upon a past project completed by Dr. Saadeh and a former student. The former project dealt with controlling the position, speed, and acceleration of a single phase 110V AC servo motor. This project is an extension of that as a 2 DOF Robotic Arm is attached to the end of the motor, and with the use of an additional motor, the two links of the robotic arm are able to rotate independently. These arms are connected and maneuvered via pulleys and belts and direct attachment to the shafts of the AC motors. A certain profile will seek to be obtained and controlled with the use of a teaching pendant. The project covers such areas as: mechanisms, motion, dynamics, motor control, programming, and electronics.IntroductionIn a recent Senior Design project, Dr. Saadeh and a former Engineering Technology (ET) student worked on controlling the position, speed, and acceleration of a single-phase 110V AC servo motor. This control was obtainable with the programming and use of a teach pendant. The previous design didn’t transmit the motion to an arm. Rather, only a 3D printed shaft sleeve with an arrow was attached to the shaft of the motor to show the direction of rotation and positioning accuracy of the system. This project continues the previous efforts through the addition of a second motor and two independently moving arms to achieve a 2 DOF (Two Degrees of Freedom) Robotic Arm. Each motor will be responsible for moving one of the two arms and controlling its position, speed, and acceleration. The system uses pulleys and belts that are either fixed to each arm (thus the arm rotates with the pulley) or allowed to freely revolve around a shaft (to transmit motion to another arm). An additional AC servo motor will be acquired for this purpose, then the system will be controllable through the use of a teach pendant.BackgroundThe term “robotics” as it is presently used, was not introduced until 1941 by Science Fiction writer Isaac Asimov. The use of the word is to describe the technology, study, and use of robots. The first industrial robot was designed by George Devol. It was called the Unimate, and helped to revolutionize the manufacturing world. It was conceived from a design for a mechanical arm patented in 1954 (granted in 1961) by Devol, and was developed as a result of the foresight and business acumen of Joseph Engelberger - the Father of Robotics.“The number of industrial robots deployed worldwide will increase to around 2.6 million units by 2019. That’s about one million units more than in the record-breaking year of 2015. Broken down according to sectors, around 70 percent of industrial robots are currently at work in the automotive, electrical/electronic and metal and machinery industry segments. In 2015, the strongest growth in the number of operational units recorded here was registered in the electronics industry, which boasted a rise of 18 percent. The metal industry posted an increase of 16 percent, with the automotive sector growing by 10 percent.”“The USA is currently the fourth largest single market for industrial robots in the world. Within the NAFTA area (USA, Canada and Mexico), the total number of newly installed industrial robots rose by 17 percent to a new record of some 36,000 units (2015). The leader of the pack was the USA, accounting for three-quarters of all units sold. 5 percent growth was recorded. With a comparatively much smaller amount of units, the demand in Canada increased by 49 percent (5,466 units), while that in Mexico grew by 119 percent (3,474 units). With a stable economic situation, it may be expected that North America will see average annual growth of 5 to 10 percent in sales of robots from 2016 to 2019.The USA plays a leading role when it comes to automation in the automotive industry. US car makers are ranked third in robot density, behind Japan and the Republic of Korea. The US automotive industry has performed well over the last six years. 2015 proved to be the most successful year since 2005. Major manufacturers from the US, Europe and Asia embarked on restructuring programmes resulting in the installation of some 80,000 industrial robots between 2010 and 2015. This is the largest investment worldwide, second only to China at around 90,000 units.” Purpose of ProjectThe overall purpose of this project was to design, build, program, and control a 2 DOF Robotic Arm. This project can be used as a teaching tool to instruct students and give them hands on experience in areas such as: robotics, programming, trajectory planning, and mechanical design.AccomplishmentsThe following have been achieved during the duration of this project:Design of 2 DOF Robotic ArmSelection of Timing Belt and corresponding pulleysDesign and 3D printing of Shaft AdaptersConstruction of frame for second Servo Drive and MotorConstruction of PrototypeRedesign of joint of 2 DOF Robotic ArmDevelopment of Arduino Code for control of 2 DOF Robotic ArmDevelopment of Arduino Code using dual axis joystick for controlDevelopment of Arduino Code using serial input for controlTrajectory Planning and ProfilingDeliverablesThe initial deliverables for this project included the following:Mechanical DesignMotion TransmissionLinkageSelection of Belts/PulleysOrder Motor and ControllerBuild PrototypeRedesign of Teach PendantControl of 2 DOF Robotic ArmTrajectory Planning and ProfilingAs the project progressed, the following deliverable was added:- Redesign of Robotic Arm JointOf these deliverables, the only one not obtained by the conclusion of this project was the redesign of the teach pendant. Challenges and SolutionsDuring this project there were challenges. Initially there was communication between one Arduino MEGA and both DYN4 Servo Drives as shown in the following diagram:After some time, there were communication failures between the Arduino MEGA and the second DYN4 Servo Drive. This issue was remedied by the acquisition and usage of a second Arduino MEGA communicating at first exclusively with the second DYN4 Servo Drive. This incurred another challenge as communication needed to happen between the two Arduino MEGA’s. After searching multiple Arduino Forums and the Arduino Reference Library a solution was obtained to use wire transmissions to send signals from the first Arduino to the second Arduino.After the redesign of the elbow joint of the 2 DOF Robotic Arm, there are limitations on the range of motion that the second link can achieve without doing damage to the system. In order to overcome this challenge, parameters were set in the code to not allow the links to rotate past a certain point in either direction. For the second link, this range was determined to be from -127° to +127°. For the first link, this range was determined to be from -90° to +180°. The limit of motion for the first link is to prevent the 2 DOF Robotic Arm from hitting and disconnecting the wires that are placed on the back side of the system. Research and EquationsThe following table contains the specs for the DMM Servo Motor:Rated Voltage200 VRated Output1.0 kWRated Torque4.77 N-m (3.53 ft-lb)Inst. Max. Torque14.3 N-m (10.58 ft-lb)Rated Current8.2 AMax. Current24.6 ARated Speed1500 rpmMax. Speed3000 rpmRotor Inertia8.5 kg*cm2 (2.901 lb*in2)Torque Coefficient0.774 N-m/AMass8.95 kg (0.613 slug/19.7 lbm)Radial Load Rating600 N (134.89 lb)Thrust Load Rating300 N (67.44 lb)RatingsTime Rating: ContinuousThermal Class: FExcitation Method: Permanent MagnetInsulation Resistance: DC500V, >20MΩNoise: ≤60dB; No Special NoiseEnvironmentAmbient Temperature: 0-40 °CStorage: -20-50°CAmbient Humidity: 20-80%No CondensationEnclosureIP65Shock98 m s2Max. (10G or 322 fts2)Applicable Servo DriveDYN4The following information is in regards to the DYN4 Servo Drive from DMM:Single PhaseRated Current – 8.0 ASpace Vector PWMModulation Algorithm which translates phase voltage coming from controllerDYN4-H01A2-00 - AC Servo Drives.Industry standard Position, Velocity, Torque servo modes4-wire high speed serial encoder bus, with 8-bit security codeDMMDRV4? graphical interface, Adaptive Tuning TechnologySerial [UART,SPI], Pulse/Sign, CW/CCW, A/B Quadrature, and Analog CommandA/B/Z Quadrature incremental encoder outputMotor Current, Absolute Position, Position Error Monitor OutputsRS485/RS232 networkingIntegrated Point to Point S-curve motion, linear & circular coordinated motionNewer pictures showing the current layout of the 2 DOF Robotic Arm are included after the References section. The following figure shows the initial design of the 2 DOF Robotic Arm:The following equations were used during the course of this project. A description of what they are used for follows each equation:L= 4C2-D-d2+12(DθD-dθd)Used for determining the length of the timing beltθd=π-2sin-1D-d2CUsed to determine θdθD=π+2sin-1D-d2CUsed to determine θD1695450264160The above equations are used in reference to the following diagram:The pulleys used for this project are equal in diameter; therefore the equation can be simplified –L= 4C2For this project, C = 24; therefore L = 48.The following five equations were used in the code for inverse kinematics:β=cos-1(L12+L22-D22*L1*L2)α=sin-1(L2*sinβD)θ1=tan-1(yx)-αθ2=180-βD=x2+y2Within the code, x = user input and y varies on the function being executed. The servo motors are from Dynamic Motor Motion, a Canadian company, and therefore the dimensions are in metric units. The shafts for the motors were approximately 19 mm. The C-Channel procured from ServoCity, a US company, were in US Customary Units. The C-channel utilized a 0.770” hole pattern. In order to overcome this, an adapter needed to be used. After researching the market, it was determined that an adapter meeting the required specifications would need to be designed and 3D printed. The following figures show the final design of these shaft adapters:3419475266700-19050266700CodingAll of the code used for this project is included as an appendix after the References section of the report. There are two separate codes being used to control the 2 DOF Robotic Arm. The first is referred to as the Master Code, and the second is referred to as the Slave Code. The Master Code operates as follows:Asks for user input to select a function: A, B, or C.A: y=xB: y=110x2C: r2=(x-h)2+(y-k)2If A is selected: asks for user to input x values. Runs function LineIf B is selected: asks for user to input x values. Runs function ParabolaIf C is selected: asks for user to input center of circle at (h,k) and for radius. Runs function CircleWithin each function, a for loop increments the value of x to a predetermined end value, the angles that both motors need to turn is calculated based upon starting position, and then a move command is sent to the first Servo Drive.Note: The code utilizes if statements to check if the calculated angles can be safely reached. If these angles cannot be reached – no move command is given.Wire transmission event sends four bytes of data to the second Arduino. The first byte contains the angle that the first motor rotates, and the second byte contains the calculated angle for the second motor. The third byte is for checking if the actual angle for the second motor is less than 1.0 or greater than -1.0, and the fourth byte is for checking if the actual angle for the first motor is less than 1.0 or greater than -1.0.The calculated angles are now saved as previous angles to use as reference point for next set of calculated angles. This information is printed to the serial monitor to check for accuracy.The Slave Code operates as follows:Wire transmission event begins when the first Arduino sends the four bytes previously mentioned. This information is used to determine what the correct angle for the second motor should be. Once calculated, a move command is sent to the second motor.This code only runs when wire transmission occurs. With this method, the second link is completely dependent upon a signal being sent from the first Arduino. Dynamic Motor Motion (DMM) provides factory code for DYN232M Serial Communication Protocol between the DYN4 Servo Drive and the Arduino MEGA. This part of the code has remained unaltered.CalculationsAll calculations done within the code were double checked by hand to ensure that the correct results were being obtained. All calculations involved use the aforementioned equations. These same equations were then formatted to be compatible within the Arduino code. ImplementationTests were run with just the two Arduino MEGA’s before power was turned on for the 2 DOF Robotic Arm. This was done to make sure that the code would operate properly before attempting to move the links. After angle values were calculated and printed through the serial monitor, the 2 DOF Robotic Arm was manipulated manually to determine if all of the calculated angles could be reached without incurring damage to the user or 2 DOF Robotic Arm. Once it was determined that these angles could be reached safely, power was supplied to the servo drives and motors. SafetyThe DYN4 Servo Drives have a factory setting that resets the Max Speed and Max Acceleration parameters each time power is cycled. In order to ensure the safety of the user and 2 DOF Robotic Arm, the DYN4 Servo Drives are checked with DMM Tuning Program and cable to ensure that Max Speed and Max Acceleration parameters have been set to safe operating range.Other safety precautions include:The full length of the 2 DOF Robotic Arm is approximately 44 inches. In order to protect the arm in the event of power failure, the base of the system is approximately 48 inches above the ground.To protect the user it is recommended that the 2 DOF Robotic Arm be controlled from outside of the radius of the full extension of the links.All Servo Drives and Arduino MEGA’s have been properly grounded, and no alterations to wires are made with power being supplied to the system.Use of if statement to set angle parameters for both motors. If either motor cannot or should not rotate to a calculated angle – no move commands or wire transmission takes place. The code prints a statement showing the user that the angles cannot be safely reached and asks for new input.Trajectory PlanningFor trajectory planning and profiling, three predetermined functions were used. These functions included a line, parabola, and circle. The following describes how each function operates within the code:LineTake user input for x and set y equal to this.Take user input for final value of xIncrements x value by 0.1 and stops after reaching value equivalent to final value of x. This small increment allows for smoother motion. ParabolaTake user input for x and set y equal to one-tenth multiplied by x squared.Take user input for final value of xThe value for y is scaled down by one-tenth in order for all of the calculated positions to be obtainableThe x value is incremented by 0.1 and stops after reaching value equivalent to final value of x.CircleTake user input for center and radius and calculates angles and move positions for top half of circle, then calculates angles and move positions for bottom half.In order to check the trajectory to ensure accurate paths, an end-effector was fastened to the end of Link 2 that would be able to trace the path upon a board. The end-effector was a black marker, and the board used was a simple tri-fold poster board.Future WorkIf this project is continued in a subsequent semester the following can be worked and/or improved upon from initial design: Redesign of teach pendant. Previous project had code developed for menu in teach pendant. Can develop further to allow user to scroll through and select functions or switch between functions and analog control. Codes for menu and joystick control are included after main codesCoding updates. Using smaller servos to control end-effector on end of link 2. Redesign arm to not have any limits on reachable angle for link 2. References Code for Arduino/Motor 1/* Zachary Guy 5/9/19 2 DOF Robotic Arm Master Code Dr. M. Saadeh - Adivsor DYN232M_Arduino_SourceCode.ino - DMM Technology Corp. DYN2 and DYN4 AC Servo drive Arduino UART RS232 communication code. Version 1.0 - Release May 2018 Distributed By: DMM Technology Corp. dmm- Download DMMDRV Software for setting and checking Servo Drive Parameters. Servo Drive settings may reset to factory settings and ignore parameters introduced by Arduino MEGA upon each power up. For safety, check these settings upon each intial use of this 2 DOF Robotic Arm. DMMDRV base program direct executable 2018 Ver. FE473 Distributed By: DMM Technology Corp. */ //////////////////////////////////////////////////////////////////////////////////////#define Go_Absolute_Pos 0x01#define Go_Relative_Pos 0x03#define Is_AbsPos32 0x1b#define General_Read 0x0e #define Is_TrqCurrent 0x1E#define Read_MainGain 0x18#define Set_MainGain 0x10#define Set_SpeedGain 0x11#define Set_IntGain 0x12#define Set_HighSpeed 0x14#define Set_HighAccel 0x15#define Set_Pos_OnRange 0x16#define Is_MainGain 0x10#define Is_SpeedGain 0x11#define Is_IntGain 0x12#define Is_TrqCons 0x13#define Is_HighSpeed 0x14#define Is_HighAccel 0x15#define Is_Driver_ID 0x16#define Is_Pos_OnRange 0x17#define Is_Status 0x19#define Is_Config 0x1a/*Variables*/char InputBuffer[256]; //Input buffer from RS232,char OutputBuffer[256]; //Output buffer to RS232,unsigned char InBfTopPointer, InBfBtmPointer; //input buffer pointersunsigned char OutBfTopPointer, OutBfBtmPointer; //output buffer pointersunsigned char Read_Package_Buffer[8], Read_Num, Read_Package_Length, Global_Func;unsigned char MotorPosition32Ready_Flag, MotorTorqueCurrentReady_Flag, MainGainRead_Flag;unsigned char Driver_MainGain, Driver_SpeedGain, Driver_IntGain, Driver_TrqCons, Driver_HighSpeed, Driver_HighAccel,Driver_ReadID,Driver_Status,Driver_Config,Driver_OnRange;long Motor_Pos32, MotorTorqueCurrent;/*Prototypes*/void move_rel32(char ID, long pos);void ReadMotorTorqueCurrent(char ID);void ReadMotorPosition32(char ID);void move_abs32(char MotorID, long Pos32);void Turn_const_speed(char ID, long spd);void ReadPackage(void);void Get_Function(void);long Cal_SignValue(unsigned char One_Package[8]);long Cal_Value(unsigned char One_Package[8]);void Send_Package(char ID , long Displacement);void Make_CRC_Send(unsigned char Plength, unsigned char B[8]);/**/////////////////////////////////////////////////////////////////////////////////////////int incomingByte = 0;// Define Arm Constraintsconst float Link_1 = 22.5; //Length of link attached to Motor 1 (inches)const float Link_2 = 20.5; //Length of link attached to Motor 2 (inches)const float radius = 43.0; //Total length of 2DOF Arm (inches)// Angle Variablesfloat alpha; // Angle whose cosine is equivalent to Link_1 / Dfloat beta_deg; // Angle opposite Dfloat beta_rad; // To solve for correct angles in Arduino in radians firstfloat calc_angle1; // Angle that Link_1 moves with respect to x-axisfloat calc_angle2; // Angle that Link_2 moves with respect to Link_1-axislong real_angle1_actual; // Adjusting for motor positionfloat prev_angle1 = 0;float prev_angle2 = 0;float real_angle1 = 0;float real_angle2 = 0;//float D;float x_input = 0;float x_end = 0;const float pi = M_PI;#include <Wire.h>////////////////////////////////////////////////////////////////////////////////////void setup() { Wire.begin(); // join I2C bus (address optional for master) Serial.begin(38400); // Write to Serial Monitor Serial3.begin(38400); // Communicate with Servo Drive //Initial Settings for Servo Motor 1 Global_Func = (char)Set_MainGain; Send_Package(1, 20); Global_Func = (char)Set_SpeedGain; Send_Package(1, 20); Global_Func = (char)Set_IntGain; Send_Package(1, 1); Global_Func = (char)Set_HighSpeed; Send_Package(1, 1); Global_Func = (char)Set_HighAccel; Send_Package(1, 1); Global_Func = (char)Set_Pos_OnRange; Send_Package(1, 127); Serial.println("Select a Function: A, B, or C");}byte a = 0;byte b = 0;byte c = 0;byte d = 0;////////////////////////////////////////////////////////////////////////////////////void loop() { while (Serial.available() > 0) { delay(1000); incomingByte = Serial.read(); if (incomingByte == 65 || incomingByte == 97) { //Entering A or a Serial.println("You will run this function: "); Serial.println("y = x"); //Run func_1 Line(); } else if (incomingByte == 66 || incomingByte == 98) { //Entering B or b Serial.println("You will run this function: "); Serial.println("y = (1/10)*x^2"); //Run func_2 Parabola(); } else if (incomingByte == 67 || incomingByte == 99) { //Entering C or c Serial.println("You will run this function: "); Serial.println("(x-h)^2 + (y-k)^2 = r^2"); //Run func_3 Circle(); } else { //Entering anything else Serial.println("Select a Function: A, B, or C"); }}}////////////////////////////////////////////////////////////////////////////////////* For safety and testing purposes - ALWAYS check angles manually before powering up the 2 DOF Robotic Arm. Also, the linear line that has been tested and found to work is a line starting at y=x @ x = 23 Safety Parameters have been set to limit angular motion of arms. If any angle is out of reach, no signal will be sent to first servo drive to move Link 1, and no wire transmission will take place to move Link 2. */void Line(){ float x_begin = 0; float x_final = 0; Serial.println("Enter starting x value"); x_begin = Serial.parseFloat(); delay(1000); Serial.println("Enter ending x value"); x_final = Serial.parseFloat(); delay(1000); Serial.print("Starting x value: ");Serial.println(x_begin,DEC); Serial.print("Ending x value: ");Serial.println(x_final,DEC); //Moving to initial position float y_initial = x_begin; CalculateD(x_begin,y_initial); CalculateServoAngles(x_begin,y_initial); move_rel32(1, real_angle1_actual); delay(25); //Sending initial position signal to Arduino 2 Wire.beginTransmission(8); Wire.write(a); Wire.write(b); Wire.write(c); Wire.write(d); Wire.endTransmission(); delay(1000); prev_angle1 = calc_angle1; prev_angle2 = calc_angle2; delay(2000); for (float x = x_begin; x <= x_final; x=x+0.1) { float y = x; CalculateD(x,y); delay(50); CalculateServoAngles(x,y); delay(50); if (calc_angle1 >= 0 && calc_angle1 <= 180 && calc_angle2 >= -127 && calc_angle2 <= 127) { move_rel32(1, real_angle1_actual); delay(25); ////////////Transmission to Arduino 2/////////// Wire.beginTransmission(8); Wire.write(a); Wire.write(b); Wire.write(c); Wire.write(d); Wire.endTransmission(); prev_angle1 = calc_angle1; prev_angle2 = calc_angle2; } else { Serial.println("These angles cannot be safely reached"); }}}//////////////////////////////////////////////////////////////////////////////////////* For safety and testing purposes - ALWAYS check angles manually before powering up the 2 DOF Robotic Arm. ********************************************************************************* USE X_INPUT = 21 AND X_END = 23. THESE VALUES ARE SAFE FOR THE ARM TO REACH. ********************************************************************************* Safety Parameters have been set to limit angular motion of arms. If any angle is out of reach, no signal will be sent to first servo drive to move Link 1, and no wire transmission will take place to move Link 2. */void Parabola(){ Serial.println("Enter x value"); x_input = Serial.parseFloat(); delay(1000); Serial.print("x=");Serial.println(x_input,DEC); Serial.println("Enter ending x value"); x_end = Serial.parseFloat(); delay(1000); Serial.print("x=");Serial.println(x_end,DEC); float y_int = (1/10)*sq(x_input); CalculateD(x_input,y_int); CalculateServoAngles(x_input,y_int); move_rel32(1, real_angle1_actual); delay(25); ////////////Transmission to Arduino 2/////////// Wire.beginTransmission(8); Wire.write(a); Wire.write(b); Wire.write(c); Wire.write(d); Wire.endTransmission(); //////////////////////////////////////////////// prev_angle1 = calc_angle1; prev_angle2 = calc_angle2; delay(1000); for (float x = x_input+0.1; x <= x_end; x=x+0.1) { float y = (1/10)*sq(x); CalculateD(x,y); delay(50); CalculateServoAngles(x,y); delay(50);if (calc_angle1 >= -90 && calc_angle1 <= 180 && calc_angle2 >= -127 && calc_angle2 <= 127) { ////////////Moving Motor 1////////////////////// move_rel32(1, real_angle1_actual); delay(25); //////////////////////////////////////////////// ////////////Transmission to Arduino 2/////////// Wire.beginTransmission(8); Wire.write(a); Wire.write(b); Wire.write(c); Wire.write(d); Wire.endTransmission(); //////////////////////////////////////////////// prev_angle1 = calc_angle1; prev_angle2 = calc_angle2;} else { Serial.println("These angles cannot be safely reached"); }}}//////////////////////////////////////////////////////////////////////////////////////* For safety and testing purposes - ALWAYS check angles manually before powering up the 2 DOF Robotic Arm. Also, the circle that has been tested and found to work is a circle with a center at (23,23) and a radius = 2. Safety Parameters have been set to limit angular motion of arms. If any angle is out of reach, no signal will be sent to first servo drive to move Link 1, and no wire transmission will take place to move Link 2. */void Circle(){ Serial.println("Enter center of circle (h,k): "); delay(1000); float h = Serial.parseFloat(); delay(1000); float k = Serial.parseFloat(); delay(1000); Serial.println("Enter the radius of circle (r): "); delay(1000); float r = Serial.parseFloat(); delay(1000); Serial.print("You want to create a circle of radius: "); Serial.println(r); Serial.print("With a center located at: ("); Serial.print(h); Serial.print(","); Serial.print(k); Serial.println(")"); float xStart = h - r; float xFinish = h + r; float yStart = k; float y = 0.0; float r_sq = sq(r); CalculateD(xStart,yStart); delay(50); CalculateServoAngles(xStart,yStart); delay(50); move_rel32(1, real_angle1_actual); delay(25); ////////////Transmission to Arduino 2/////////// Wire.beginTransmission(8); Wire.write(a); Wire.write(b); Wire.write(c); Wire.write(d); Wire.endTransmission(); delay(500); //////////////////////////////////////////////// prev_angle1 = calc_angle1; prev_angle2 = calc_angle2; for (float x = xStart; x <= xFinish; x=x+0.1) { y = k + sqrt(r_sq -(sq(x-h))); CalculateD(x,y); delay(50); CalculateServoAngles(x,y); delay(50);if (calc_angle1 >= 0 && calc_angle1 <= 180 && calc_angle2 >= -127 && calc_angle2 <= 127) { ////////////Moving Motor 1////////////////////// move_rel32(1, real_angle1_actual); delay(25); //////////////////////////////////////////////// ////////////Transmission to Arduino 2/////////// Wire.beginTransmission(8); Wire.write(a); Wire.write(b); Wire.write(c); Wire.write(d); Wire.endTransmission(); //////////////////////////////////////////////// prev_angle1 = calc_angle1; prev_angle2 = calc_angle2; } else { Serial.println("These angles cannot be safely reached"); } } delay(50); for (float x = xFinish; x >= xStart; x=x-0.1) { y = k - sqrt(r_sq -(sq(x-h))); CalculateD(x,y); delay(50); CalculateServoAngles(x,y); delay(50);if (calc_angle1 >= 0 && calc_angle1 <= 180 && calc_angle2 >= -127 && calc_angle2 <= 127) { ////////////Moving Motor 1////////////////////// move_rel32(1, real_angle1_actual); delay(25); //////////////////////////////////////////////// ////////////Transmission to Arduino 2/////////// Wire.beginTransmission(8); Wire.write(a); Wire.write(b); Wire.write(c); Wire.write(d); Wire.endTransmission(); //////////////////////////////////////////////// prev_angle1 = calc_angle1; prev_angle2 = calc_angle2;} else { Serial.println("These angles cannot be safely reached"); }} Serial.println("The circle is complete."); }///////////////////////////////////////////////////////////////////////////////////void CalculateD(float x, float y) { D = sqrt(sq(x)+sq(y));}///////////////////////////////////////////////////////////////////////////////////void CalculateServoAngles(float x, float y) { beta_deg = ((acos((sq(Link_1)+sq(Link_2)-sq(D))/(2*Link_1*Link_2)))*(180/pi)); beta_rad = (acos((sq(Link_1)+sq(Link_2)-sq(D))/(2*Link_1*Link_2))); alpha = ((asin((Link_2*sin(beta_rad))/D))*(180/pi)); calc_angle1 = (((atan(y/x))*(180/pi)) - alpha); calc_angle2 = (180 - beta_deg); real_angle1 = calc_angle1 - prev_angle1; real_angle2 = calc_angle2 - prev_angle2; //For Wire Transmission if (real_angle2 < 1.0 && real_angle2 > -1.0) { a = ((real_angle2 * 100) + 127); c = 1; } else { a = real_angle2 + 127; c = 2; } if (real_angle1 < 1.0 && real_angle1 > -1.0) { b = ((real_angle1*100) + 80); d = 1; } else { b = real_angle1 + 80; d = 2; } //For sending correct motor command real_angle1_actual = (real_angle1 * (65536/360) * (1)); //change from -1 to 1 to change direction of rotation}///////////////////////////////////////////////////////////////////////////////////void move_rel32(char ID, long pos) { char Axis_Num = ID; Global_Func = (char)Go_Relative_Pos; Send_Package(Axis_Num, pos);}//////////////////////////////////////////////////////////////////////DYN232M SERIAL PROTOCOL PACKAGE FUNCTIONS///// /////Do NOT modify//////* Everything below this line is for the serial communication between the Arduino MEGA and the DYN4 Servo Drive */void ReadPackage(void) { unsigned char c, cif; while (Serial3.available() > 0) { //Modifying to serial_3 InputBuffer[InBfTopPointer] = Serial3.read(); //Load InputBuffer with received packets InBfTopPointer++; } while (InBfBtmPointer != InBfTopPointer) { c = InputBuffer[InBfBtmPointer]; InBfBtmPointer++; cif = c & 0x80; if (cif == 0) { Read_Num = 0; Read_Package_Length = 0; } if (cif == 0 || Read_Num > 0) { Read_Package_Buffer[Read_Num] = c; Read_Num++; if (Read_Num == 2) { cif = c >> 5; cif = cif & 0x03; Read_Package_Length = 4 + cif; c = 0; } if (Read_Num == Read_Package_Length) { Get_Function(); Read_Num = 0; Read_Package_Length = 0; } } }}void Get_Function(void) { char ID, ReceivedFunction_Code, CRC_Check; long Temp32; ID = Read_Package_Buffer[0] & 0x7f; ReceivedFunction_Code = Read_Package_Buffer[1] & 0x1f; CRC_Check = 0; for (int i = 0; i < Read_Package_Length - 1; i++) { CRC_Check += Read_Package_Buffer[i]; } CRC_Check ^= Read_Package_Buffer[Read_Package_Length - 1]; CRC_Check &= 0x7f; if (CRC_Check != 0) { } else { switch (ReceivedFunction_Code) { case Is_AbsPos32: Motor_Pos32 = Cal_SignValue(Read_Package_Buffer); MotorPosition32Ready_Flag = 0x00; break; case Is_TrqCurrent: MotorTorqueCurrent = Cal_SignValue(Read_Package_Buffer); break; case Is_Status: Driver_Status = (char)Cal_SignValue(Read_Package_Buffer); // Driver_Status=drive status byte data break; case Is_Config: Temp32 = Cal_Value(Read_Package_Buffer); //Driver_Config = drive configuration setting break; case Is_MainGain: Driver_MainGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_MainGain = Driver_MainGain & 0x7f; break; case Is_SpeedGain: Driver_SpeedGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_SpeedGain = Driver_SpeedGain & 0x7f; break; case Is_IntGain: Driver_IntGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_IntGain = Driver_IntGain & 0x7f; break; case Is_TrqCons: Driver_TrqCons = (char)Cal_SignValue(Read_Package_Buffer); Driver_TrqCons = Driver_TrqCons & 0x7f; break; case Is_HighSpeed: Driver_HighSpeed = (char)Cal_SignValue(Read_Package_Buffer); Driver_HighSpeed = Driver_HighSpeed & 0x7f; break; case Is_HighAccel: Driver_HighAccel = (char)Cal_SignValue(Read_Package_Buffer); Driver_HighAccel = Driver_HighAccel & 0x7f; break; case Is_Driver_ID: Driver_ReadID = ID; break; case Is_Pos_OnRange: Driver_OnRange = (char)Cal_SignValue(Read_Package_Buffer); Driver_OnRange = Driver_OnRange & 0x7f; break; } }}long Cal_SignValue(unsigned char One_Package[8]) //Get data with sign - long{ char Package_Length,OneChar,i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar>>5; OneChar = OneChar&0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 reprents sign */ OneChar = OneChar << 1; Lcmd = (long)OneChar; /* Sign extended to 32bits */ Lcmd = Lcmd >> 1; for(i=3;i<Package_Length-1;i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd<<7; Lcmd += OneChar; } return(Lcmd); /* Lcmd : -2^27 ~ 2^27 - 1 */}long Cal_Value(unsigned char One_Package[8]){ char Package_Length,OneChar,i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar>>5; OneChar = OneChar&0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 reprents sign */ OneChar &= 0x7f; Lcmd = (long)OneChar; /*Sign extended to 32bits */ for(i=3;i<Package_Length-1;i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd<<7; Lcmd += OneChar; } return(Lcmd); /*Lcmd : -2^27 ~ 2^27 - 1 */}void Send_Package(char ID , long Displacement) { unsigned char B[8], Package_Length, Function_Code; long TempLong; B[1] = B[2] = B[3] = B[4] = B[5] = (unsigned char)0x80; B[0] = ID & 0x7f; Function_Code = Global_Func & 0x1f; TempLong = Displacement & 0x0fffffff; //Max 28bits B[5] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[4] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[3] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[2] += (unsigned char)TempLong & 0x0000007f; Package_Length = 7; TempLong = Displacement; TempLong = TempLong >> 20; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Three byte data B[2] = B[3]; B[3] = B[4]; B[4] = B[5]; Package_Length = 6; } TempLong = Displacement; TempLong = TempLong >> 13; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Two byte data B[2] = B[3]; B[3] = B[4]; Package_Length = 5; } TempLong = Displacement; TempLong = TempLong >> 6; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //One byte data B[2] = B[3]; Package_Length = 4; } B[1] += (Package_Length - 4) * 32 + Function_Code; Make_CRC_Send(Package_Length, B);}void Make_CRC_Send(unsigned char Plength, unsigned char B[8]) { unsigned char Error_Check = 0; char RS232_HardwareShiftRegister; for (int i = 0; i < Plength - 1; i++) { OutputBuffer[OutBfTopPointer] = B[i]; OutBfTopPointer++; Error_Check += B[i]; } Error_Check = Error_Check | 0x80; OutputBuffer[OutBfTopPointer] = Error_Check; OutBfTopPointer++; while (OutBfBtmPointer != OutBfTopPointer) { RS232_HardwareShiftRegister = OutputBuffer[OutBfBtmPointer]; //Serial.print("RS232_HardwareShiftRegister: "); //Serial.println(RS232_HardwareShiftRegister, DEC); Serial3.write(RS232_HardwareShiftRegister); //modifying to serial_3 OutBfBtmPointer++; // Change to next byte in OutputBuffer to send }}Final Code for Arduino/Motor 2/* Zachary Guy 5/9/19 2 DOF Robotic Arm Slave Code Dr. M. Saadeh - Adivsor DYN232M_Arduino_SourceCode.ino - DMM Technology Corp. DYN2 and DYN4 AC Servo drive Arduino UART RS232 communication code. Version 1.0 - Release May 2018 Distributed By: DMM Technology Corp. dmm- Download DMMDRV Software for setting and checking Servo Drive Parameters. Servo Drive settings may reset to factory settings and ignore parameters introduced by Arduino MEGA upon each power up. For safety, check these settings upon each intial use of this 2 DOF Robotic Arm. DMMDRV base program direct executable 2018 Ver. FE473 Distributed By: DMM Technology Corp. *//////////////////////////////////////////////////////////////////#define Go_Absolute_Pos 0x01#define Go_Relative_Pos 0x03#define Is_AbsPos32 0x1b#define General_Read 0x0e #define Is_TrqCurrent 0x1E#define Read_MainGain 0x18#define Set_MainGain 0x10#define Set_SpeedGain 0x11#define Set_IntGain 0x12#define Set_HighSpeed 0x14#define Set_HighAccel 0x15#define Set_Pos_OnRange 0x16#define Is_MainGain 0x10#define Is_SpeedGain 0x11#define Is_IntGain 0x12#define Is_TrqCons 0x13#define Is_HighSpeed 0x14#define Is_HighAccel 0x15#define Is_Driver_ID 0x16#define Is_Pos_OnRange 0x17#define Is_Status 0x19#define Is_Config 0x1a/*Variables*/char InputBuffer[256]; //Input buffer from RS232,char OutputBuffer[256]; //Output buffer to RS232,unsigned char InBfTopPointer, InBfBtmPointer; //input buffer pointersunsigned char OutBfTopPointer, OutBfBtmPointer; //output buffer pointersunsigned char Read_Package_Buffer[8], Read_Num, Read_Package_Length, Global_Func;unsigned char MotorPosition32Ready_Flag, MotorTorqueCurrentReady_Flag, MainGainRead_Flag;unsigned char Driver_MainGain, Driver_SpeedGain, Driver_IntGain, Driver_TrqCons, Driver_HighSpeed, Driver_HighAccel,Driver_ReadID,Driver_Status,Driver_Config,Driver_OnRange;long Motor_Pos32, MotorTorqueCurrent;/*Prototypes*/void move_rel32(char ID, long pos);void ReadMotorTorqueCurrent(char ID);void ReadMotorPosition32(char ID);void move_abs32(char MotorID, long Pos32);void Turn_const_speed(char ID, long spd);void ReadPackage(void);void Get_Function(void);long Cal_SignValue(unsigned char One_Package[8]);long Cal_Value(unsigned char One_Package[8]);void Send_Package(char ID , long Displacement);void Make_CRC_Send(unsigned char Plength, unsigned char B[8]);/**//////////////////////////////////////////////////////////////////#include <Wire.h>float angle;float theta_1;long theta_2_actual; // Adjusting for motor position//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////void setup() { Wire.begin(8); Wire.onReceive(receiveEvent); Serial.begin(38400); Serial3.begin(38400); // Communicating with Servo Drive //Initial Settings for Servo Motor 2 Global_Func = (char)Set_MainGain; Send_Package(2, 20); Global_Func = (char)Set_SpeedGain; Send_Package(2, 20); Global_Func = (char)Set_IntGain; Send_Package(2, 1); Global_Func = (char)Set_HighSpeed; Send_Package(2, 1); Global_Func = (char)Set_HighAccel; Send_Package(2, 1); Global_Func = (char)Set_Pos_OnRange; Send_Package(2, 127);}//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////void loop() { delay(100);}//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////void receiveEvent(int howMany) { int a = Wire.read(); int b = Wire.read(); int c = Wire.read(); int d = Wire.read(); if (c == 1) { angle = ((a-127.0)/100.0); } else if (c == 2) { angle = a - 127.0; } if (d == 1) { theta_1 = ((b-80.0)/100.0); } else if (d == 2) { theta_1 = b - 80.0; } Serial.print("Motor 2 Angle: "); Serial.println(angle); Serial.print("Motor 2 Angle will actually move: "); Serial.println((theta_1 + angle)); theta_2_actual = ((theta_1 + angle) * (65536/360) * (-1)); //multiply actual by -1 to change direction of rotation move_rel32(2, theta_2_actual); delay(25);}/////////////////////////////////////////////////////////////////void move_rel32(char ID, long pos) { char Axis_Num = ID; Global_Func = (char)Go_Relative_Pos; Send_Package(Axis_Num, pos);}///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////DYN232M SERIAL PROTOCOL PACKAGE FUNCTIONS///// /////Do NOT modify//////* Everything below this line is for the serial communication between the Arduino MEGA and the DYN4 Servo Drive */void ReadPackage(void) { unsigned char c, cif; while (Serial3.available() > 0) { //Modifying to serial_3 InputBuffer[InBfTopPointer] = Serial3.read(); //Load InputBuffer with received packets InBfTopPointer++; } while (InBfBtmPointer != InBfTopPointer) { c = InputBuffer[InBfBtmPointer]; InBfBtmPointer++; cif = c & 0x80; if (cif == 0) { Read_Num = 0; Read_Package_Length = 0; } if (cif == 0 || Read_Num > 0) { Read_Package_Buffer[Read_Num] = c; Read_Num++; if (Read_Num == 2) { cif = c >> 5; cif = cif & 0x03; Read_Package_Length = 4 + cif; c = 0; } if (Read_Num == Read_Package_Length) { Get_Function(); Read_Num = 0; Read_Package_Length = 0; } } }}void Get_Function(void) { char ID, ReceivedFunction_Code, CRC_Check; long Temp32; ID = Read_Package_Buffer[0] & 0x7f; ReceivedFunction_Code = Read_Package_Buffer[1] & 0x1f; CRC_Check = 0; for (int i = 0; i < Read_Package_Length - 1; i++) { CRC_Check += Read_Package_Buffer[i]; } CRC_Check ^= Read_Package_Buffer[Read_Package_Length - 1]; CRC_Check &= 0x7f; if (CRC_Check != 0) { } else { switch (ReceivedFunction_Code) { case Is_AbsPos32: Motor_Pos32 = Cal_SignValue(Read_Package_Buffer); MotorPosition32Ready_Flag = 0x00; break; case Is_TrqCurrent: MotorTorqueCurrent = Cal_SignValue(Read_Package_Buffer); break; case Is_Status: Driver_Status = (char)Cal_SignValue(Read_Package_Buffer); // Driver_Status=drive status byte data break; case Is_Config: Temp32 = Cal_Value(Read_Package_Buffer); //Driver_Config = drive configuration setting break; case Is_MainGain: Driver_MainGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_MainGain = Driver_MainGain & 0x7f; break; case Is_SpeedGain: Driver_SpeedGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_SpeedGain = Driver_SpeedGain & 0x7f; break; case Is_IntGain: Driver_IntGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_IntGain = Driver_IntGain & 0x7f; break; case Is_TrqCons: Driver_TrqCons = (char)Cal_SignValue(Read_Package_Buffer); Driver_TrqCons = Driver_TrqCons & 0x7f; break; case Is_HighSpeed: Driver_HighSpeed = (char)Cal_SignValue(Read_Package_Buffer); Driver_HighSpeed = Driver_HighSpeed & 0x7f; break; case Is_HighAccel: Driver_HighAccel = (char)Cal_SignValue(Read_Package_Buffer); Driver_HighAccel = Driver_HighAccel & 0x7f; break; case Is_Driver_ID: Driver_ReadID = ID; break; case Is_Pos_OnRange: Driver_OnRange = (char)Cal_SignValue(Read_Package_Buffer); Driver_OnRange = Driver_OnRange & 0x7f; break; } }}long Cal_SignValue(unsigned char One_Package[8]) //Get data with sign - long{ char Package_Length,OneChar,i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar>>5; OneChar = OneChar&0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 reprents sign */ OneChar = OneChar << 1; Lcmd = (long)OneChar; /* Sign extended to 32bits */ Lcmd = Lcmd >> 1; for(i=3;i<Package_Length-1;i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd<<7; Lcmd += OneChar; } return(Lcmd); /* Lcmd : -2^27 ~ 2^27 - 1 */}long Cal_Value(unsigned char One_Package[8]){ char Package_Length,OneChar,i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar>>5; OneChar = OneChar&0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 reprents sign */ OneChar &= 0x7f; Lcmd = (long)OneChar; /*Sign extended to 32bits */ for(i=3;i<Package_Length-1;i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd<<7; Lcmd += OneChar; } return(Lcmd); /*Lcmd : -2^27 ~ 2^27 - 1 */}void Send_Package(char ID , long Displacement) { unsigned char B[8], Package_Length, Function_Code; long TempLong; B[1] = B[2] = B[3] = B[4] = B[5] = (unsigned char)0x80; B[0] = ID & 0x7f; Function_Code = Global_Func & 0x1f; TempLong = Displacement & 0x0fffffff; //Max 28bits B[5] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[4] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[3] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[2] += (unsigned char)TempLong & 0x0000007f; Package_Length = 7; TempLong = Displacement; TempLong = TempLong >> 20; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Three byte data B[2] = B[3]; B[3] = B[4]; B[4] = B[5]; Package_Length = 6; } TempLong = Displacement; TempLong = TempLong >> 13; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Two byte data B[2] = B[3]; B[3] = B[4]; Package_Length = 5; } TempLong = Displacement; TempLong = TempLong >> 6; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //One byte data B[2] = B[3]; Package_Length = 4; } B[1] += (Package_Length - 4) * 32 + Function_Code; Make_CRC_Send(Package_Length, B);}void Make_CRC_Send(unsigned char Plength, unsigned char B[8]) { unsigned char Error_Check = 0; char RS232_HardwareShiftRegister; for (int i = 0; i < Plength - 1; i++) { OutputBuffer[OutBfTopPointer] = B[i]; OutBfTopPointer++; Error_Check += B[i]; } Error_Check = Error_Check | 0x80; OutputBuffer[OutBfTopPointer] = Error_Check; OutBfTopPointer++; while (OutBfBtmPointer != OutBfTopPointer) { RS232_HardwareShiftRegister = OutputBuffer[OutBfBtmPointer]; //Serial.print("RS232_HardwareShiftRegister: "); //Serial.println(RS232_HardwareShiftRegister, DEC); Serial3.write(RS232_HardwareShiftRegister); //modifying to serial_3 OutBfBtmPointer++; // Change to next byte in OutputBuffer to send }}Code Created by Former Student – MENU SETUP INCLUDED/////////////////////////////* DMM Tech Provided Code */////////////////////////////#define Go_Absolute_Pos 1 //0x01#define Go_Relative_Pos 3 //0x03#define Is_AbsPos32 27 //0x1b#define General_Read 14 //0x0e#define Is_TrqCurrent 0x1E //0x1e#define Read_MainGain 24 //0x18#define Set_MainGain 0x10#define Set_SpeedGain 0x11#define Set_IntGain 0x12#define Set_HighSpeed 0x14#define Set_HighAccel 0x15#define Set_Pos_OnRange 0x16char InputBuffer[256]; //Input buffer from RS232char OutputBuffer [256]; //Output buffer to RS232unsigned char InBfTopPointer, InBfBtmPointer; //input buffer pointersunsigned char OutBfTopPointer, OutBfBtmPointer; //output buffer pointersunsigned char Read_Package_Buffer[8], Read_Num, Read_Package_Length, Global_Func;unsigned char MotorPosition32Ready_Flag, MotorTorqueCurrentReady_Flag, MainGainRead_Flag;long Motor_Pos32, MotorTorqueCurrent;int MainGain_Read;///////////////////////////* End of DMM Tech Code *///////////////////////////#define Turn_ConstSpeed 0x0a#define Set_GearNumber 0x17#define Set_Origin 0x00#include <Button.h>#include <LiquidCrystal_I2C.h>Button upButton = Button(6, PULLUP);Button downButton = Button(5, PULLUP);Button leftButton = Button(4, PULLUP);Button rightButton = Button(3, PULLUP);Button selectButton = Button(2, PULLUP);int lastButton = 0;int topMenuCount, posMenuCount, angleMenuCount, radianMenuCount, speedMenuCount;int exitAction, posAction, angleAction, radianAction, motorAction, speedAction, constSpeedAction;int rpmAction, accAction, setAngleAction, setRadianAction;double rpmCount, accCount;double angle, setAngleCount;double rad, setRadianCount;double rpmValue, accValue;int jsUD, rpm;double gear_Ratio = 0.25;LiquidCrystal_I2C lcd(0x20, 16, 2);void setup() { Serial.begin(9600); Serial3.begin(38400); lcd.init(); lcd.backlight(); lcd.clear();///////////////////////////////////////* Initial Settings for Servo Motor *///////////////////////////////////////Global_Func = (char)Set_MainGain;Send_Package(0, 40);Global_Func = (char)Set_SpeedGain;Send_Package(0, 40);Global_Func = (char)Set_IntGain;Send_Package(0, 1);Global_Func = (char)Set_HighSpeed;Send_Package(0, 85);Global_Func = (char)Set_HighAccel;Send_Package(0, 20);Global_Func = (char)Set_Pos_OnRange;Send_Package(0, 10);Global_Func = (char)Set_GearNumber;Send_Package(0, 16384);}void loop() { checkButton(); topMenu();}///////////////* Top Menu *///////////////void topMenu() { topMenuCount = navButton(lastButton, topMenuCount); switch (topMenuCount) { case -1: topMenuCount = 2; break; case 0: lcd.setCursor(0, 0); lcd.print("Position Servo"); lcd.setCursor(0, 1); lcd.print(""); posAction = selectCheck(); while (posAction == 1) { checkButton(); posMenu(); } break; case 1: lcd.setCursor(0, 0); lcd.print("Speed Servo"); lcd.setCursor(0, 1); lcd.print(""); speedAction = selectCheck(); while (speedAction == 1) { checkButton(); speedMenu(); } break; case 2: lcd.setCursor(0, 0); lcd.print("Torque Servo"); break; case 3: topMenuCount = 0; break; }}///////////////////* Postion Menu *///////////////////void posMenu() { posMenuCount = navButton(lastButton, posMenuCount); switch (posMenuCount) { case -1: posMenuCount = 2; break; case 0: lcd.setCursor(0, 0); lcd.print("Move to Angle"); angleAction = selectCheck(); while (angleAction == 1) { checkButton(); angleMenu(); } break; case 1: lcd.setCursor(0, 0); lcd.print("Move by Radians"); radianAction = selectCheck(); while (radianAction == 1) { checkButton(); radianMenu(); } break; case 2: lcd.setCursor(0, 0); lcd.print("Want to Exit?"); lcd.setCursor(0, 1); lcd.print("Press Select"); exitAction = selectCheck(); if (exitAction == 1) { posAction = 0; posMenuCount = 0; exitAction = 0; } break; case 3: posMenuCount = 0; break; }}/////////////////* Speed Menu */////////////////void speedMenu() { speedMenuCount = navButton(lastButton, speedMenuCount); switch (speedMenuCount) { case -1: speedMenuCount = 2; break; case 0: lcd.setCursor (0, 0); lcd.print("Constant Speed"); lcd.setCursor(0, 1); lcd.print("Press Select"); constSpeedAction = selectCheck(); while (constSpeedAction == 1) { checkButton(); setConstant(); } break; case 1: lcd.setCursor(0, 0); lcd.print("Analog"); lcd.setCursor(0, 1); lcd.print("Press Select"); break; case 2: lcd.setCursor(0, 0); lcd.print("Want to Exit?"); lcd.setCursor(0, 1); lcd.print("Press Select"); exitAction = selectCheck(); if (exitAction == 1) { speedAction = 0; speedMenuCount = 0; exitAction = 0; } break; case 3: speedMenuCount = 0; break; }}///////////////////////////////////////////////////* Postion Angle Menu and Position Radian Angle *///////////////////////////////////////////////////void angleMenu() { angleMenuCount = navButton(lastButton, angleMenuCount); switch (angleMenuCount) { case -1: angleMenuCount = 4; break; case 0: lcd.setCursor(0, 0); lcd.print("To Set RPM"); lcd.setCursor(0, 1); lcd.print("Press Select"); rpmAction = selectCheck(); while (rpmAction == 1) { checkButton(); setRPM(); } break; case 1: lcd.setCursor(0, 0); lcd.print("To Set ACC"); lcd.setCursor(0, 1); lcd.print("Press Select"); accAction = selectCheck(); while (accAction == 1) { checkButton(); setACC(); } break; case 2: lcd.setCursor(0, 0); lcd.print("To Set Angle"); lcd.setCursor(0, 1); lcd.print("Press Select"); setAngleAction = selectCheck(); while (setAngleAction == 1) { checkButton(); setAngle(); } break; case 3: lcd.setCursor(0, 0); lcd.print("Move Motor?"); lcd.setCursor(0, 1); lcd.print("Press Select"); motorAction = selectCheck(); while (motorAction == 1) { int id = 0; sendMotorAngle(id, angle); motorAction = 0; } break; case 4: lcd.setCursor(0, 0); lcd.print("Want to Exit?"); lcd.setCursor(0, 1); lcd.print("Press Select"); exitAction = selectCheck(); if (exitAction == 1) { angleAction = 0; angleMenuCount = 0; exitAction = 0; } break; case 5: angleMenuCount = 0; break; }}void radianMenu() { radianMenuCount = navButton(lastButton, radianMenuCount); switch (radianMenuCount) { case -1: radianMenuCount = 4; break; case 0: lcd.setCursor(0, 0); lcd.print("To Set RPM"); lcd.setCursor("Press Select"); rpmAction = selectCheck(); while (rpmAction == 1) { checkButton(); setRPM(); } break; case 1: lcd.setCursor(0, 0); lcd.print("To Set ACC"); lcd.setCursor(0, 1); lcd.print("Press Select"); accAction = selectCheck(); while (accAction == 1) { checkButton(); setACC(); } break; case 2: lcd.setCursor(0, 0); lcd.print("To Set Radians"); lcd.setCursor(0, 1); lcd.print("Press Select"); setRadianAction = selectCheck(); while (setRadianAction == 1) { checkButton(); setRadian(); } break; case 3: lcd.setCursor(0, 0); lcd.print("Move Motor?"); lcd.setCursor(0, 1); lcd.print("Press Select"); motorAction = selectCheck(); while (motorAction == 1) { int id = 0; sendMotorAngle(id, setRadianCount); motorAction = 0; } break; case 4: lcd.setCursor(0, 0); lcd.print("Want to Exit?"); lcd.setCursor(0, 1); lcd.print("Press Select"); exitAction = selectCheck(); if (exitAction == 1) { radianAction = 0; radianMenuCount = 0; exitAction = 0; } break; case 5: radianMenuCount = 0; break; }}////////////////////////////////* This is all button checks *////////////////////////////////void checkButton() { if (upButton.uniquePress()) { //Serial.println("Button Up"); lastButton = 1; } if (downButton.uniquePress()) { //Serial.println("Button Down"); lastButton = 2; } if (leftButton.uniquePress()) { //Serial.println("Button Left"); lastButton = 3; } if (rightButton.uniquePress()) { //Serial.println("Button Right"); lastButton = 4; } if (selectButton.uniquePress()) { //Serial.println("Select"); lastButton = 5; } if (lastButton > 0) { }}int navButton(int button, int count) { switch (button) { case 1: count++; lastButton = 0; lcd.clear(); break; case 2: count--; lastButton = 0; lcd.clear(); break; } return count;}int selectCheck() { int sel = 0; if (lastButton == 5) { sel = 1; lcd.clear(); } else { sel = 0; } lastButton = 0; return sel;}///////////////////////////////////////* This is where parameters are set *///////////////////////////////////////void setRPM() { rpmCount = navButton(lastButton, rpmCount); if (rpmCount == -1) { rpmCount = 85; } else if (rpmCount == 86) { rpmCount = 0; } rpmActual(); lcd.setCursor(0, 0); lcd.print("Set RPM"); lcd.setCursor(0, 1); lcd.print("RPM: "); lcd.print(rpmValue); lcd.print(" RPM"); exitAction = selectCheck(); if (exitAction == 1) { rpmAction = 0; exitAction = 0; Global_Func = (char)Set_HighSpeed; Send_Package(0, rpmCount); }}void setACC() { accCount = navButton(lastButton, accCount); if (accCount == -1) { accCount = 20; } else if (accCount == 21) { accCount = 0; } accActual(); lcd.setCursor(0, 0); lcd.print("Set Acceleration"); lcd.setCursor(0, 1); lcd.print(accValue); lcd.print(" RPM/s"); exitAction = selectCheck(); if (exitAction == 1) { accAction = 0; exitAction = 0; Global_Func = (char)Set_HighAccel; Send_Package(0, accCount); }}void setRadian() { setRadianCount = navButton(lastButton, setRadianCount); if (setRadianCount == -1) { setRadianCount = 10; } else if (setRadianCount == 11) { setRadianCount = 0; } lcd.setCursor(0, 0); lcd.print("Set Radians"); lcd.setCursor(0, 1); lcd.print(accValue); lcd.print("Radians "); exitAction = selectCheck(); if (exitAction == 1) { setRadianAction = 0; exitAction = 0; }}void setAngle() { setAngleCount = navButton(lastButton, setAngleCount); if (setAngleCount == -1) { setAngleCount = 360; } else if (setAngleCount == 361) { setAngleCount = 0; } lcd.setCursor(0, 0); lcd.print("Set Angle"); lcd.setCursor(0, 1); lcd.print("Angle: "); lcd.print(setAngleCount); angle = map(setAngleCount, -360, 360, -65536, 65536); exitAction = selectCheck(); if (exitAction == 1) { setAngleAction = 0; exitAction = 0; }}void setConstant() { lcd.setCursor(0, 0); lcd.print("Use JoyStick"); lcd.setCursor(0, 1); lcd.print("RPM"); rpmJSRead(); joyStickSpeed(); exitAction = selectCheck(); if (exitAction == 1) { constSpeedAction = 0; exitAction = 0; }}////////////////////////////////////* Actual values for Servo Motor *////////////////////////////////////void rpmActual() { rpmValue = ((sq(rpmCount + 3)) / 16) * 12.21 * gear_Ratio;}void accActual() { accValue = accCount * 635.78 * gear_Ratio;}////////////////////////////////////* Move Commands for Servo Motor *////////////////////////////////////void sendMotorRadians (int id, int disp) { Global_Func = (char)Go_Relative_Pos; Send_Package(id, disp * 65536);}void sendMotorAngle(int id, double disp) { Global_Func = (char)Go_Absolute_Pos; Send_Package(id, disp);}void sendMotorConstSpeed(int id, int rpm) { Global_Func = (char)Turn_ConstSpeed; Send_Package(id, rpm);}void setOrigin() { Global_Func = (char)Set_Origin; Send_Package (0, 0);}void joyStickSpeed() { Global_Func = (char)Turn_ConstSpeed; Send_Package(0, rpm);}void rpmJSRead() { jsUD = analogRead(A1); if (jsUD < 450) { rpm = map(jsUD, 0, 450, -1000, 0); } else if (jsUD > 550) { rpm = map(jsUD, 550, 1023, 0, 1000); } else { rpm = 0;}}/////////////////////////////////////////////////////////////* DMM-Tech Servo Functions do NOT change below this line */////////////////////////////////////////////////////////////void ReadPackage(void) { unsigned char c, cif; while (Serial3.available() > 0) { InputBuffer[InBfTopPointer] = Serial3.read(); //Load InputBuffer with recieved packets InBfTopPointer++; } while (InBfBtmPointer != InBfTopPointer) { c = InputBuffer[InBfBtmPointer]; InBfBtmPointer++; cif = c & 0x80 if (cif == 0) { Read_Num = 0; Read_Package_Length = 0; } if (cif == 0 || Read_Num > 0) { Read_Package_Buffer[Read_Num] = c; Read_Num++; if (Read_Num == 2) { cif = c >> 5; cif = cif & 0x03; Read_Package_Length = 4 + cif; c = 0; } if (Read_Num == Read_Package_Length) { Get_Function(); Read_Num = 0; Read_Package_Length = 0; } } }}void Get_Function(void) { char ID, ReceivedFunction_Code, CRC_Check; ID = Read_Package_Buffer[0] & 0x7f; ReceivedFunction_Code = Read_Package_Buffer[1] & 0x1f; CRC_Check = 0; for (int i = 0; i < Read_Package_Length -1; i++) { CRC_Check += Read_Package_Buffer[i]; } CRC_Check ^= Read_Package_Buffer[Read_Package_Length - 1]; CRC_Check &= 0x7f; if (CRC_Check != 0) {}else { switch (ReceivedFunction_Code) { case Is_AbsPos32: Motor_Pos32 = Cal_SignValue(Read_Package_Buffer); MotorPosition32Ready_Flag = 0x00; //Serial.println("flag cleared"); break; case Is_TrqCurrent: MotorTorqueCurrent = Cal_SignValue(Read_Package_Buffer); break; case Read_MainGain: MainGainRead_Flag = 0x00; break; }}}long Cal_SignValue(unsigned char One_Package[8]) //Get data with sign - long{ char Package_Length, OneChar, i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar >> 5; OneChar = OneChar & 0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 represents sign */ OneChar = OneChar << 1; Lcmd = (long)OneChar; /* Sign extended to 32 bits */ Lcmd = Lcmd >> 1; for (i = 3; i < Package_Length - 1; i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd << 7; Lcmd += OneChar; } return (Lcmd); /* Lcmd : -2^27 ~ 2^27 - 1 */}void Send_Package(char ID, long Displacement) { unsigned char B[8], Package_Length, Function_Code; long TempLong; B[1] = B[2] = B[3] = B[4] = B[5] = (unsigned char)0x80; B[0] = ID & 0x7f; Function_Code = Global_Func & 0x1f; TempLong = Displacement & 0x0fffffff; //Max 28bits B[5] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[4] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[3] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[2] += (unsigned char)TempLong & 0x0000007f; Package_Length = 7; TempLong = Displacement TempLong = TempLong >> 20; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Three byte data B[2] = B[3]; B[3] = B[4]; B[4] = B[5]; Package_Length = 6; } TempLong = Displacement TempLong = TempLong >> 13; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Two byte data B[2] = B[3]; B[3] = B[4]; Package_Length = 5; } TempLong = Displacement TempLong = TempLong >> 6; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //One byte data B[2] = B[3]; Package_Length = 4; } B[1] += (Package_Length - 4) * 32 + Function_Code; Make_CRC_Send(Package_Length, B);}void Make_CRC_Send(unsigned char Plength, unsigned char B[8]) { char RS232_HardwareShiftRegister; for (int i = 0; i < Plength - 1; i++) { OutputBuffer[OutBfTopPointer] = B[i]; OutBfTopPointer++; Error_Check += B[i]; } Error_Check = Error_Check | 0x80; OutputBuffer[OutBfTopPointer] = Error_Check; OutBfTopPointer++; while (OutBfBtmPointer != OutBfTopPointer) { RS232_HardwareShiftRegister = Output[OutBfBtmPointer]; //Serial.print("RS232_HardwareShiftRegister: "); //Serial.println(RS232_HardwareShiftRegister, DEC); Serial3.write(RS232_HardwareShiftRegister); OutBfBtmPointer++; // Change to next byte in OutputBuffer to send }}void ReadMotorTorqueCurrent(void) { char ID = 0; //Suppose read 0 axis motor Global_Func = General_Read; Send_Package(ID, Is_TrqCurrent); MotorTorqueCurrentReady_Flag = 0xff; while (MotorTorqueCurrentReady_Flag != 0x00) { ReadPackage(); }}void ReadMotorPosition32(void) { Global_Func = (char)General_Read; Send_Package(0, Is_AbsPos32); MotorPosition32Ready_Flag = 0xff; while (MotorPosition32Ready_Flag != 0x00) { ReadPackage(); }}void MoveMotorToAbsolutePosition32(char MotorID, long Pos32) { char Axis_Num = MotorID; Global_Func = (char)Go_Absolute_Pos; Send_Package(Axis_Num, Pos 32);}/////////////////////////////////* End of DMM Servo Functions */////////////////////////////////U/D Joystick Control – CREATED THIS SEMESTER// Zachary Guy// 2/26/19, 2/28/19// Senior Design - 2DOF Robotic Arm/* I am breaking this code into pieces to troubleshoot * and obtain functionallity again. This first code will use * an Arduino MEGA to communicate with Servo Drive (w/ ID=1) * The U/D potentiometer of the joystick will be included * in this code for control of one Servo Motor (Link 1) * This is the link that is fastened to the motor shaft via * 3D printed shaft adapter. * USING TX/RX 3 DYN232M_Arduino_SourceCode.ino - DMM Technology Corp. DYN2 and DYN4 AC Servo drive Arduino UART RS232 communication code. Version 1.0 - Release May 2018 Distributed By: DMM Technology Corp. dmm-*/#define Go_Absolute_Pos 0x01#define Go_Relative_Pos 0x03#define Is_AbsPos32 0x1b#define General_Read 0x0e #define Is_TrqCurrent 0x1E#define Read_MainGain 0x18#define Set_MainGain 0x10#define Set_SpeedGain 0x11#define Set_IntGain 0x12#define Set_HighSpeed 0x14#define Set_HighAccel 0x15#define Set_Pos_OnRange 0x16#define Is_MainGain 0x10#define Is_SpeedGain 0x11#define Is_IntGain 0x12#define Is_TrqCons 0x13#define Is_HighSpeed 0x14#define Is_HighAccel 0x15#define Is_Driver_ID 0x16#define Is_Pos_OnRange 0x17#define Is_Status 0x19#define Is_Config 0x1a// Add additional function code according to user requirement/*Variables*/char InputBuffer[256]; //Input buffer from RS232,char OutputBuffer[256]; //Output buffer to RS232,unsigned char InBfTopPointer, InBfBtmPointer; //input buffer pointersunsigned char OutBfTopPointer, OutBfBtmPointer; //output buffer pointersunsigned char Read_Package_Buffer[8], Read_Num, Read_Package_Length, Global_Func;unsigned char MotorPosition32Ready_Flag, MotorTorqueCurrentReady_Flag, MainGainRead_Flag;unsigned char Driver_MainGain, Driver_SpeedGain, Driver_IntGain, Driver_TrqCons, Driver_HighSpeed, Driver_HighAccel,Driver_ReadID,Driver_Status,Driver_Config,Driver_OnRange;long Motor_Pos32, MotorTorqueCurrent;/*Prototypes*/void move_rel32(char ID, long pos);void ReadMotorTorqueCurrent(char ID);void ReadMotorPosition32(char ID);void move_abs32(char MotorID, long Pos32);void Turn_const_speed(char ID, long spd);void ReadPackage(void);void Get_Function(void);long Cal_SignValue(unsigned char One_Package[8]);long Cal_Value(unsigned char One_Package[8]);void Send_Package(char ID , long Displacement);void Make_CRC_Send(unsigned char Plength, unsigned char B[8]);/**/int val_1, jsUD; //These are used to write to the Servo Drivevoid setup(){ Serial.begin(38400); // Serial0 used to write to Serial Monitor Serial3.begin(38400); // Servo drive connected to Serial_3 pinMode(LED_BUILTIN, OUTPUT); Serial.println("PROGRAM STARTING"); //Writing to Serial Monitor //Initial Settings for Servo Motor 1 Global_Func = (char)Set_MainGain; Send_Package(1, 20); Global_Func = (char)Set_SpeedGain; Send_Package(1, 20); Global_Func = (char)Set_IntGain; Send_Package(1, 1); Global_Func = (char)Set_HighSpeed; Send_Package(1, 1); Global_Func = (char)Set_HighAccel; Send_Package(1, 1); Global_Func = (char)Set_Pos_OnRange; Send_Package(1, 127); //Global_Func = (char)Set_GearNumber; //Send_Package(1, 16384);}void loop(){ Serial.println("STARTING LOOP"); jsUD = analogRead(A1);//connected to A1 on Arduino MEGA Serial.println(jsUD);//troubleshooting input value of joystick// Map of Up/Down Joystick for Motor 1 if (jsUD < 450) { val_1 = map(A1, 0, 450, -65536, 0); //-65536 = 1 rotation CCW } else if (jsUD > 550) { val_1 = map(A1, 550, 1023, 0, 65536); //65536 = 1 rotation CW } else { val_1 = 0; //No motion } //This is where I call global function defined by DMM digitalWrite(LED_BUILTIN, HIGH); move_rel32(1, val_1); delay(25); //Have to give time for signal to be sent and read //MODIFY Delay to 25ms from 100ms //ReadMotorPosition32(1); delay(100); //Serial.print("Axis 1 Encoder Position: "); //Serial.println(Motor_Pos32); //digitalWrite(LED_BUILTIN, LOW); //delay(1000); //Serial.println("END LOOP"); Serial.println("");}/////DMM Command Functions///////This command rotates the motor relative to it's current positionvoid move_rel32(char ID, long pos){ char Axis_Num = ID; Global_Func = (char)Go_Relative_Pos; Send_Package(Axis_Num, pos);}//This command reads the current motor position and we can use this to track position on the serial monitorvoid ReadMotorPosition32(char ID) { Global_Func = (char)General_Read; Send_Package(ID , Is_AbsPos32); MotorPosition32Ready_Flag = 0xff; while(MotorPosition32Ready_Flag != 0x00) { ReadPackage(); }}/////DYN232M SERIAL PROTOCOL PACKAGE FUNCTIONS//////* Everything below this line is for the serial communication * between the Arduino MEGA and the DYN4 Servo Drive * I have not modified anything except the Serial.available * or Serial.read in order to account for which port I am using * on the Arduino MEGA. */void ReadPackage(void) { unsigned char c, cif; while (Serial3.available() > 0) { //Modifying to serial_3 InputBuffer[InBfTopPointer] = Serial3.read(); //Load InputBuffer with received packets InBfTopPointer++; } while (InBfBtmPointer != InBfTopPointer) { c = InputBuffer[InBfBtmPointer]; InBfBtmPointer++; cif = c & 0x80; if (cif == 0) { Read_Num = 0; Read_Package_Length = 0; } if (cif == 0 || Read_Num > 0) { Read_Package_Buffer[Read_Num] = c; Read_Num++; if (Read_Num == 2) { cif = c >> 5; cif = cif & 0x03; Read_Package_Length = 4 + cif; c = 0; } if (Read_Num == Read_Package_Length) { Get_Function(); Read_Num = 0; Read_Package_Length = 0; } } }}void Get_Function(void) { char ID, ReceivedFunction_Code, CRC_Check; long Temp32; ID = Read_Package_Buffer[0] & 0x7f; ReceivedFunction_Code = Read_Package_Buffer[1] & 0x1f; CRC_Check = 0; for (int i = 0; i < Read_Package_Length - 1; i++) { CRC_Check += Read_Package_Buffer[i]; } CRC_Check ^= Read_Package_Buffer[Read_Package_Length - 1]; CRC_Check &= 0x7f; if (CRC_Check != 0) { } else { switch (ReceivedFunction_Code) { case Is_AbsPos32: Motor_Pos32 = Cal_SignValue(Read_Package_Buffer); MotorPosition32Ready_Flag = 0x00; break; case Is_TrqCurrent: MotorTorqueCurrent = Cal_SignValue(Read_Package_Buffer); break; case Is_Status: Driver_Status = (char)Cal_SignValue(Read_Package_Buffer); // Driver_Status=drive status byte data break; case Is_Config: Temp32 = Cal_Value(Read_Package_Buffer); //Driver_Config = drive configuration setting break; case Is_MainGain: Driver_MainGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_MainGain = Driver_MainGain & 0x7f; break; case Is_SpeedGain: Driver_SpeedGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_SpeedGain = Driver_SpeedGain & 0x7f; break; case Is_IntGain: Driver_IntGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_IntGain = Driver_IntGain & 0x7f; break; case Is_TrqCons: Driver_TrqCons = (char)Cal_SignValue(Read_Package_Buffer); Driver_TrqCons = Driver_TrqCons & 0x7f; break; case Is_HighSpeed: Driver_HighSpeed = (char)Cal_SignValue(Read_Package_Buffer); Driver_HighSpeed = Driver_HighSpeed & 0x7f; break; case Is_HighAccel: Driver_HighAccel = (char)Cal_SignValue(Read_Package_Buffer); Driver_HighAccel = Driver_HighAccel & 0x7f; break; case Is_Driver_ID: Driver_ReadID = ID; break; case Is_Pos_OnRange: Driver_OnRange = (char)Cal_SignValue(Read_Package_Buffer); Driver_OnRange = Driver_OnRange & 0x7f; break; } }}long Cal_SignValue(unsigned char One_Package[8]) //Get data with sign - long{ char Package_Length,OneChar,i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar>>5; OneChar = OneChar&0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 reprents sign */ OneChar = OneChar << 1; Lcmd = (long)OneChar; /* Sign extended to 32bits */ Lcmd = Lcmd >> 1; for(i=3;i<Package_Length-1;i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd<<7; Lcmd += OneChar; } return(Lcmd); /* Lcmd : -2^27 ~ 2^27 - 1 */}long Cal_Value(unsigned char One_Package[8]){ char Package_Length,OneChar,i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar>>5; OneChar = OneChar&0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 reprents sign */ OneChar &= 0x7f; Lcmd = (long)OneChar; /*Sign extended to 32bits */ for(i=3;i<Package_Length-1;i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd<<7; Lcmd += OneChar; } return(Lcmd); /*Lcmd : -2^27 ~ 2^27 - 1 */}void Send_Package(char ID , long Displacement) { unsigned char B[8], Package_Length, Function_Code; long TempLong; B[1] = B[2] = B[3] = B[4] = B[5] = (unsigned char)0x80; B[0] = ID & 0x7f; Function_Code = Global_Func & 0x1f; TempLong = Displacement & 0x0fffffff; //Max 28bits B[5] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[4] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[3] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[2] += (unsigned char)TempLong & 0x0000007f; Package_Length = 7; TempLong = Displacement; TempLong = TempLong >> 20; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Three byte data B[2] = B[3]; B[3] = B[4]; B[4] = B[5]; Package_Length = 6; } TempLong = Displacement; TempLong = TempLong >> 13; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Two byte data B[2] = B[3]; B[3] = B[4]; Package_Length = 5; } TempLong = Displacement; TempLong = TempLong >> 6; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //One byte data B[2] = B[3]; Package_Length = 4; } B[1] += (Package_Length - 4) * 32 + Function_Code; Make_CRC_Send(Package_Length, B);}void Make_CRC_Send(unsigned char Plength, unsigned char B[8]) { unsigned char Error_Check = 0; char RS232_HardwareShiftRegister; for (int i = 0; i < Plength - 1; i++) { OutputBuffer[OutBfTopPointer] = B[i]; OutBfTopPointer++; Error_Check += B[i]; } Error_Check = Error_Check | 0x80; OutputBuffer[OutBfTopPointer] = Error_Check; OutBfTopPointer++; while (OutBfBtmPointer != OutBfTopPointer) { RS232_HardwareShiftRegister = OutputBuffer[OutBfBtmPointer]; //Serial.print("RS232_HardwareShiftRegister: "); //Serial.println(RS232_HardwareShiftRegister, DEC); Serial3.write(RS232_HardwareShiftRegister); //modifying to serial_3 OutBfBtmPointer++; // Change to next byte in OutputBuffer to send }}L/R Joystick Control – CREATED THIS SEMESTER// Zachary Guy// 2/27/19, 2/28/19// Senior Design - 2DOF Robotic Arm/* I am breaking this code into pieces to troubleshoot * and obtain functionallity again. This second code will use * an Arduino MEGA to communicate with Servo Drive (w/ ID=2) * The L/R potentiometer of the joystick will be included * in this code for control of one Servo Motor (Link 2) * This is the link that is fastened via pulley and * belt system to the motor shaft via 3D printed shaft adapter. * USING TX/RX 2 DYN232M_Arduino_SourceCode.ino - DMM Technology Corp. DYN2 and DYN4 AC Servo drive Arduino UART RS232 communication code. Version 1.0 - Release May 2018 Distributed By: DMM Technology Corp. dmm-*/#define Go_Absolute_Pos 0x01#define Go_Relative_Pos 0x03#define Is_AbsPos32 0x1b#define General_Read 0x0e #define Is_TrqCurrent 0x1E#define Read_MainGain 0x18#define Set_MainGain 0x10#define Set_SpeedGain 0x11#define Set_IntGain 0x12#define Set_HighSpeed 0x14#define Set_HighAccel 0x15#define Set_Pos_OnRange 0x16#define Is_MainGain 0x10#define Is_SpeedGain 0x11#define Is_IntGain 0x12#define Is_TrqCons 0x13#define Is_HighSpeed 0x14#define Is_HighAccel 0x15#define Is_Driver_ID 0x16#define Is_Pos_OnRange 0x17#define Is_Status 0x19#define Is_Config 0x1a// Add additional function code according to user requirement/*Variables*/char InputBuffer[256]; //Input buffer from RS232,char OutputBuffer[256]; //Output buffer to RS232,unsigned char InBfTopPointer, InBfBtmPointer; //input buffer pointersunsigned char OutBfTopPointer, OutBfBtmPointer; //output buffer pointersunsigned char Read_Package_Buffer[8], Read_Num, Read_Package_Length, Global_Func;unsigned char MotorPosition32Ready_Flag, MotorTorqueCurrentReady_Flag, MainGainRead_Flag;unsigned char Driver_MainGain, Driver_SpeedGain, Driver_IntGain, Driver_TrqCons, Driver_HighSpeed, Driver_HighAccel,Driver_ReadID,Driver_Status,Driver_Config,Driver_OnRange;long Motor_Pos32, MotorTorqueCurrent;/*Prototypes*/void move_rel32(char ID, long pos);void ReadMotorTorqueCurrent(char ID);void ReadMotorPosition32(char ID);void move_abs32(char MotorID, long Pos32);void Turn_const_speed(char ID, long spd);void ReadPackage(void);void Get_Function(void);long Cal_SignValue(unsigned char One_Package[8]);long Cal_Value(unsigned char One_Package[8]);void Send_Package(char ID , long Displacement);void Make_CRC_Send(unsigned char Plength, unsigned char B[8]);/**/int val_2, jsLR; //These are used to write to the Servo Drivevoid setup(){ Serial.begin(38400); // Serial0 used to write to Serial Monitor Serial3.begin(38400); // Servo drive connected to Serial_2 pinMode(LED_BUILTIN, OUTPUT); //Serial.println("PROGRAM STARTING"); //Writing to Serial Monitor //Initial Settings for Servo Motor 2 Global_Func = (char)Set_MainGain; Send_Package(2, 20); Global_Func = (char)Set_SpeedGain; Send_Package(2, 20); Global_Func = (char)Set_IntGain; Send_Package(2, 1); Global_Func = (char)Set_HighSpeed; Send_Package(2, 1); Global_Func = (char)Set_HighAccel; Send_Package(2, 1); Global_Func = (char)Set_Pos_OnRange; Send_Package(2, 127); //Global_Func = (char)Set_GearNumber; //Send_Package(2, 16384);}void loop(){ Serial.println("STARTING LOOP"); jsLR = analogRead(A1);//connected to A1 on Arduino MEGA Serial.println(jsLR);//troubleshooting input value of joystick// Map of Up/Down Joystick for Motor 1 if (jsLR < 450) { val_2 = map(A1, 0, 450, -65536, 0); //-65536 = 1 rotation CCW } else if (jsLR > 550) { val_2 = map(A1, 550, 1023, 0, 65536); //65536 = 1 rotation CW } else { val_2 = 0; //No motion } //This is where I call global function defined by DMM digitalWrite(LED_BUILTIN, HIGH); move_rel32(2, val_2); delay(25); //Have to give time for signal to be sent and read //MODIFY Delay to 25ms from 100ms //ReadMotorPosition32(1); delay(100); //Serial.print("Axis 1 Encoder Position: "); //Serial.println(Motor_Pos32); //digitalWrite(LED_BUILTIN, LOW); //delay(1000); //Serial.println("END LOOP"); Serial.println("");}/////DMM Command Functions///////This command rotates the motor relative to it's current positionvoid move_rel32(char ID, long pos){ char Axis_Num = ID; Global_Func = (char)Go_Relative_Pos; Send_Package(Axis_Num, pos);}//This command reads the current motor position and we can use this to track position on the serial monitorvoid ReadMotorPosition32(char ID) { Global_Func = (char)General_Read; Send_Package(ID , Is_AbsPos32); MotorPosition32Ready_Flag = 0xff; while(MotorPosition32Ready_Flag != 0x00) { ReadPackage(); }}/////DYN232M SERIAL PROTOCOL PACKAGE FUNCTIONS//////* Everything below this line is for the serial communication * between the Arduino MEGA and the DYN4 Servo Drive * I have not modified anything except the Serial.available * or Serial.read in order to account for which port I am using * on the Arduino MEGA. */void ReadPackage(void) { unsigned char c, cif; while (Serial3.available() > 0) { //Modifying to serial_2 from normal serial_3 InputBuffer[InBfTopPointer] = Serial3.read(); //Load InputBuffer with received packets InBfTopPointer++; } while (InBfBtmPointer != InBfTopPointer) { c = InputBuffer[InBfBtmPointer]; InBfBtmPointer++; cif = c & 0x80; if (cif == 0) { Read_Num = 0; Read_Package_Length = 0; } if (cif == 0 || Read_Num > 0) { Read_Package_Buffer[Read_Num] = c; Read_Num++; if (Read_Num == 2) { cif = c >> 5; cif = cif & 0x03; Read_Package_Length = 4 + cif; c = 0; } if (Read_Num == Read_Package_Length) { Get_Function(); Read_Num = 0; Read_Package_Length = 0; } } }}void Get_Function(void) { char ID, ReceivedFunction_Code, CRC_Check; long Temp32; ID = Read_Package_Buffer[0] & 0x7f; ReceivedFunction_Code = Read_Package_Buffer[1] & 0x1f; CRC_Check = 0; for (int i = 0; i < Read_Package_Length - 1; i++) { CRC_Check += Read_Package_Buffer[i]; } CRC_Check ^= Read_Package_Buffer[Read_Package_Length - 1]; CRC_Check &= 0x7f; if (CRC_Check != 0) { } else { switch (ReceivedFunction_Code) { case Is_AbsPos32: Motor_Pos32 = Cal_SignValue(Read_Package_Buffer); MotorPosition32Ready_Flag = 0x00; break; case Is_TrqCurrent: MotorTorqueCurrent = Cal_SignValue(Read_Package_Buffer); break; case Is_Status: Driver_Status = (char)Cal_SignValue(Read_Package_Buffer); // Driver_Status=drive status byte data break; case Is_Config: Temp32 = Cal_Value(Read_Package_Buffer); //Driver_Config = drive configuration setting break; case Is_MainGain: Driver_MainGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_MainGain = Driver_MainGain & 0x7f; break; case Is_SpeedGain: Driver_SpeedGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_SpeedGain = Driver_SpeedGain & 0x7f; break; case Is_IntGain: Driver_IntGain = (char)Cal_SignValue(Read_Package_Buffer); Driver_IntGain = Driver_IntGain & 0x7f; break; case Is_TrqCons: Driver_TrqCons = (char)Cal_SignValue(Read_Package_Buffer); Driver_TrqCons = Driver_TrqCons & 0x7f; break; case Is_HighSpeed: Driver_HighSpeed = (char)Cal_SignValue(Read_Package_Buffer); Driver_HighSpeed = Driver_HighSpeed & 0x7f; break; case Is_HighAccel: Driver_HighAccel = (char)Cal_SignValue(Read_Package_Buffer); Driver_HighAccel = Driver_HighAccel & 0x7f; break; case Is_Driver_ID: Driver_ReadID = ID; break; case Is_Pos_OnRange: Driver_OnRange = (char)Cal_SignValue(Read_Package_Buffer); Driver_OnRange = Driver_OnRange & 0x7f; break; } }}long Cal_SignValue(unsigned char One_Package[8]) //Get data with sign - long{ char Package_Length,OneChar,i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar>>5; OneChar = OneChar&0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 reprents sign */ OneChar = OneChar << 1; Lcmd = (long)OneChar; /* Sign extended to 32bits */ Lcmd = Lcmd >> 1; for(i=3;i<Package_Length-1;i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd<<7; Lcmd += OneChar; } return(Lcmd); /* Lcmd : -2^27 ~ 2^27 - 1 */}long Cal_Value(unsigned char One_Package[8]){ char Package_Length,OneChar,i; long Lcmd; OneChar = One_Package[1]; OneChar = OneChar>>5; OneChar = OneChar&0x03; Package_Length = 4 + OneChar; OneChar = One_Package[2]; /*First byte 0x7f, bit 6 reprents sign */ OneChar &= 0x7f; Lcmd = (long)OneChar; /*Sign extended to 32bits */ for(i=3;i<Package_Length-1;i++) { OneChar = One_Package[i]; OneChar &= 0x7f; Lcmd = Lcmd<<7; Lcmd += OneChar; } return(Lcmd); /*Lcmd : -2^27 ~ 2^27 - 1 */}void Send_Package(char ID , long Displacement) { unsigned char B[8], Package_Length, Function_Code; long TempLong; B[1] = B[2] = B[3] = B[4] = B[5] = (unsigned char)0x80; B[0] = ID & 0x7f; Function_Code = Global_Func & 0x1f; TempLong = Displacement & 0x0fffffff; //Max 28bits B[5] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[4] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[3] += (unsigned char)TempLong & 0x0000007f; TempLong = TempLong >> 7; B[2] += (unsigned char)TempLong & 0x0000007f; Package_Length = 7; TempLong = Displacement; TempLong = TempLong >> 20; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Three byte data B[2] = B[3]; B[3] = B[4]; B[4] = B[5]; Package_Length = 6; } TempLong = Displacement; TempLong = TempLong >> 13; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //Two byte data B[2] = B[3]; B[3] = B[4]; Package_Length = 5; } TempLong = Displacement; TempLong = TempLong >> 6; if (( TempLong == 0x00000000) || ( TempLong == 0xffffffff)) { //One byte data B[2] = B[3]; Package_Length = 4; } B[1] += (Package_Length - 4) * 32 + Function_Code; Make_CRC_Send(Package_Length, B);}void Make_CRC_Send(unsigned char Plength, unsigned char B[8]) { unsigned char Error_Check = 0; char RS232_HardwareShiftRegister; for (int i = 0; i < Plength - 1; i++) { OutputBuffer[OutBfTopPointer] = B[i]; OutBfTopPointer++; Error_Check += B[i]; } Error_Check = Error_Check | 0x80; OutputBuffer[OutBfTopPointer] = Error_Check; OutBfTopPointer++; while (OutBfBtmPointer != OutBfTopPointer) { RS232_HardwareShiftRegister = OutputBuffer[OutBfBtmPointer]; //Serial.print("RS232_HardwareShiftRegister: "); //Serial.println(RS232_HardwareShiftRegister, DEC); Serial3.write(RS232_HardwareShiftRegister); //modifying to serial_2 from serial_3 OutBfBtmPointer++; // Change to next byte in OutputBuffer to send }} ................
................

In order to avoid copyright disputes, this page is only a partial summary.

Google Online Preview   Download