ECE 4760 final Project
By Tiange Zhao, Tianyu Liang
In this project, we built a writing machine using a 4-DOF robot arm. This robot arm is controlled by PIC32 and can write the 26 letters and ten numbers. Users can send instructions to the microcontroller through the serial interface to control the robot arm to write words or digits they want.
Since what we want is to control the arm write on a 2D surface, the goal is to transfer the xy axis into the PWM duty cycle of each servo. We used two steps to achieve this.
The first step is to find the relationship between the angle of each part of the robot arm and the Ο-π axis.
From the image above, we can see that the servo0 changes the π in a Ο-π coordinate system while servo1,2,3 changes the Ο in the Ο-π coordinate system. Assume π increases as rotating clockwisely.
Since we need to keep the z axis of marker constant when writing, an equation of π1 and π2 can be listed.
H=L1+L2sinπ1-L3sin(π/2-π1-π2)
And Ο can be expressed as
Ο=L2cosπ1+L3cos(π/2-π1-π2)
We sweep π1 from arcsin((H-L1)/(L2+L3)) to π/4 so that we set up the relationship between π1 π2 and Ο.
Servo3 is used to keep the marker vertical at all times. So
π3=π/2-π1-π2
From the image above, we can find that
π0=arctan(x/y)
Ο2 = x2 + y2
Due to the limitation of budget and time, this robot arm has no feedback and other sensors. We have to slow down its writing speed to make it work more stable and precisely. These are achieved by software.
The robot arm has 6 DOFs at first. We removed one and fixed one to make it a 4-DOF arm. The movements are controlled by servos. Due to the budget, the whole system had no feedback and other sensors. The servo used is MG90S. Its supply voltage is 4.8V to 6V. For each servo, the peak current while working is about 500mA, so the output of the power supply we use should be more than 6V*0.5A*4=12W. We used a 50W(5V 10A) power supply.
Since the amplitude of the PWM to drive this kind of servo is 4.8 to 6V and the amplitude of PWM wave generated by PIC32 is only 3.3V, a converter is needed. We used 74lc32(OR gate) to fix this problem. The schematic shows below.
The output of the PIC32 and the output of the converter is shown below.
The robot arm has four degrees of freedom, so there are we four PWM waves to control the angle positions. Timer 3 is used to generate the wave period and OC1 - OC4 four output compare units which maps to RB7, RB8, RB2 and RB9. The generate period parameter is set to 50000, and frequency division parameter is set to 16. The PWM signal frequency is 50HZ.
The duty cycle determined the angle of servo. The relationship between duty cycle and the servo angle is shown in the table below
Duty cycle | Angel (degree) |
---|---|
20% (min) | 0 (minimum angel) |
30% | 90 |
40% (max) | 180 (maximum angel) |
The PWM generation function is in the thread: protothread_timer which enters every 1ms. In this thread, the PWM duty cycles of four PWM waves are updated based on the structure of characters to change the posture of the arm.
As mentioned above, the position of the pen is determined by the angles of four servos and we have already built a map table from xy coordinate of the pen to the angles of the four servo. With the duty-cycle and angle transformation table above, we wrote a function called write_xy to draw a single point on position (x, y).
However, only have the βwrite pointβ function is not convenient enough to write characters. So another function called drawline is developed based on the write point function. The input of the drawline function is the xy coordinate of the start point and the end point. And it finds ten points on this line and draw a straight line in an accumulated way. The drawline function has three steps to draw a line: lift the pen from the original position, move to the start point, drop down the pen and write a line form the start point to the end point. The lifting movement can avoid draw mistakenly when we write two separated lines.
The drawline function is the foundation of the characters, we can build a character database to store the start point and end point of lines in the characters and call the drawline function to write the characters.
Another thing of writing characters is when to drop the pen down on the paper to write a character and when to move the pen above the paper in between of writing two characters. Another function called setxy is applied on this situation. Setxy function has two parameters, the first one is the target coordinate, the other one is the state of the arm when it reaches that position. The state of the arm are dropping the pen down or uplifting the pen.
Since there is no feedback or other sensors to control the speed of the servos, in software, we reduce the PWM duty cycle smoothly to avoid moving arm so fast and braking the mechanical structure of the arm.
The basic idea of writing characters on the paper is that we store the line information of 10 digits (0 - 9) and 26 letters (A - Z) as a hash table, when we receive a character input command from serial we check the table then send the coordinate information to the drawline function.For example, the data structure of letter βAβ can be: [(1,2,1 + font_size * 2,2 - font_size), (1,2,1 + font_size + 2,2 + font_size), (2,1.5,2,1.5 + font_size)] which means the start points and the end points of 3 different lines and the font_size parameter determined the size of the character. The example below shows characters βAβ when font_size = 1.
One thing needs to clarify is that the numbers in the point coordinate above is not the absolute position of the points but the relative position according to original point (0, 0). In fact the original point is the desired position where we want to write this character, and based on this position, we add relative offset and get the actual position of the line points.
The writing arm is controlled by the input instruction form serial command line. From the serial, users can set characters they want, the original x y coordinate of the characters, font size of the characters. In addition to those basic information of the characters, there are three modes of the format. Basic mode is for writing a single character, it will reset the original point coordinate to (0, 0) every time the arm finish writing a character. Line mode is for writing a word (with multiple characters). In this mode after writing a character, the arm will automatically find original point coordinate of the next character without overlapping with the previous one. The third mode is the test mode, which is designed for this course. The arm will write the course number βECE4760β automatically without user input those characters.
The PWM is 50HZ with duty cycle from 20% to 40%. And the hardware increase the amplitude of the PWM output from PIC32 to 5V. The following image is the results of the amplified PWM.
Because of the imprecision of the mechanical structure and approximation of the servo angles the accuracy of writing varies related to the distance from the robot arm. The figure below shows the range where the arm can reach and the accuracy distribution in the work range.
The figure below are character β5β and βHβ written in the line mode to test the variance of accuracy of different x y coordinate.
The figure below are character β5β in different font size and the test mode result.
In this project, we built a writing robot arm and finally make it write character : ). We learned a lot about the inverse kinematics and simplified it to apply to our project. Another achievement is that we build the whole dataset of digits and capital letters. In fact with the basic write line functions,it is flexible to add new things into our dataset.
This project is a way to transform the input characters or drawing to a electrical device to the physical doodle on the paper. It is kind of like a real-time printer but in a handwriting way. Also it is a good way to teach children how to write digits and letters (the order of writing different lines). If we increase our character data set, for example, adding Chinese characters, it offers a good way to learn to write Chinese characters.
Since mentioned above, the accuracy of this writing arm is not high enough and the range it can write neat letters is not large enough. So we are planning to improve the mathematical approximation and the inverse kinematics method we used. We are also planning to add more characters in to the dataset, for example chinese characters. And instead of draw straight lines only, we can also develop functions to draw curves.
// set the pen to x y positon
void setxy(double x, double y, int down){
int a,b;
int i;
a = getsita12(x,y);
b = getsita0(x,y);
SetDCOC2PWM(angle1[a]-500); //servo1
delay_ms(50);
SetDCOC3PWM(angle0[b]); //servo0
SetDCOC1PWM(angle2[a]); //servo2
SetDCOC4PWM(angle3[a]+300); //servo3
delay_ms(50);
if(down == 1){ //put down the pen smmothly
for(i = 0;i <= 24; i++){
SetDCOC2PWM(angle1[a]-500+20*i); //servo1
delay_ms(50);
}
}
delay_ms(1000);
}
// write a single point at (x, y)
void writexy(double x, double y){
int a,b;
a = getsita12(x,y);
b = getsita0(x,y);
SetDCOC3PWM(angle0[b]); //servo0
SetDCOC2PWM(angle1[a]); //servo1
SetDCOC1PWM(angle2[a]); //servo2
SetDCOC4PWM(angle3[a]+300); //servo3
delay_ms(100);
}
// draw a straight line accumulatedly
void drawline(double x1, double y1, double x2, double y2){
double stepx,stepy;
int i;
stepx = (x2-x1)/10.0; // calculate the step size
stepy = (y2-y1)/10.0;
writexy(x1,y1);
for(i=0;i<=10;i++){
x1 = x1 + stepx;
y1 = y1 + stepy;
writexy(x1,y1);
}
}
Hardware, inverse kinematics, software(writing function, serial command)
mechanical structure, inverse kinematics, software(PWM generation, character database)