The robot arm has 6 motors.
- Zone A: Control motor 2, 3, 4 (control three hand joints)
- Zone B: Control motor 1 (control base)
- Zone C: Control motor 5 (control rotation of gripper)
- Zone D: Control motor 6 (control gripper)
When a user touches or sweeps finger (or clicks or moves mouse), we can get coordinate (x, y). The working flow is as follows:
In case of Zone A, to calculate angles of motor 2, 3, 4, we need to do some geometric calculation. You can refer to it at the end of this page.
Server Side (Arduino code):
Once receiving a set of angles from clients, six motors moves from the current angles to the new angles gradually. Six motor should move and reach new angles at the same time. Before going into detail how to control all motors, let’s look at how to control a single motor. Suppose that we want to move a motor from current angle (angle) to new angle (new_angle). Since speed of motor is high, we should slow down it. To do like that, two following steps are repeated until the motor reach new angle:
- Move motor with a small step.
- Pause a small time and then move another step.
The following diagram illustrates the above scheme in case new angle is greater than current angle:
Wherestep_numis number of steps the motor has to take.
step and timeis predefined values. Two later ones decide the speed and smoothness. The above is just only for one robot. To make robots start to move and reach destination at the same time, we can do as follows: Six motors take the same step_num, but
step of each motor is different from each other. So we have to choose step_num in this project is maximum.
Generally, working flow of Arduino is as follows:
Let’s make a robot arm calculation into the following geometry problem:
- C is fixed
- A known point - D is the input from user
- A known point - CB, BA, AD (denoted by b, a, d respectively)
- Lengths of each arm segments Find: angles C, B, A Solution:
- Make assumption that angle B and A are the same
- Add some additional points and segment
- We knew points C and D => we can calculate the length of DC (denoted by c)
- We can also calculate the δ
- Look at triangle ABE, we can infer that AE = BE and ∠E = π - 2α.
- The Law of Cosines in triangle CDE:
- Change (1) and (2) into (3), we have:
- Simplify the above:
- Since we know a, b, c and d, solve the above quadratic equation, we can calculate the value of α. - And β = π – α - Until now we found β, let’s find γ - The Law of Cosines in triangles BDC and BDA:
- Solve this set of equations, we can calculate γ.
- So, their required angles is: (δ+γ), β and β. These are angles of motors 2, 3 and 4, respectively.
Source code inlude two files:
- RobotArmWeb.ino: Arduino code
- Remote_arm.php: Web app code, which is uploaded to PHPoC WiFi Shield or PHPoC Shield. (See instruction in this article.)
You also need to upload the image file flywheel.png to PHPoC Shield.