Yamaha YK500XGP Design Project

Software Whitepaper

M. Van Genderen, C. Hansen, S. Sidorov

Faculty of Electrical & Computer Engineering, University of British Columbia

Abstract

A SCARA robotic arm was developed, based on the Yamaha YK500XGP design. Our SCARA was designed with simplicity and affordability as priorities, as this team plans to physically build the designed arm in the future. Our SCARA arm will draw shapes in its region of operation. While simple, it leaves a lot of room for expansion of the idea. For a basic demonstration of this capability, hardcoding a shape to draw would work as a proof of concept. From this proof of concept, it’s relatively straightforward to extend the system to reading from text files, to reading from a phone app via Wifi or Bluetooth, for example.

Control Frequency Calculations

Software

The following is a report on the Software part of the project, which includes the design of our actuators and controller, as well as all the C code we wrote for this project.

Controller Design

Table 1: Controller RCGs

Property	Requirement	Constraint	Goal
Rise Time	–	–	< 1 s
Motor Speed	> 0.5 rad/s	–	3 rad/s
Percentage Overshoot	–	–	<3%
Settle Time	–	–	< 0.5 s

O2 Amp

We designed second order transfer functions to approximate our op amp. Settlement time, rise time, and peak time values from the real amplifier’s step response were used to approximate the second order transfer function. We used the best possible approximation which did not destabilize our system (Plot labeled Tuned) as our model. The matlab code producing this graph and containing our op amp calculations can be found in the appendix.

Figure 1: Amplitude TF Step Responses

Motor and Gears

We used the standard motor transfer function for this project as shown below:

Figure 2: The Motor Feedback Circuit

Figure 3: The Motor TF Circuit (Including a Gear Ratio)

We’re using the low speed version of the mechanical transfer function, and therefore must multiply the torque by the gear ratio before entering the mechanical TF (since the torque is greater on the higher gear side of the gearbox). In the feedback loop we must multiply by the gear ratio to account for the increased speed on the low gear side of the gearbox. The values shown in the diagram above are listed in the actuator section of this report.

Sensor

Our sensor has a resolution of 1024 and our microcontroller can only read whole values. The resolution represents how many times the counter will increment per complete revolution of the motor. Therefore, we can convert from radians to our resolution's interpretation of that angle by multiplying by 1023/(2π). Since our microcontroller will only read whole numbers of this value, we must either floor or ceiling this value. Finally we multiply by the inverse to maintain unity gain for the sensor.

Figure 4: Model of Sensor in Simulink

Figure 5: Before and After Roof Block. Yellow->before,Blue -> after

Figure 6: Before and After Floor Block. Yellow->before,Blue -> after

Control Frequency Calculations

In order to calculate the control frequency (CF), we use a ESP32 emulator and perform the following:

Store the value of a ESP32 timer which is automatically started when the microcontroller runs (by calling micros() function)
Loop ISR n times (where n is greater than a thousand)
Once again, retrieve the microcontroller’s relative time
The difference in time between the previous time measurements is an approximation of the average time to complete the ISR + time to run additional code(ie. micros() and for loop)
Run a separate program to approximate the time to complete this additional code, find average ISR time, and multiply by 2 to avoid incomplete ISR calls

For 100000 iterations we compute a find a total execution time of 36527335 microseconds, and we find that the time to run additional code is about 5 microseconds:

Therefore our control frequency is:

Sensor Filter

// Fourth order sensor filter

*angle1 = (*angle1) * 0.2 + olda1[0] * 0.2 + olda1[1] * 0.2 + olda1[2] * 0.2 + olda1[3] * 0.2;

*angle2 = (*angle2) * 0.2 + olda2[0] * 0.2 + olda2[1] * 0.2 + olda2[2] * 0.2 + olda2[3] * 0.2;

olda1[3] = olda1[2];

olda1[2] = olda1[1];

olda1[1] = olda1[0];

olda1[0] = *angle1;

olda2[3] = olda2[2];

olda2[2] = olda2[1];

olda2[1] = olda2[0];

olda2[0] = *angle1;

From we can build I first order low pass filter to represent this in simulink suing the following formulas:

The controller dynamics of the filter are as shown in lecture:

Figure 7: Controller Dynamics in Simulink

Figure 8: Controller Dynamics before and after (blue -> after)

Derivative Filter

Once again we are using a finite impulse response (FIR) filter

// fourth order derivative filter

edot = edot * 0.2 + oldedot[0] * 0.2 + oldedot[1] * 0.2 + oldedot[2] * 0.2 + oldedot[3] * 0.2;

oldedot[3] = oldedot[2];

oldedot[2] = oldedot[1];

oldedot[1] = oldedot[0];

oldedot[0] = edot;

And we use the below to model this in Simulink:

Figure 9: Derivative TF in Simulink

10-Step Process

This process is very similar for both motors, so I will go through them both simultaneously and only explain my thinking in detail for motor one. One note, we completed steps 2-7 with the function cdes, which can be found in the appendix.

Step 1: Identification of the System

Using the models discussed above, we are representing our system as shown:

Figure 10: Simulink System

Step 2: Find Phase X-Over Freq

Executing the step2 inner function from the cdes function produces the following bode diagram.

Figure 11: Bode Plots for Finding Phase Crossover Frequency (Motor 1 and 2, Respectively)

From this we can find that our initial zero for motor 1 is 1.5565 rad/s. Running the same function for motor two results in an initial zero of z2 = 5.9370 rad/s.

Step 3:

Using standard standard kp, kd, ki and dynamics equations, the step 3 function runs and finds the following:

Motor 1:

kp = 1.2847

ki = 1

kd = 0.9975

Dynamics:

Motor 2:

kp = 0.3367

ki = 1

kd = 0.0283

Dynamics:

Step 4: Find New Phase X-Over Freq

We recomputed the dynamics, found the new zero and repeated this 20 times with the step4 function in cdes. The resulting values:

Motor1:

kp4 = 2.2335

ki4 = 1

kd4 = 1.2469

Dynamics:

Motor2:

kp4 = 0.1971

ki4 = 1

kd4 = 0.0097

Dynamics:

Step 5: Initial Gain K

We can compute the initial gian with the margin function and the values found in step4

Figure 12. Root Locus of Motor 1 (Left) and Motor 2 (Right) Open-Loop Transfer Function

Initial Gain is…

Motor1:

k = 2.7191

Motor 2:

k = 17.5274

Step 6: Check Result

Figure 13: Nyqlog Plots; Motor 1

We can observe by comparing the nyqlog plots with and without k that the corner moves nearer to the odB line as desired. Phase margin is also improved. The same can be observed for motor 2:

Figure 14: Nyqlog Plots; Motor 2

Step 7: Step Response

Using step 7 in the CDEs function, let's generate three step responses optimizing for different qualities–overshoot (kos), settle time(kts) and steady-state error(kes).

Figure 15: Step Responses for Motors 1 and 2, Respectively

Since we're both concerned about the speed and overshoot of our arm, let's use the ktos step responses as a basis before we begin heuristically tuning with non-linearities added to the system.

Heuristic Tuning

The below is our simulink system with nonlinearities considered.

Besides simulationx we considered the following non-linearities:

Discrete signal of PID

Added zero hold block after pid. Holds for the time between ISR executions (1/CF)

Voltage limit of microcontroller (approximately 3.3V)

Added a saturation block after PID

Voltage limit of amplifier

There are electronics preventing the amplifier from exceeding 15 volts
Added saturation block after amplifier

Floor or ceiling block

As explained in the sensor model design section, a floor or ceiling block is required since the microcontroller receives whole values

Figure 16: Simulink System with Co-Simulation Block Connected

Tuning:

We notice that it's much more difficult to get a successful simulation with all these added nonlinearities. Normally the amplifier acted as sort of low pass filter to smooth out the PWM data from the PID, however with all these added nonlinearities the output of the amplifier remains unsmooth:

Figure 17: Unfiltered Amplifier Output Voltage

To prevent this from impairing the model of our system, we've added a basic lowpass filter, and adjusted the cutoff frequency until the system reacted appropriately.

Figure 18: Filtered Amplifier Output Voltage

I also noticed that the system was overshooting overall, and observed that since we were flooring the sensor data, the error being sent through the feedback was less than it should have been. In an attempt to correct this, I opted to use the ceiling block instead (switching to this representation in code would be very simple) and it corrected a good portion of the overshoot.

Figure 19: SimulationX Co-Simulation Output (positions are in red and yellow, speeds in blue and green)

Eventually tuned to work fairly well. Both angles were set to settle at π/16. The largest overshoot is less than 1 percent, satisfying our Percentage Overshoot RCG.

Figure 20: Path Traced by the SCARA (in meters)

Actuator Design

Table 2: Actuator RCGs

Property	Requirement	Constraint	Goal
Stall Torque	> 3.5 Nm	–	> 35 Nm
Speed	> 3 rad/s	–	–
Encoder Resolution (CPR)	1024	–	–

For our actuators, we picked the BAG Motor1 mounted to a Versaplanetary 100:1 gearbox2, with a Versaplanetary integrated encoder3.

BAG Motor

Vnominal = 12V DC

Ωno load = 13,180 rpm

Ino load = 1.8A

Pmax = 149 W

τstall = 0.4 N-m

Istall = 53A

VersaPlanetary Gearbox v2

Stage: Single Stage

Gear Ratio: 100:1

Shaft: ½” Hex

Encoder: Integrated

The main criteria that influenced our decision: we wanted it to be inexpensive (accessibility), easy to assemble and mount (simplicity), have sufficient torque (enough to move the arm, with a safety factor of 10), and sufficient speed (say, > 3 rad/s). For information about the encoder resolution can be found in the Hardware report.

We needed to determine the motor parameters left implicit in the datasheet – R, L, KM, the Js and the Bs – in order to construct our transfer functions. We did so in the following way:

Resistance R

The resistance was calculated using the following formula: R = Vnominal / Istall = 0.2264 Ω.

Inductance L

A representative sample of motors was sampled from the Maxon catalog, and the relationship between their resistances and inductances was plotted. The relationship was observed to be approximately linear. Performing a linear regression and plugging in our calculated R, we get L = 0.059155 mH.

Figure 21: Inductance vs Resistance Plot for 10 Maxon Motors

Motor Constant KM

Similarly, plotting torque vs current from the provided BAG motor data4, we perform a linear regression. The slope of the line is our motor constant; KM = 0.008474576271 Nm/A.

Inertias of the motor and gearbox were taken from the solidworks models, after inputting the appropriate material information. The inertia of the Head and Head + Arm assemblies was determined in a similar way. After that, they were combined into two comprehensive Js for each respective motor, using the formula J = i2⋅(Jmotor + Jpinion) + Jgear + Jload where i is the gear ratio. JHead = 33015120⋅10–9 kgm2 and JArm + Head = 2574181465⋅10–9 kgm2.

The Bs were determined in the following way: Bmotor = Vnominal ⋅ Ino load / (Vnominal/KM)2; Btooth = efficiency ⋅ τnominal / ωnominal; Bgear = 7⋅10–4 kg/s, assuming blown bearings and finally B = i2⋅(Bmotor + Btooth) + Bgear where i is the gear ratio. B = 0.5130 kg/s.

Since τ = Bω, the torque required to move the SCARA was calculated by plugging in our B value, calculated above, along with the minimum speed as indicated in our RCGs. τ = 0.5130 ⋅ 3 = 1.5 Nm. With a safety factor of 10, τ = 15 Nm. Since the stall torque of the BAG Motor is 0.4 (40 with a 100:1 gearbox), a 100:1 gear ratio suffices whereas, say, a 10:1 would not.

Code

Table 3: Code RCGs

Property	Requirement	Constraint	Goal
Control Frequency	–	–	>1kHz
FIlter order	–	–	>3
Code size	<4MB	–	<1MB Without xy data

Homing Logic:

We check the sensor value. If both read less than 20 centimeters then we are homed and can proceed. Otherwise we wait. See code below:

// https://howtomechatronics.com/tutorials/arduino/ultrasonic-sensor-hc-sr04/

// The above link was a very useful resource in developing this code

long duration1;

float distance1;

long duration2;

float distance2;

//if distance is less than 20 centimeter for both sensors then we are homed

while (distance1 < 0.2 || distance2 < 2)

{

// Clears the trigPin

digitalWrite(TRIG1, LOW);

delayMicroseconds(2);

// Sets the trigPin on HIGH state for 10 micro seconds

digitalWrite(TRIG1, HIGH);

delayMicroseconds(10);

digitalWrite(TRIG1, LOW);

// Reads the echoPin, returns the sound wave travel time in microseconds

duration1 = pulseIn(ECHO1, HIGH);

// Calculating the distance

distance1 = duration1 * 0.034 / 2.0;

// Clears the trigPin

digitalWrite(TRIG2, LOW);

delayMicroseconds(2);

// Sets the trigPin on HIGH state for 10 micro seconds

digitalWrite(TRIG2, HIGH);

delayMicroseconds(10);

digitalWrite(TRIG2, LOW);

// Reads the echoPin, returns the sound wave travel time in microseconds

duration2 = pulseIn(ECHO1, HIGH);

// Calculating the distance

distance2 = duration2 * 0.034 / 2.0;

}

// set counters to zero(home)

counter1 = 0;

Sensor Logic

We are interfacing with a 4 bit counter and a 1024 resolution encoder for this build.

Our sensor logic procedure is as follow:

Read from carry and borrow pins
If read is one, increment count by 16. if borrow bit is one, subtract 16 to counter
If the counter is greater than or equal to resolution(ie. It has completed a full rotation), subtract 1024 from counter (using modulo)
Read values from the 4 1 bit sensor pins
Shift and mask values appropriate to find current 4 bit sensor value
The current angle is the sum of the 4 bit sensor value, and the count

* read from sensors and update angles and counters accordingly

* PARAM: pointer to counter for motor 1

* PARAM: pointer to angle for motor 1

* PARAM: pointer to counter for motor 2

* PARAM: pointer to angle for motor 2

* PRE: each argument is a valid pointer and is already allocated memory for one integer

* Post: counter1, angle1, counter2, and angle2 are updated with new values interpreted from sensor reads

* RETURN: VOID

void readSensors(int *counter1, int *angle1, int *counter2, int *angle2)

{

char carry = digitalRead(carry1); // carry bit

char borrow = digitalRead(borrow1); // borrow bit

// if borrow bit is one, subtract 16 to counter

// if carry bit is one, add 16 to counter

*counter1 += (carry - borrow) * 16;

// when motor completes rotation, counter will exceed resolution

*counter1 = *counter1 % res; // this will subtract res from any counter >= res

// encoder for motor 1

*angle1 = digitalRead(e1Pin4); // least significant bit

*angle1 |= digitalRead(e1Pin3) << 1;

*angle1 |= digitalRead(e1Pin2) << 2;

*angle1 |= digitalRead(e1Pin1) << 3; // most significant bit

*angle1 += *counter1;

carry = digitalRead(carry2); // carry bit

borrow = digitalRead(borrow2); // borrow bit

// if borrow bit is one, subtract 16 to counter

// if carry bit is one, add 16 to counter

*counter2 += (carry - borrow) * 16;

*counter2 = *counter2 % res;

// encoder digits for second motor

*angle2 = digitalRead(e1Pin4); // least significant bit

*angle2 |= digitalRead(e1Pin3) << 1;

*angle2 |= digitalRead(e1Pin2) << 2;

*angle2 |= digitalRead(e1Pin1) << 3; // most significant bit

*angle2 += *counter2;

// Fourth order sensor filter

*angle1 = (*angle1) * 0.2 + olda1[0] * 0.2 + olda1[1] * 0.2 + olda1[2] * 0.2 + olda1[3] * 0.2;

*angle2 = (*angle2) * 0.2 + olda2[0] * 0.2 + olda2[1] * 0.2 + olda2[2] * 0.2 + olda2[3] * 0.2;

olda1[3] = olda1[2];

olda1[2] = olda1[1];

olda1[1] = olda1[0];

olda1[0] = *angle1;

olda2[3] = olda2[2];

olda2[2] = olda2[1];

olda2[1] = olda2[0];

olda2[0] = *angle1;

}

Kinematics

Inverse and forward kinematics are being applied in our build to convert from xy coordinates to angles, and from angles back to xy coordinates.

Figure 22: Inverse Kinematics Diagram

We can easily find a formula for forward kinematics using basic trigonometry

The inverse kinematics formula is more complicated, in the first quadrant we can compute it using cosine and sine law and find the following:

Inverse kinematics becomes even more complicated when considering multiple quadrants, and optimizing for nearest angle(ie. Rotating to negative ten degrees instead of positive 350 degrees). Fortunately we discovered several useful references online walking through trigonometry[5] and ideal angles per quadrant [5]. From these, we formulated the matlab and c functions available in the appendix.

* Convert arm angles to xy coordinate

* PARAM: float for angle between vertical axis and arm 1 (positive angle in clockwise direction)

* PARAM: float for angle between arm1 and arm2 (positive angle when arm2 is is more clockwise than arm 1)

* PRE: xy is a valid pointer and is already allocated memory for two floats

* Post: xy points to an array of two floats, with the first element containing the x coordinate

* and the second containing the y coordinate

* RETURN: VOID

* ATTRIBUTION: This code was with reference to the following two sources

* - https://howtomechatronics.com/projects/scara-robot-how-to-build-your-own-arduino-based-robot/

* - https://robotacademy.net.au/lesson/inverse-kinematics-for-a-2-joint-robot-arm-using-geometry/?fbclid=IwAR1dexD-6qJSMt2P9hVNzZhVUVJWzsUyGeMKlYZBgkTWxj0W2RKYrFDkPgM

void forwardKinematics(float a1, float a2, float *xy)

{

xy[0] = L1 * cos(a1) + L2 * cos(a1 + a2);

xy[1] = L1 * sin(a1) + L2 * sin(a1 + a2);

}

Path Planning

Our poth planning is time based. We adjust to the next value based on the internal clock of the microcontroller. This was chosen instead of an error based approach since our application does not require extreme precision and a time based method has a more predictable speed.

Circular path planning

function a1 = fcn(t)

persistent lastTime;

if(isempty(lastTime))

lastTime = t;

end

if((t>lastTime+0.1))

lastTime = t;

end

a1 = t*pi/2; %complete loop in 4 seconds

Back and forth path planning

function a2 = fcn(t)

if(mod(floor(t), 2)==0) %if even

a2 = pi/6;

else

a2 = 0;

end

C-code Path planning

* update path values every pathUpdateTime microseconds

* PARAM: unsigned long pointer for time since last update

* PARAM: integer pointer float for angle between arm1 and arm2 (positive angle when arm2 is is more clockwise than arm 1)

* PRE: pointI is a valid pointer and is already allocated memory for one integer

* time is a valid pointer and is already allocated memory for one unsigned long

* Post: if the update occurs, time holds current timer value and pointI is incremented by 1

* otherwise, no changes

* RETURN: VOID

void pathPlanning(unsigned long *time, int *pointI)

{

if ((micros() - *time) > pathUpdateTime)

{

*time = micros();

if (size <= (*pointI + 1))

{

exit(0);

}

(*pointI)++;

}

Self-Documentation

Our code is thoroughly commented, and designed to be easy to understand. Please see the appendix for a file

Testing

We utilized several testing methods:

Running code in Wokwi ESP 32 simulator verified that code could run on device
Run code in simulink/matlab where possible to test its validity.
Tested inverse and forward kinematics functions with known values to ensure functionality

References:

“BAG Motor.” VEX Robotics, www.vexrobotics.com/217-3351.html.
“VersaPlanetary Gearbox.” VEX Robotics, www.vexrobotics.com/versaplanetary.html.
“VersaPlanetary Integrated Encoder.” VEX Robotics, www.vexrobotics.com/217-5046.html.
“BAG Motor - VEXpro Motors - VEX Robotics.” Motors, motors.vex.com/vexpro-motors/bag-motor.
“SCARA Robot | How To Build Your Own Arduino Based Robot.” How To Mechatronics, https://howtomechatronics.com/projects/scara-robot-how-to-build-your-own-arduino-based-robot/.

Appendix:

Redacted

Abstract

Table of Contents

Software

Controller Design

O2 Amp

Motor and Gears

Sensor

Control Frequency Calculations

Sensor Filter

Derivative Filter

10-Step Process

Heuristic Tuning

Actuator Design

Code

Homing Logic:

Sensor Logic

Kinematics

Path Planning

Self-Documentation

Testing

References:

Appendix: