Ks0198 keyestudio 4DOF Robot Mechanical Arm Kit for Arduino DIY

From Keyestudio Wiki
Jump to navigation Jump to search


Ks01980.png


Kit Overview

How to DIY a robotic arm to complete multiple movements? Cool.With this keyestudio robotic arm kit, you are able to DIY your own controllable mechanical arm using ARDUINO microcontroller. It uses REV4 and 2 JoyStick modules to control the angle degree of 4 servos.
When DIY this 4DOF robot arm kit, you could get everything needed for arm installation and debugging.

There are 3 controlling methods are as follows:
1) Controlling through Wired JoyStick (included in the kit);
2) Phone Bluetooth Controlling (note: HC-06 Bluetooth module Not Included, only provide the test code for Bluetooth and APP for Android phone);
3) Wireless PS2 JoyStick Control (PS2 JoyStick module Not Included, we only provide the test code.)
You are able to get all related information in the Arm kit.
Take your brain on an inspiring journey through the world of programming. Get started now!


Kit Features

You can check out these features:

  • Detailed installation instructions
  • Detailed debugging methods, starting Arduino from entry.
  • Three controlling methods: Wired JoyStick Control; Phone Bluetooth Control; Wireless PS2 JoyStick Control.


The parameters of keyestudio TB6612FNG motor/servo drive expansion board are as follows:

  • VIN voltage: VIN = DC 7-15V
  • VIN current: 5A
  • Two-way 5V output: 5V/3A
  • TB6612FNG: VIN input DC 7-15V; average drive current 1.2A; peak current 3.2A
  • PS2 interface: compatible with Sony PS2 receiver, can be plugged directly into the expansion board.
  • Dimensions: 73*53.34mm


thumb


Part List

You can see a pretty beautiful packaging box for the arm kit, and inside the packaging you will find all the parts and screws listed below.

Please tear off the protective film on the acrylic board before mounting the kit
thumb


No. Component Quantity Picture
1 Keyestudio REV4 Main Board 1
thumb
2 keyestudio TB6612FNG motor/servo drive shield 1
thumb
3 MeArm T=3MM Black Acrylic panels 1
thumb
4 MeArm T=3MM Black Acrylic handle 1
thumb
5 MeArm Black ABS Cylindrical holder Diameter 42mm 1
thumb
6 Black 180° Servo 4
thumb
7 Keyestudio Bluetooth Module-(HC-06) 1
thumb
8 Keyestudio Joystick Module 2
thumb thumb
9 3D PS2 Joystick cap 2
thumb thumb
10 Yellow-black handle 3*40MM Phillips Screwdriver 1
thumb
11 Galvanized M2+M3 2mm thin double-headed small wrench 1
thumb
12 M3*6MM round-head cross screw 12
thumb thumb thumb thumb
13 M3*10MM round-head cross screw 22
thumb
14 M3*14MM flat-head screw 2
thumbthumb
15 M3*12MM round-head cross screw 12
thumb
16 M3*24+6MM copper pillar 4
thumb
17 M3*6mm+6mm copper pillar 10
thumb
18 M3 Stainless steel Hex Nut 22
thumb
19 M3 hexagon Lock Nut 24
thumb thumb thumb thumb thumb thumb thumb thumb thumb thumb thumb
20 M1.2x5MM Phillips tapping screw 8
thumb
21 M2x5MM Phillips tapping screw 10
thumb
22 M3 304 Stainless steel flat washer 3*7*0.5MM 10
thumb thumb thumb thumb thumb
23 M2x8MM Phillips tapping screw 2
thumb
24 M3*16MM flat-head screw 2
thumb thumb
25 Male to female 10CM jumper wire 4
thumb
26 female to female 50CM jumper wire 10
thumb
27 Black 3*100MM cable ties 1
thumb thumb thumb thumb thumb thumb thumb
28 18650 dual-cell battery case with plug lead 15CM 1
thumb


Assembly Guide

Follow the assembly steps below to build your own robot arm.

Step1: Begin with the Baseplate Assembly

(1) Firstly, you should prepare the components as follows:

  • M3*30+5MM single-pass copper pillar *4
  • M3*6mm+6mm single-pass copper pillar *4
  • M3 Hex Nut *8
  • Black Acrylic plate *1

thumb

Then, screw the copper pillars with M3 hex nuts on the black Acrylic baseplate.
thumb


(2) Then install the control board, and prepare the components as follows:

  • M3*6MM round-head screw *3
  • Keyestudio REV4 board *1
  • keyestudio TB6612FNG motor shield *1


thumb

Firstly, screw the REV4 board on the pillar using three M3*6MM round-head screws.
thumb

Then stack the motor drive shield onto the REV4 board.
thumb


(3) Completed the above assembly, let's mount the Pivot Servo Plate onto the base.

  • M3*12MM round-head screw *2
  • M3 hex lock Nut *2
  • Black 180° servo *1
  • Acrylic plate * 4


thumb

Note: before install the servo, should set the servo angle to 80 degrees.
To set the servo angle, first connect the servo to A0 of motor shield, upload the code below to REV4 board, powered on, press the reset button, servo will rotate to 80°.
thumb

Code for 80° Servo: 

int servopin=A0;//Define digital interface A0 to connect servo steering gear signal line
int myangle;     //Define angle variables
int pulsewidth; //Define pulse width variables
void setup()
{
pinMode(servopin,OUTPUT); //Set steering gear interface as Output 
} 
void servopulse(int servopin,int myangle) //Define Function
{
pulsewidth=(myangle*11)+500; //Converts the Angle to a pulse width value of 500 - 2480
digitalWrite(servopin,HIGH); //The steering gear interface level is high
delayMicroseconds(pulsewidth); //The microsecond number of the delay pulse width value
digitalWrite(servopin,LOW); //Turn the steering gear interface level to low
delay(20-pulsewidth/1000);
}
void loop()
{
servopulse(servopin,80); //Set steering gear Angle
}

//0 Degree Code: 
//		servopulse(servopin,0); 
//80 Degree Code: 
//		servopulse(servopin,80); 
//180 Degree Code:
//		servopulse(servopin,180);

Note:
Set well the servo angle and complete the below servo base plate assembly, power off the servo to avoid the angle error and make sure the servo can rotate freely. Don’t over-tighen the screws.
thumb

Adjusted well the servo motor, start to install the Servo Base Plate. Follow the marks.
thumb

Firstly mount the acrylic plate ② to the servo motor.
thumb

Then mount the two acrylic plates ④ to the servo using two M3*12MM round-head screws.
thumb

After that, screw the acrylic plate ⑤ to the servo using two M3 hex lock Nuts⑥.
thumb

You should get the detailed view shown below (pay close attention to red mark).
thumb

You should get the complete Servo Base Part.
thumb


(4) Now you should install the Servo Base part to the Base Plate:

  • M3*6MM round-head hex screw *4


thumb

Install the Servo plate to the Base Plate using four M3*6MM round-head hex screws.
thumb


Step2: Assemble Arm Middle Parts

(1) In the following section, assemble the Left Arm Servo Plate. Prepare:

  • M3*12MM round-head screw *2
  • M3 hex lock nut *2
  • M2x5MM Phillips tapping screw *1
  • M2x8MM Phillips tapping screw *1
  • Black 180° servo *1
  • White servo mount *1
  • Acrylic plate *5


thumb

Note: before install the servo plate, should set the servo angle to 180 degrees. The method is the same as 80° servo settings mentioned above. You just need to change the servopulse(servopin,80) into servopulse(servopin,180) in the code.
thumb

Now assemble the Left Arm Servo Plate as below. (pay close attention to servo direction, rocker connected to servo and marked position)
thumb

Firstly mount the acrylic plate ② to the Left servo motor.
thumb

Second, mount the two acrylic plates ④ to the servo using two M3*12MM round-head screws③. You can get the Left servo plate shown below.
thumb

Then screw the Left servo plate to acrylic plate ⑤ using two M3 hex lock Nuts⑥. Get the Left servo base plate shown below.
thumb

After that, screw the acrylic plate joint and white servo mount ⑦ to the Left servo base plate using a M2x8MM tapping screw and a M2x5MM tapping screw⑧.
thumb

Finally you should get the complete Left Arm Servo Plate. Shown below.
thumb


(2) We will now attach a servo mount to the Arm Bottom Plate.

  • M2x8MM Phillips tapping screw *2
  • White servo mount *1
  • Acrylic bottom plate *1
  • Black Cylindrical holder *1


thumb

Fix the white servo mount and black Cylindrical holder on the acrylic plate using two M2x8MM Phillips tapping screws.
thumb


(3) Completed the above assembly, move on to the Right Arm Servo Plate.

  • M3*12MM round-head screw *2
  • M3 hex lock nut *2
  • M2x5MM Phillips tapping screw *1
  • M2x8MM Phillips tapping screw *1
  • Black 180° servo *1
  • White servo mount *1
  • Black Acrylic plate *5


thumb

Note: before install the servo, should set the servo angle to 0 degrees. The method is the same as 80° servo settings mentioned above. You just need to change the servopulse(servopin,80) into servopulse(servopin,0) in the code.
thumb

Now assemble the Right Arm Servo Plate as below. (pay close attention to servo direction, rocker connected to servo and marked position)
thumb

Firstly mount the acrylic plate ② to the Right servo motor.

Second, mount the two acrylic plates ④ to the servo using two M3*12MM round-head screws③.

Then screw the Right servo plate to acrylic plate ⑤ using two M3 hex lock Nuts⑥.
thumb

After that, screw the short acrylic plate joint and white servo mount ⑦ to the Right servo motor using a M2x8MM tapping screw and a M2x5MM tapping screw⑧.
thumb thumb

Finally you should get the complete Left Servo Plate. Shown below.
thumb


(4) Next install the Servo Wrist Joint.

  • M3*10MM round-head screw *1
  • M3 hex lock nut *1
  • Acrylic plate*2


thumb

Fix the two Acrylic plates together using a M3*10MM screw and a M3 lock nut. You can get the Wrist Joint Plate shown below.
thumb


(5) The above parts are installed well. Mount the Wrist Joint and Left Arm Servo Plate together.

  • M3*10MM round-head screw *2
  • M3 hex nut *2
  • Acrylic plate*1
  • Left Arm Servo Plate
  • Wrist Joint Plate


thumb

Install both Left Arm Servo Plate and Wrist Joint plate to a single acrylic plate using two M3 hex nuts and two M3*10MM round-head screws.
thumb

You should get the complete parts shown below.
thumb


(6) Complete the whole Arm Base Parts.

  • M3*10MM round-head screw *4
  • M3 hex nut *4
  • Acrylic plate *2
  • Arm Bottom Plate⑥
  • Left Arm Servo Plate①
  • Right Arm Servo Plate⑦


thumb

Mount the Acrylic plate ② to Left Arm Servo Plate ① using a M3*10MM round-head screw and a M3 hex nut③.
thumb

Then screw another Acrylic plate ④ to Left Arm Servo Plate ① using a M3*10MM round-head screw and a M3 hex nut.
thumb

After that, snap the Arm Bottom Plate ⑥ between the both side plate of Arm BasePlate.
thumb

At last, mount the Right Arm Servo Plate ⑦ to the BasePlate using two M3*10MM round-head screws and two M3 hex nuts.
thumb

With this step completed, the Arm Base Parts is really starting to take shape!
thumb thumb


(7) Now we should install the Wrist Joint Connector between the Left and the Right Wrist Joint.

  • M3*10MM round-head screw *7
  • M3*12MM round-head screw *1
  • M3 hex lock nut *8
  • Acrylic plate*6
  • Arm Base Parts①


thumb

Fix well the Acrylic plates②③④.
thumb thumb

Then screw them to Arm Base Parts① with 7 M3*10MM screws, a M3*12MM screw and 8 M3 lock nuts. Don’t over-tighten the screws as they serve as pivot points to allow the robot arm to move. (Note: only red circle there use the M3*12MM screw, other wrist joints all use M3*10MM screw.)
thumb

Installed well.
thumb thumb


Step3: Assemble the Claw Servo Plate

(1) We are now on the final stretch, it’s time to work on the claw.

  • M3*16MM flat-head screw *2
  • M3*14MM flat-head screw *2
  • M3 hex lock nut *4
  • Black 180° servo *1
  • Acrylic plate*6

thumb

Note: before install the claw servo motor, should set the servo angle to 0 degrees. The method is the same as 80° servo settings mentioned above. You just need to change the servopulse(servopin,80) into servopulse(servopin,0) in the code.
thumb

We will start by mounting the Claw Servo Motor Plate.
thumb

Fix well the Acrylic Plates②③④, and insert the servo motor. Get the servo plate shown below.
thumb

Then tighten a Acrylic Plate ⑤ to the above servo plates using two M3*16MM flat-head screws and two M3*16MM round-head screws and four M3 hex nuts.
thumb


(2) Next is to install the Gripper Plate.

  • M1.2x5MM Phillips tapping screw *4
  • M2x5MM Phillips tapping screw *4
  • White servo mount *1
  • Gripper Acrylic plate *3


thumb

Firstly fix the white servo mount ② onto acrylic plate ① using four M1.2x5MM tapping screws③.
thumb

Then fix the acrylic plate ④ onto another longer acrylic plate ⑤ using four M2x5MM tapping screws.
thumb


(3) Mount the grippers onto the claw servo plate.

  • M3*12MM round-head screw *2
  • M3 hex lock nut *2
  • M3 flat washer *7
  • Servo screw *1①
  • Claw servo plate②
  • Gripper plate③
  • Gripper plate④
  • Gripper Acrylic plate*1⑤


thumb

Place the four flat washers onto a M3*12MM round-head screw connected to the Claw servo plate.
thumb

Then fix a M3 hex lock nut to the Gripper plate③. Next, place the Gripper plate onto Claw servo plate.
thumb

After that, place another Gripper plate⑤ onto Claw servo plate using 3 flat washers and a M3*12MM round-head screw.
thumb

Finally, screw the Gripper plate ④ onto the shaft of servo motor with a servo screw.
thumb

Claw servo plate completed!
thumb


Step4: Final Assembly

(1) Now we will assemble all the finished parts together

  • M3*10MM round-head screw *3
  • M3 hex lock nut *1
  • M2x5MM tapping screw *1
  • Servo Base Plate①
  • Arm Middle Parts②
  • Claw Servo Plate③


thumb

Mount the Claw Servo Plate ③ to Arm Middle Parts ② using three M3*10MM round-head screws and a M3 hex lock nut.
thumb thumb thumb

After that, screw them to Servo Base Plate ① using a M2x5MM tapping screw.
thumb thumb

The robot arm kit is nearly complete! It’s now time to connect the servo to drive shield.
Hookup Guide:
thumb thumb


Step5: Assemble the Joystick Control Plate

(1) The last step is to assemble the Joystick control plate

  • M3*6MM round-head screw *4
  • M3*6+6mm single-pass copper pillar *6
  • M3 hex nut *6
  • Joystick module *2
  • Black Joystick plate *1


thumb

Mount 6 single-pass copper pillars onto Joystick plate with 6 M3 hex nuts.
thumb

Then screw the two Joystick modules onto the Acrylic plate using four M3*6MM round-head screws.
thumb


(2) Finally connect the Joystick module to drive shield using female-to-female jumper wires.


thumb

Hookup Guide:
thumb

Congrats! Your robot arm kit is complete!
thumb
Done wiring, power and program everything up to give your robot arm kit a try!


Robot Arm Projects

Project 1: Getting Started with ARDUINO

1) Core Part of Robot

When it comes to using the REV4 as core of our robot, the REV4 is the best board to get started with electronics and coding. If this is your first experience tinkering with the platform, the REV4 is the most robust board you can start playing with.


KS0470-1-1(2).jpg

Well, let's at first have a look at this REV4 board.

KS0341 引脚标图.jpg

KS0313 5.1-1.png USB Connection

Arduino board can be powered via USB connector. Or you can program the board via the USB port.

KS0313 5.1-2.png DC Power Jack

Arduino board can be supplied with power from the DC power jack

KS0313 5.1-3.png Voltage Regulator

To control the voltage provided to the Arduino board, as well as to stabilize the DC voltage used by the processor and other components.

KS0313 5.1-4.png Crystal Oscillator

How does Arduino calculate time? by using a crystal oscillator. The number printed on the top of the Arduino crystal is 16.000H9H. It tells us that the frequency is 16,000,000 Hertz or 16MHz.

KS0313 5.1-5.png Arduino RESET

You can reset your Arduino board, for example, start the program from the very beginning. Firstly, use the RESET button（17）. Or you can connect an external reset button to Arduino pin 5 labeled RESET

KS0313 5.1-6.png Pin Header（3.3V，5V，GND，Vin

KS0313 5.1-7.png3.3V - provides 3.3V output voltage
KS0313 5.1-8.png5V - provides 5V output voltage
Using 3.3 volts and 5 volts, most components can normally operate with Arduino board together.
KS0313 5.1-9.pngGND（Ground pins）- two GND headers on Arduino, each of which can be used for circuit ground.
KS0313 5.1-10.pngVin - You can supply an external power (like AC power supply) through this pin to Arduino board.

KS0313 5.1-11.png Analog Pins

Arduino REV4 board has 6 analog inputs, labeled A0 through A5. These pins can read the signal from analog sensors (such as humidity sensor or temperature sensor), and convert it into the digital value that can read by microcontrollers)

KS0313 5.1-12.png Microcontroller

Each Arduino board has its own microcontroller. You can regard it as the brain of your board.
The main IC (integrated circuit) on the Arduino is slightly different from the panel pair. Microcontrollers are usually from ATMEL. Before you load a new program from the Arduino IDE, you must know what IC is on your board. This information can be checked at the top of IC.

KS0313 5.1-13.png ICSP (In-Circuit Serial Programming) Header

In most case, ICSP is the AVR, an Arduino micro-header consisting of MOSI, MISO, SCK, RESET, VCC, and GND.It is often called the SPI (serial peripheral interface) and can be considered an "extension" of the output.In fact, slave the output devices under the SPI bus host.

KS0313 5.1-14.png Power LED Indicator

Powering the Arduino, LED on means that your circuit board is correctly powered on. If LED is off, connection is wrong.

KS0313 5.1-15.png TX and RX LED

Onboard you can find two labels:　RX(receive ) and TX (transmit)
First appear on digital pin 0 and 1 for serial communication;
Besides, the RX LED on the board will flash in different speed when serial data is being transmitted. The flash speed depends on the baud rate set by board. And RX LED will also flash during the receiving process.

KS0313 5.1-16.png Digital I/O

Arduino REV4 has 14 digital input/output pins (of which 6 can be used as PWM outputs). These pins can be configured as digital input pin to read the logic value (0 or 1). Or used as digital output pin to drive different modules like LED, relay, etc. The pin labeled “〜” can be used to generate PWM.

KS0313 5.1-17.png AREF

Reference voltage( 0-5V) for the analog inputs. Used with analogReference().


Installing Arduino IDE

When you get the REV4 development board, first you should install the software and driver of Arduino. You can see all the Arduino software versions from the link below:
https://www.arduino.cc/en/Main/OldSoftwareReleases#1.5.x
Or you can browse the ARDUINO website at this link, https://www.arduino.cc, pop up the following interface.
KS0313-1.png

Then click the SOFTWARE on the browse bar, you will have two options ONLINE TOOLS and DOWNLOADS.
KS0313-2.png

Click DOWNLOADS, it will appear the latest software version of ARDUINO 1.8.5 shown as below.
KS0313-3.png

In this software page, on the right side you can see the version of development software for different operating systems. So ARDUINO has a rather powerful compatibility. You should download the software that is compatible with the operating system of your computer.
In our project, we will take WINDOWS system as an example here. There are also two options under Windows system, one is installed version, the other is non-installed version. For simple installed version, first click Windows Installer, you will get the following page.

KS0313-4.png

KS0313-5.png

This way you just need to click JUST DOWNLOAD, then click the downloaded file to install it.
For non-installed version, first click Windows ZIP file, you will also get the pop-up interface as the above figure.
Click JUST DOWNLOAD, and when the ZIP file is downloaded well to your computer, you can directly unzip the file and then click the icon of ARDUINO program to start it.


Installing Arduino (Windows)

Install Arduino with the exe. Installation package
thumb

Click“I Agree”to see the following interface.
thumb

Click “Next”. Pop up the interface below.
thumb

You can press Browse… to choose an installation path or directly type in the directory you want.
Then click “Install” to initiate installation.
thumb

Wait for the installing process, if appear the interface of Window Security, just continue to click Install to finish the installation.
thumb

All right, up to now, you have completed the Arduino setup! The following icon will appear on your PC desktop.
Ks0313图片1.png

Double-click the icon of Arduino to enter the desired development environment shown as below.
717.png


Installing Driver

Next, we will introduce the driver installation of REV4 development board. The driver installation may have slight differences in different computer systems. So in the following let’s move on to the driver installation in the WIN 7 system.
The Arduino folder contains both the Arduino program itself and the drivers that allow the Arduino to be connected to your computer by a USB cable. Before we launch the Arduino software, you are going to install the USB drivers.
Plug one end of your USB cable into the Arduino and the other into a USB socket on your computer.
When you connect REV4 board to your computer at the first time, right click the icon of your “Computer” —>for “Properties”—> click the “Device manager”, under “Other Devices”, you should see an icon for“Unknown device” with a little yellow warning triangle next to it. This is your Arduino.

Driver 1.png

Then right-click on the device and select the top menu option (Update Driver Software...) shown as the figure below..
Driver 2.png

It will then be prompted to either “Search Automatically for updated driversoftware” or “Browse my computer for driver software”. Shown as below. In this page, select “Browse my computer for driver software”.
Driver 3.png

After that, select the option to browseand navigate to the “drivers” folder of Arduino installation.
KS0286-4.png

Click “Next” and you may get a security warning, if so, allow the software to be installed. Shown as below.
Driver 5.png

Once the software has been installed, you will get a confirmation message. Installation completed, click “Close”.
Driver 6.png

Up to now, the driver is installed well. Then you can right click “Computer” —>“Properties”—>“Device manager”, you should see the device as the figure shown below.
Driver 7.png


2) Example Use of ARDUINO IDE

STEP 1: Open Arduino
In the previous, we have introduced the driver installation of REV4 development board. So this time let’s first have basic understanding of the development environment of ARDUINO. After that, you will learn how to upload the program to Arduino board.
First of all, open the unzipped folder of ARDUINO development software and click icon of ARDUINO to open the software, as the figure shown below.
Arduino folder.png


STEP 2: Build Projects
When open the Arduino software, you will have two options as below:

  • Build a new project
  • Open an exiting project example

If you want to build a new project, please select “File”→then click “New”, you will see the software interface as follows.

Arduino 1-8-5 new.png0313 箭头.pngArduino 1-8-5 new2.png

If you want to open an example project, please select File→Example→Basics→Blink. Shown below.

Arduino 1-8-5 example.png 0313 箭头.pngArduino 1-8-5 example2.png


STEP 3: Select Arduino Board
On the Arduino software, you should click Tools→Board , select the correct board. Here in our tutorial we should select Arduino UNO. Shown as below.
Arduino 1-8-5 board.png


STEP 4: Select Serial Port
If you are not sure which port is correct, at first directly open the Control Panel of your computer, then click to open Device Manager, you can check the COM port here. Shown as below.

Driver 7.png

Then you should click Tools→Serial Port. It may be COM3 or higher (COM1 and COM2 are usually reserved as hardware serial port).
Arduino 1-8-5 port.png


STEP 5: Upload the Code to Your Board
Before showing you how to upload the code to your board, first of all let me introduce the function of each icon on the Tool bar of Arduino IDE. Look at the picture showed below.
图片1- arduino toolbar.png

IDE 1.png Verify/Compile Check the code for errors
IDE 2.png Upload Upload the current Sketch to the Arduino
IDE 3.png New Create a new blank Sketch
IDE 4.png Open Show a list of Sketches
IDE 5.png Save Save the current Sketch
IDE 6.png Serial Monitor Display the serial data being sent from the Arduino



Project 2: Joint Rotation and Pin Control

1)Joint Rotation and Servo Angle Settings

Name 0° 180°
Servo 1（baseplate） Rotate toward the rightmost Rotate toward the leftmost
Servo 2（right side） Rocker arm connected to Servo 2 draws back stretch out
Servo 3（left side） Rocker arm connected to Servo 3 stretches out draw back
Servo 4（clamp claw） closed opened


2)Pin Control

Name IO Pin
Servo 1 (baseplate) A1
Servo 2 (right side) A0
Servo 3（left side） 6
Servo 4（clamp claw） 9
Right Joystick X A2
Right Joystick Y A5
Right Joystick Z (key) 7
Left Joystick X A3
Left Joystick Y A4
Left Joystick Z 8
D1/DAT of PS2 12
D0/CMD of PS2 10
CE/SEL of PS2 11
CLK of PS2 13


Project 3: JoyStick Controlled Robot Arm

1)Servo Control

How to adjust the Servo Angel

Hookup Guide:

Hookup 1.jpg


Code to Note:
In the previous assembly, we have set the square wave and servo angle. So we should use the servo libraries to control the angle of each servo. The test result is the same. Before using, place the Servo folder inside the libraries folder of Arduino IDE directory, then open the Arduino IDE, and the libraries are able to use.
You can download all the libraries for test code from the link:
https://drive.google.com/open?id=1HtyzuNT59Av5QP8nO0kv6syQsoK1fUOw


Test Code 1: 

#include <Servo.h>
Servo myservo;  // create servo object to control a servo

void setup()
{
  Serial.begin(9600); 
  delay(1000);
}

void loop() 
{
  myservo.attach(A0);  // modify each pin to adjust 
  myservo.write(0);  // angle value  
  delay(1000);
}


Test Result:
Stack the drive shield onto REV4 board and connect the servo motor, upload well the code. Powered on, press the reset button, the servo will automatically rotate to 0°.


Automatic Turning

Description:
In the previous section, you have learned to set the Servo angle. In fact, we just need to continually change the angle of 4 Servo motors, thus make the 4DOF robot arm operate different motions.
Hookup Guide:

Hookup 2.png


Test Code 2: 

#include <Servo.h>
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;
int pos1=80, pos2=60, pos3=130, pos4=0; 

void setup()
{
  myservo1.attach(A1);  // attaches the servo on pin 9 to the servo object
  myservo2.attach(A0);
  myservo3.attach(6);
  myservo4.attach(9);
  
  myservo1.write(pos1);
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);
}

void loop() 
{
  // turn right
  for(pos1;pos1>0;pos1--)
  {
    myservo1.write(pos1);
    delay(5);      // delay 5ms（used to adjust the servo speed）
  }
  delay(1000);
  
  // open the claw
 for(pos4;pos4<100;pos4++)
 {
   myservo4.write(pos4);
 }
  delay(1000);
  
  // right servo rotates to 100 degrees
  for(pos2;pos2>50;pos2--)
  {
    myservo2.write(pos2);
    delay(5);
  }
  // left servo rotates to 5 degrees
  for(pos3;pos3>50;pos3--)
  {
    myservo3.write(pos3);
    delay(5);
  }
  delay(1500);
  
  // close the claw 
  for(pos4;pos4>0;pos4--)
  {
    myservo4.write(pos4);
  }
  delay(1000);
  
  // left servo rotates to100 degrees, rocker arm lifts.
  for(pos3;pos3<120;pos3++)
  {
    myservo3.write(pos3);
    delay(5);
  }
  delay(1000);

  // turn to left 
  for(pos1;pos1<180;pos1++)
  {
    myservo1.write(pos1);
    delay(5);
  }
  delay(1000);

  // Lower the arm 
  for(pos3;pos3>50;pos3--)
  {
    myservo3.write(pos3);
    delay(5);
  }
  delay(1000);

  // open the claw
  for(pos4;pos4<100;pos4++)
  {
    myservo4.write(pos4);
  }
  delay(1000);

  // lift up the arm 
  for(pos3;pos3<120;pos3++)
  {
    myservo3.write(pos3);
    delay(5);
  }
  delay(1000);

  // close the claw 
  for(pos4;pos4>0;pos4--)
  {
    myservo4.write(pos4);
  }
  delay(1000);
}


Test Result:
Stack the drive shield onto REV4 board and connect the servo motor, upload well the code. Powered on, press the reset button, the robot arm will realize a cyclic motion. Turn to the right, stretch out the arm, lower, the claw is clamped; then the arm is retracted, raised, turned to the left, stretched out, lowered and the claw is opened.
After that, return to the right and continue to repeat the motion.


2)Read the JoyStick Value

Description:
The sensor’s pin X, Y are for analog sensor, so directly read the measured analog value. Pin Z is a digital button, first should set the pin to Input status and then read the measured value 1 (pressed down) or 0 (not press). Check out the value printed on the serial monitor.

Hookup Guide:

0198.png


Test Code 3:

const int right_X = A2; // define the right X pin to A2 
const int right_Y = A5; // define the right Y pin to A5 
const int right_key = 7; //define the right key pin to 7（that is the value Z）

const int left_X = A3; //define the left X pin to A3
const int left_Y = A4;  // define the left Y pin to A4
const int left_key = 8; //define the left key pin to 8（that is the value Z） 

void setup() 
{
  pinMode(right_key, INPUT);   // set the right/left key to INPUT 
  pinMode(left_key, INPUT);
  Serial.begin(9600); //  set the baud rate to 9600
}

void loop() 
{
  int x1,y1,z1;  // define the variable, used to save the joystick value it reads
  int x2,y2,z2;
  
  x1 = analogRead(right_X); // read the value of right X 
  y1 = analogRead(right_Y);  // read the value of right Y 
  z1 = digitalRead(right_key);  //// read the value of right Z 
  
  x2 = analogRead(left_X);  // read the value of left X
  y2 = analogRead(left_Y);  // read the value of left Y 
  z2 = digitalRead(left_key);  // read the value of left Z 
  
  Serial.print("right_X = "); // on the serial monitor, print out right_X = 
  Serial.println(x1 ,DEC);  // print out the value of right X and line wrap
  Serial.print("right_Y = ");
  Serial.println(y1 ,DEC);
  //Serial.print("right_key = ");
  //Serial.println(z1 ,DEC);
 // Serial.println("**********right**********");
  /*Serial.print("left_X = ");
  Serial.println(x2 ,DEC);
  Serial.print("left_Y = ");
  Serial.println(y2 ,DEC);
  Serial.print("left_key = ");
  Serial.println(z2 ,DEC);
  Serial.println("*********left***********");*/
  delay(200);
}


Test Result:
Hook it up and upload well the code. Connect the REV4 to computer using a USB cable, then open the serial monitor and set the baud rate to 9600, you should see the analog value of the right Joystick pin X,Y.
Result 3.jpg


3)Dual-JoyStick Controlling

Description:
In the previous section, we have introduced how to use 4 Servo to control the robot arm. Next, combine those two experiments. Use two Joystick modules to control 4DOF robot arm realize different motions.
At first, set the boot posture. The Joystick control is shown as below table.

Right Joystick Servo Left Joystick Servo
X1<50 Servo 1 gradually reduces to 0° (push the right joystick to the right, the servo that controls the arm rotation turns right, and stops at 0° ) X2<50 Servo 4 gradually reduces to 0° (push the left joystick to the right, the claw is closed)
X1>1000 Servo 1 gradually increases to 180° (push the right joystick to the left, the servo that controls the arm rotation turns left, and stops at 180° ) X2>1000 Servo 4 gradually increases to 180° (push the left joystick to the left, the claw opens)
Y1>1000 Servo 2 gradually reduces to 0° ( that is, lift up the robot upper arm) Y2>1000 Servo 3 gradually reduces to 35° ( that is, stretch out the robot lower arm)
Y1<50 Servo 2 gradually reduces to 180° ( that is, lower the robot upper arm) Y2<50 Servo 3 gradually increases to 180° ( that is, draw back the robot lower arm)


Hookup Guide:

Hookup 2.png 0198.png

Test Code 4: 

#include <Servo.h>  // add the servo libraries 
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;  

int pos1=80, pos2=60, pos3=130, pos4=0;  // define the variable of 4 servo angle,and assign the initial value (that is the boot posture angle value) 

const int right_X = A2; // define the right X pin to A2 
const int right_Y = A5; // define the right Y pin to A5
const int right_key = 7; // define the right key pin to 7（that is the value of Z）

const int left_X = A3; // define the left X pin to A3
const int left_Y = A4;  // define the left X pin to A4
const int left_key = 8; //define the left key pin to 8（that is the value of Z）

int x1,y1,z1;  // define the variable, used to save the joystick value it read.
int x2,y2,z2;


void setup() 
{
  
  // boot posture 
  myservo1.write(pos1);  
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);

  pinMode(right_key, INPUT);   // set the right/left key to INPUT
  pinMode(left_key, INPUT);
  Serial.begin(9600); //  set the baud rate to 9600
}

void loop() 
{
  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  // set the control pin of servo 2 to A0
  myservo3.attach(6);   //set the control pin of servo 3 to D6
  myservo4.attach(9);   // set the control pin of servo 4 to D9

  x1 = analogRead(right_X); //read the right X value 
  y1 = analogRead(right_Y);  // read the right Y value 
  z1 = digitalRead(right_key);  //// read the right Z value 
  
  x2 = analogRead(left_X);  //read the left X value
  y2 = analogRead(left_Y);  //read the left Y value 
  z2 = digitalRead(left_key);  // read the left Z value	
  //delay(5);  // lower the speed overall

  // claw
  zhuazi();
  // rotate
  zhuandong();
  // upper arm
  xiaobi();
  //lower arm
  dabi();
}

//claw
void zhuazi()
{
    //claw
  if(x2<50) // if push the left joystick to the right
  {
      pos4=pos4-2;  //current angle of servo 4 subtracts 2（change the value you subtract, thus change the closed speed of claw）
      //Serial.println(pos4);
      myservo4.write(pos4);  // servo 4 operates the action, claw is gradually closed.
      delay(5);
      if(pos4<2)  // if pos4 value subtracts to 2, the claw in 37 degrees we have tested is closed.
      {            //（should change the value based on the fact）
        pos4=2;   // stop subtraction when reduce to 2
      }
   }
  if(x2>1000) //// if push the left joystick to the left 
  {
      pos4=pos4+8; // current angle of servo 4 plus 8（change the value you plus, thus change the open speed of claw）
      //Serial.println(pos4);
      myservo4.write(pos4); // servo 4 operates the motion, the claw gradually opens.
      delay(5);
      if(pos4>108)  // limit the largest angle when open the claw  
      {
        pos4=108;
      }
  }
}
//******************************************************
 // turn
void zhuandong()
{
  if(x1<50)  // if push the right joystick to the right
  {
    pos1=pos1-1;  //pos1 subtracts 1
    myservo1.write(pos1);  // servo 1 operates the motion, the arm turns right.
    delay(5);
    if(pos1<1)   // limit the angle when turn right
    {
      pos1=1;
    }
  }
  if(x1>1000)  // if push the right joystick to the let
  {
    pos1=pos1+1;  //pos1 plus 1
    myservo1.write(pos1);  // arm turns left 
    delay(5);
    if(pos1>180)  // limit the angle when turn left 
    {
      pos1=180;
    }
  }
}

//**********************************************************/
//upper arm
void xiaobi()
{
    if(y1>1000) // if push the right joystick upward
  {
    pos2=pos2-1;
    myservo2.write(pos2); // the upper arm will lift
    delay(5);
    if(pos2<0)  // limit the lifting angle
    {
      pos2=0;
    }
  }
  if(y1<50)  // if push the right joystick downward
  {
    pos2=pos2+1;  
    myservo2.write(pos2);  // upper arm will go down  
    delay(5);
    if(pos2>180)  // limit the angle when go down
    {
      pos2=180;
    }
  }
}

//*************************************************************/
// lower arm
void dabi()
{
    if(y2<50)  // if push the left joystick upward 
  {
    pos3=pos3+1;
    myservo3.write(pos3);  // lower arm will stretch out 
    delay(5);
    if(pos3>180)   // limit the stretched angle 
    {
      pos3=180;
    }
  }
  if(y2>1000)  // if push the left joystick downward
  {
    pos3=pos3-1;
    myservo3.write(pos3);  // lower arm will draw back
    delay(5);
    if(pos3<35)  // limit the retracted angle 
    {
      pos3=35;
    }
  }
}


Test Result:
Upload the code to main board and stack the shield onto it. Wire it up, 4DOF robot arm will keep the initial position, push the two Joysticks to operate the robot arm.


4)Add Memory Function

Memorize One Posture

Description:
In the previous section, use the analog value of pin X,Y of 2 Joystick modules to control the robot arm.
In the following experiment, we add a memory function for the robot arm, making it remember a posture then operate. Set 4 variables for saving the angle value of 4 servos, use the Joystick to control a posture. Press the key Z1 of right Joystick to save the angle value of 4 servos; press the key Z2 of left Joystick to make the servo operate a posture saved in the variable.


Hookup Guide:

Hookup 2.png 0198.png


Test Code 5:

#include <Servo.h>  // add servo libraries
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;  
int pos1=80, pos2=60, pos3=130, pos4=0;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)

const int right_X = A2; // define the right X pin to A2 
const int right_Y = A5; // define the right Y pin to A3 
const int right_key = 7; // define the right key pin to 7（that is Z value）

const int left_X = A3; // define the left X pin to A3
const int left_Y = A4;  // define the left Y pin to A4
const int left_key = 8; // define the left key pin to 8（that is Z value）

int x1,y1,z1;  // define the variable, used to save the joystick value.
int x2,y2,z2;
int s1,s2,s3,s4;

void setup() 
{
  // boot posture
  myservo1.write(pos1);  
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);
    
  
  pinMode(right_key, INPUT);   // set the right/left key to INPUT 
  pinMode(left_key, INPUT);
  Serial.begin(9600); //  set the baud rate to 9600
}

void loop() 
{
  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  // set the control pin of servo 2 to A0
  myservo3.attach(6);   //set the control pin of servo 3 to D6
  myservo4.attach(9);   //set the control pin of servo 4 to D9
  
  x1 = analogRead(right_X); // read the right X value
  y1 = analogRead(right_Y);  // read the right Y value
  z1 = digitalRead(right_key);  //// read the right key Z value
  
  x2 = analogRead(left_X);  // read the left X value
  y2 = analogRead(left_Y);  //read the left Y value
  z2 = digitalRead(left_key);  //read the left key Z value
  //delay(5);  // reduce the speed overall
  if(z1==1)  // if the right joystick key is pressed
  {
    delay(10);  // delay for eliminating shake 
    if(z1==1)    // judge again if the right key is pressed
    {
      s1=myservo1.read();  // read the angle value of each servo 
      s2=myservo2.read();
      s3=myservo3.read();
      s4=myservo4.read();
    }
  }
  if(z2==1) // if the left key is pressed
  {
    delay(10);
    if(z2==1)
    {
        pos1=myservo1.read();  // record the angle value of 4 servos in current posture
        pos2=myservo2.read();
        pos3=myservo3.read();
        pos4=myservo4.read();

        if(pos1<s1)  // if angle of servo 1 is smaller than variable s1 value
        {
          while(pos1<s1)  //while loop，rotate the servo to the position of the value stored in the array.
          {
            myservo1.write(pos1); // servo 1 operates the motion
            pos1++;  //pos1 plus 1
            delay(5);  // delay for 5ms，controlling the rotation speed of servo. 
          }
          
        }
        else  // if angle of servo 1 is greater than the value stored in array 1.
        {
          while(pos1>s1)     //while loop,rotate the servo to the position of the value stored in the array.
          {
            myservo1.write(pos1);  //  servo 1 operates the motion
            pos1--;     //pos1 subtracts 1
            delay(5);   // delay for 5ms，controlling the rotation speed of servo.
          }
        }
      //*************************************************  
      // the explanation is the same as servo 1 
        if(pos2<s2)
        {
          while(pos2<s2)
          {
            myservo2.write(pos2);
            pos2++;
            delay(5);
          }
        }
        else
        {
          while(pos2>s2)
          {
            myservo2.write(pos2);
            pos2--;
            delay(5);
          }
        }
     //*************************************************  
      // the explanation is the same as servo 1
        if(pos3<s3)
        {
          while(pos3<s3)
          {
            myservo3.write(pos3);
            pos3++;
            delay(5);
          }
        }
        else
        {
          while(pos3>s3)
          {
            myservo3.write(pos3);
            pos3--;
            delay(5);
          }
        }
      //*************************************************  
      // the explanation is the same as servo 1
        if(pos4<s4)
        {
          while(pos4<s4)
          {
            myservo4.write(pos4);
            pos4++;
            delay(5);
          }
        }
        else
        {
          while(pos4>s4)
          {
            myservo4.write(pos4);
            pos4--;
            delay(5);
          }
        }
        
    }
  }
  
  //claw
  zhuazi();
  //turn
  zhuandong();
  // upper arm 
  xiaobi();
  // lower arm
  dabi();
}


//claw
void zhuazi()
{
    //claw
  if(x2<50) // if push the left joystick to the right
  {
      pos4=pos4-2;  // current angle of servo 4 subtracts 2（change the value you subtract, thus change the closed speed of claw）
      //Serial.println(pos4);
      myservo4.write(pos4);  //servo 4 operates the action, claw is gradually closed
      delay(5);
      if(pos4<2)  // if pos4 value subtracts to 2, the claw in 37 degrees we have tested is closed.）
      {            //（should change the value based on the fact）
        pos4=2;   // stop subtraction when reduce to 2
      }
   }
  if(x2>1000) //// if push the left joystick to the left 
  {
      pos4=pos4+8; // current angle of servo 4 plus 8（change the value you plus, thus change the open speed of claw）
      //Serial.println(pos4);
      myservo4.write(pos4); //  servo 4 operates the motion, the claw gradually opens. 
      delay(5);
      if(pos4>90)  // limit the largest angle when open 
      {
        pos4=90;
      }
  }
}
//******************************************************
 // turn
void zhuandong()
{
  if(x1<50)  // if push the right joystick to the right 
  {
    pos1=pos1-1;  //pos1 subtracts 1
    myservo1.write(pos1);  // servo 1 operates the motion, the arm turns right. 
    delay(5);
    if(pos1<1)   //  limit the angle when turn right
    {
      pos1=1;
    }
  }
  if(x1>1000)  // if push the right joystick to the left
  {
    pos1=pos1+1;  //pos1 plus 1
    myservo1.write(pos1);  // robot arm turns left
    delay(5);
    if(pos1>180)  // limit the angle when turn left 
    {
      pos1=180;
    }
  }
}

//**********************************************************/
// upper arm
void xiaobi()
{
    if(y1>1000) // if push the right joystick upward 
  {
    pos2=pos2-1;
    myservo2.write(pos2); // the upper arm will lift 
    delay(5);
    if(pos2<0)  // limit the lifting angle
    {
      pos2=0;
    }
  }
  if(y1<50)  // if push the right joystick downward 
  {
    pos2=pos2+1;  
    myservo2.write(pos2);  // the upper arm will go down 
    delay(5);
    if(pos2>180)  // limit the angle when go down 
    {
      pos2=180;
    }
  }
}

//*************************************************************/
// lower arm
void dabi()
{
  if(y2>1000)  // if push the left joystick upward 
  {
    pos3=pos3-1;
    myservo3.write(pos3);  // the lower arm will stretch out
    delay(5);
    if(pos3<35)  //  limit the stretched angle
    {
      pos3=35;
    }
  }
  
  if(y2<50)  // if push the left joystick downward
  {
    pos3=pos3+1;
    myservo3.write(pos3);  // the lower arm will draw back 
    delay(5);
    if(pos3>180)   // limit the retracted angle 
    {
      pos3=180;
    }
  }  
}


Test Result:
Wire it up, stack the shield onto REV4 board, upload the code. Powered on, press the key Z1 of right Joystick to save the angle value of 4 servos control. Press the key Z2 of left Joystick to operate a servo posture saved in the variable.


Memorize Several Postures

Description:
In the previous section, we have set the angle of 4 servos to make the robot arm remember and operate a posture. To extend the experiment, next make it remember several postures, at most 10 (you can set it in the code), then make 4DOF robot arm continually operate the posture in memory. That is, make robot arm memorize a group of actions, and you can set the memorizing speed in the code.


Hookup Guide:

Hookup 2.png 0198.png


Test Code 6: 

#include <Servo.h>  // add the servo libraries
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;  
int pos1=80, pos2=60, pos3=130, pos4=0;  //  define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)

const int right_X = A2; //  define the right X pin to A2  
const int right_Y = A5; // define the right Y pin to A5 
const int right_key = 7; // define the right key pin to 7（that is Z value）

const int left_X = A3; //  define the left X pin to A3
const int left_Y = A4;  //  define the left Y pin to A4
const int left_key = 8; // define the left key pin to 8（that is Z value） 

int x1,y1,z1;  //define the variable, used to save the joystick value.
int x2,y2,z2;
int s1,s2,s3,s4;
int jiyi1[10];  // define 4 array, separately used to save the angle of four servo.
int jiyi2[10];  //（array length is 10，namely can save angle data of 0~10 servo ）
int jiyi3[10];  // if need to save more data, just change the number 10 to be more larger number.
int jiyi4[10];
int i=0; // for loop
int j=0; // save the last value of i

void setup() 
{
  // boot posture
  myservo1.write(pos1);  
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);
    
  
  pinMode(right_key, INPUT);   // set the right/left key to INPUT
  pinMode(left_key, INPUT);
  Serial.begin(9600); //  set baud rate to 9600
}

void loop() 
{
  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  // set the control pin of servo 2 to A0
  myservo3.attach(6);   // set the control pin of servo 3 to D6
  myservo4.attach(9);   // set the control pin of servo 4 to D9
  
  x1 = analogRead(right_X); // read the right X value 
  y1 = analogRead(right_Y);  // read the right Y value  
  z1 = digitalRead(right_key);  // read the right Z value  
  
  x2 = analogRead(left_X);  // read the left X value  
  y2 = analogRead(left_Y);  // read the left Y value 
  z2 = digitalRead(left_key);  // read the left Z value 
  //delay(5);  // reduce the speed overall 
  if(z1==1)   // if the right joystick key is pressed 
  {
    delay(10);   // delay for eliminating shake 
    if(z1==1)   // judge again if the right key is pressed
    {
      s1=myservo1.read();    // read the angle value of each servo  
      delay(100);
      Serial.println(s1);
      s2=myservo2.read();
      delay(100);
      Serial.println(s2);
      s3=myservo3.read();
      delay(100);
      Serial.println(s3);
      s4=myservo4.read();
      delay(100);
      Serial.println(s4);
      
      jiyi1[i]=s1;   // Save the read servo value to the array sequentially
      jiyi2[i]=s2;
      jiyi3[i]=s3;
      jiyi4[i]=s4;
      i++;     //i value plus 1
      j=i;     // assign the last value of i to j
      delay(100);
      Serial.println(i);   // on the serial monitor, print out the value i
    }
  }
      
  if(z2==1) // if the left joystick key is pressed 
  {
    delay(10);
    if(z2==1)   // judge again if the left key is pressed 
    {
      i=0;  // assign i to 0，prepare for the next memory
      pos1 = myservo1.read();    // memorize the angle value of 4 servo posture
      pos2 = myservo2.read();
      pos3 = myservo3.read();
      pos4 = myservo4.read();

      for(int k=0;k<j;k++)   // loop for j times, perform all actions saved.
      {
        if(pos1<jiyi1[k])    // if the current servo 1 angle is less than the value stored in array 1.
        {
          while(pos1<jiyi1[k])  //while loop, make servo turn to the position of value stored in the array.
          {
            myservo1.write(pos1);  // servo 1 performs the action
            delay(5);    // delay 5ms，controlling the servo rotating speed
            pos1++;  //pos1 plus 1
            //Serial.println(pos1);
          }
        }
        else    // if the current servo 1 angle is greater than the value stored in array 1.
        {
         while(pos1>jiyi1[k])     //while loop, make servo turn to the position of value stored in the array.
          {
            myservo1.write(pos1);   // servo 1 performs the action
            delay(5);    //delay 5ms，controlling the servo rotating speed
            pos1--;     //pos1 subtracts 1
            //Serial.println(pos1);
          }
        }
//***************************************************************
  //the explanation is the same as the previous servo 
        if(pos2<jiyi2[k])
        {
          while(pos2<jiyi2[k])
          {
            myservo2.write(pos2);
            delay(5);
            pos2++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos2>jiyi2[k])
          {
            myservo2.write(pos2);
            delay(5);
            pos2--;
            //Serial.println(pos1);
          }
        }
 //***************************************************************
  // the explanation is the same as the previous servo  

        if(pos3<jiyi3[k])
        {
          while(pos3<jiyi3[k])
          {
            myservo3.write(pos3);
            delay(5);
            pos3++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos3>jiyi3[k])
          {
            myservo3.write(pos3);
            delay(5);
            pos3--;
            //Serial.println(pos1);
          }
        }
 //***************************************************************
  //the explanation is the same as the previous servo 

        if(pos4<jiyi4[k])
        {
          while(pos4<jiyi4[k])
          {
            myservo4.write(pos4);
            delay(5);
            pos4++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos4>jiyi4[k])
          {
            myservo4.write(pos4);
            delay(5);
            pos4--;
            //Serial.println(pos1);
          }
        }
      }
    }
  }
  
  //claw
  zhuazi();
  //turn
  zhuandong();
  //upper arm 
  xiaobi();
  // lower arm 
  dabi();
}


//claw
void zhuazi()
{
    //claw
  if(x2<50) // if push the left joystick to the right 
  {
      pos4=pos4-2;  // angle of servo 4, subtract 2 (change the value you subtract, thus change the closed speed of claw)
      //Serial.println(pos4);
      myservo4.write(pos4);  // servo 4 operates the motion and claw is gradually closed. 
      delay(5);
      if(pos4<2)  // if pos4 value subtracts to 2, the claw in 37 degrees we have tested is closed.）  
      {            //（should change the value based on the fact）
        pos4=2;   // stop subtraction when reduce to 2 
      }
   }
  if(x2>1000) //// if push the left joystick to the left
  {
      pos4=pos4+8; // current angle of servo 4 plus 8（change the value you plus, thus change the open speed of claw）
      //Serial.println(pos4);
      myservo4.write(pos4); //  servo 4 operates the action, claw gradually opens.
      delay(5);
      if(pos4>90)  // limit the largest angle opened
      {
        pos4=90;
      }
  }
}
//******************************************************
 // turn 
void zhuandong()
{
  if(x1<50)  // if push the right joystick to the right 
  {
    pos1=pos1-1;  //pos1 subtracts 1
    myservo1.write(pos1);  // servo 1 operates the motion and robot arm turns right 
    delay(5);
    if(pos1<1)   // limit the angle when turn right  
    {
      pos1=1;
    }
  }
  if(x1>1000)  // if push the right joystick to the left 
  {
    pos1=pos1+1;  //pos1 plus 1
    myservo1.write(pos1);  // robot arm turns left 
    delay(5);
    if(pos1>180)  // limit the angle when turn left 
    {
      pos1=180;
    }
  }
}

//**********************************************************/
// upper arm 
void xiaobi()
{
    if(y1>1000) // if push the right joystick upward 
  {
    pos2=pos2-1;
    myservo2.write(pos2); // the upper arm will lift 
    delay(5);
    if(pos2<0)  // limit the lifting angle 
    {
      pos2=0;
    }
  }
  if(y1<50)  // if push the right joystick downward
  {
    pos2=pos2+1;  
    myservo2.write(pos2);  // the upper arm will go down 
    delay(5);
    if(pos2>180)  // limit the declining angle 
    {
      pos2=180;
    }
  }
}

//*************************************************************/
// lower arm 
void dabi()
{
  if(y2>1000)  // if push the left joystick upward
  {
    pos3=pos3-1;
    myservo3.write(pos3);  // the lower arm will stretch out 
    delay(5);
    if(pos3<35)  // limit the stretched angle
    {
      pos3=35;
    }
  }
  
  if(y2<50)  // if push the left joystick downward 
  {
    pos3=pos3+1;
    myservo3.write(pos3);  // the lower arm will draw back 
    delay(5);
    if(pos3>180)   // limit the retracted angle
    {
      pos3=180;
    }
  }  
}


Test Result:
Wire it up, stack the shield onto REV4 board, upload the code. Powered on, press the key Z1 of right Joystick to save the angle value of 4 servos. Press down the key Z1 to memorize different postures, at most 10 postures in the code. If need to memorize more postures, you can set it in the code. When memorizing successfully, press down the key Z2 of left Joystick to make the robot arm carry out several postures stored successively.


Memorize Several Postures And Loop

Description:
In the previous section, we have introduced how to make 4DOF robot arm to memorize and perform a group of posture. Furthermore, let’s extend one more loop function. That is, when robot arm performs all the memorized actions, it will not stop, and continue to repeat those actions.
In the following experiment, press the key Z1, 4DOF robot arm will exit the looping action. Press the key Z1 again, start to memorize the posture, after that, press the key Z2 to loop the memorized actions.

Hookup Guide:

Hookup 2.png 0198.png


Test Code 7: 

#include <Servo.h>  //  add the servo libraries
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;  
int pos1=80, pos2=60, pos3=130, pos4=0;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
const int right_X = A2; //  define the right X pin to A2 
const int right_Y = A5; //  define the right Y pin to A5 
const int right_key = 7; // define the right key pin to 7（that is Z value）

const int left_X = A3; //define the left X pin to A3
const int left_Y = A4;  // define the left Y pin to A4
const int left_key = 8; // define the left key pin to 8（that is Z value）

int x1,y1,z1;  // define the variable, used to save the joystick value.
int x2,y2,z2;
int s1,s2,s3,s4;
int jiyi1[20];  //define 4 array, separately used to save the angle of four servo.
int jiyi2[20];  // （array length is 20，namely can save angle data of 0~20 servo）
int jiyi3[20];  //if need to save more data, just change the number 20 to be more larger number.
int jiyi4[20];
int i=0;  // for loop
int j=0;  //  save the last value of i

void setup() 
{
  // boot posture
  myservo1.write(pos1);  //turn servo 1 to 90 degrees
  delay(1000);
  myservo2.write(pos2);  // turn servo 2 to 90 degrees
  myservo3.write(pos3);  // turn servo 3 to 120 degrees
  myservo4.write(pos4);  // turn servo 4 to 35 degrees 
  delay(1500);
    
  
  pinMode(right_key, INPUT);   // set the right/left key to INOUT
  pinMode(left_key, INPUT);
  Serial.begin(9600);   // set the baud rate to 9600
}

void loop() 
{
  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  // set the control pin of servo 2 to A0
  myservo3.attach(6);   //set the control pin of servo 3 to D6
  myservo4.attach(9);   // set the control pin of servo 4 to D9
  
  x1 = analogRead(right_X); // read the right X value
  y1 = analogRead(right_Y);  //read the right Y value
  z1 = digitalRead(right_key);  //read the right Z value 
  
  x2 = analogRead(left_X);  //  read the left X value
  y2 = analogRead(left_Y);  // read the left Y value
  z2 = digitalRead(left_key);  // read the left Z value
  //delay(5);  // delay, used to reduce the joystick value read, that is reduce the whole speed.
  if(z1==1)   // if the joystick right key is pressed
  {
    delay(10);  // delay for eliminating shake 
    if(z1==1)   // judge again if the right key is pressed
    {
      s1=myservo1.read();  // read the angle value of servo 1 and assign it to s1
      delay(100);
      Serial.println(s1); // print out the angle value of servo 1 on the serial monitor
      s2=myservo2.read();  //  read the angle value of servo 2 and assign it to s2
      delay(100);
      Serial.println(s2);
      s3=myservo3.read();   //  read the angle value of servo 3 and assign it to s3
      delay(100);
      Serial.println(s3);
      s4=myservo4.read();   // read the angle value of servo 4 and assign it to s4 
      delay(100);
      Serial.println(s4);
       
      jiyi1[i]=s1;  // Save the read servo value to the array sequentially
      jiyi2[i]=s2;
      jiyi3[i]=s3;
      jiyi4[i]=s4;
      i++;   //i plus 1
      j=i;   // assign the last value of i to j
      delay(100);  // delay 100ms
      Serial.println(i);  // print out the value i
    }
  }
  if(z2==1) //  if the left joystick key is pressed
  {
    delay(10);  // delay for eliminating shake 
    if(z2==1)   //judge again if the left key is pressed 
    {
      pos1 = myservo1.read();  // memorize the angle value of 4 servo posture
      pos2 = myservo2.read();
      pos3 = myservo3.read();
      pos4 = myservo4.read();
      
      while(z2==1)  // loop, make the arm repeat the action.
      {
        for(int k=1;k<j;k++)  //for loop, perform all the stored actions.
        {
          if(pos1<jiyi1[k])   // if the current servo 1 angle is less than the value stored in array 1.
          {
            while(pos1<jiyi1[k])  //while loop, make servo turn to the position of value stored in the array.
            {
              myservo1.write(pos1);  //servo 1 performs the action 
              delay(5);   //delay 5ms，controlling the servo rotating speed.
              pos1++;   //pos1 plus 1
              //Serial.println(pos1);
            }
          }
          else   //if the current servo 1 angle is greater than the value stored in array 1.
          {
           while(pos1>jiyi1[k])    //while loop, make servo turn to the position of value stored in the array.
            {
              myservo1.write(pos1);  //servo 1 performs the action
              delay(5);   //delay 5ms，controlling the servo rotating speed.
              pos1--;   //pos1 subtracts 1
              //Serial.println(pos1);
            }
          }
  //***************************************************************
  //the explanation is the same as the previous servo. 
          if(pos2<jiyi2[k])
          {
            while(pos2<jiyi2[k])
            {
              myservo2.write(pos2);
              delay(5);
              pos2++;
              //Serial.println(pos1);
            }
          }
          else
          {
           while(pos2>jiyi2[k])
            {
              myservo2.write(pos2);
              delay(5);
              pos2--;
              //Serial.println(pos1);
            }
          }
   //*********************************************
   //the explanation is the same as the previous servo.
          if(pos3<jiyi3[k])
          {
            while(pos3<jiyi3[k])
            {
              myservo3.write(pos3);
              delay(5);
              pos3++;
              //Serial.println(pos1);
            }
          }
          else
          {
           while(pos3>jiyi3[k])
            {
              myservo3.write(pos3);
              delay(5);
              pos3--;
              //Serial.println(pos1);
            }
          }
//*********************************************
//the explanation is the same as the previous servo.
          if(pos4<jiyi4[k])
          {
            while(pos4<jiyi4[k])
            {
              myservo4.write(pos4);
              delay(5);
              pos4++;
              //Serial.println(pos1);
            }
          }
          else
          {
           while(pos4>jiyi4[k])
            {
              myservo4.write(pos4);
              delay(5);
              pos4--;
              //Serial.println(pos1);
            }
          }
        }
//************************************************************
// for exiting the loop
            z1 = digitalRead(right_key);  // read the right Z value
            if(z1==1)  // if the right key is pressed
            {
              delay(10);  //eliminate the shake
              if(z1==1)  // if the key z1 is pressed
              {
                pos1=jiyi1[(j-1)];  // assign the last angle value saved in array to pos
                pos2=jiyi2[(j-1)];  // for exiting the loop, still access to joystick control.
                pos3=jiyi3[(j-1)];  
                pos4=jiyi4[(j-1)];
                i=0;  // assign i as 0，prepare for saving the angle value using array
                z2=0; // assign z2 as 0，for exiting the while loop
                break;  //exit the current loop
              }
            }
//********************************************************
      }
    }
  }
  
  //claw
  zhuazi();
  //turn
  zhuandong();
  //upper arm
  xiaobi();
  //lower arm
  dabi();
}


//claw
void zhuazi()
{
    //claw
  if(x2<50) // if push the left joystick to the right 
  {
      pos4=pos4-2;  // angle of servo 4, subtract 2 (change the value you subtract, thus change the closed speed of claw)
      //Serial.println(pos4);
      myservo4.write(pos4);  //  servo 4 operates the motion and claw is gradually closed.
      delay(5);
      if(pos4<2)  // if pos4 value subtracts to 2, the claw in 37 degrees we have tested is closed.）
      {            //（should change the value based on the fact）
        pos4=2;   //stop subtraction when reduce to 2
      }
   }
  if(x2>1000) ////if push the left joystick to the left
  {
      pos4=pos4+8; //current angle of servo 4 plus 8（change the value you plus, thus change the open speed of claw）
      //Serial.println(pos4);
      myservo4.write(pos4); // servo 4 operates the action, claw gradually opens.
      delay(5);
      if(pos4>90)  //limit the largest angle opened
      {
        pos4=90;
      }
  }
}
//******************************************************
 //turn
void zhuandong()
{
  if(x1<50)  //if push the right joystick to the right 
  {
    pos1=pos1-1;  //pos1 subtracts 1
    myservo1.write(pos1);  // servo 1 performs the action, the robot arm turns right.
    delay(5);
    if(pos1<1)   // limit the right turning angle 
    {
      pos1=1;
    }
  }
  if(x1>1000)  // if push the right joystick to the left
  {
    pos1=pos1+1;  //pos1 plus 1
    myservo1.write(pos1);  //the robot arm turns left.
    delay(5);
    if(pos1>180)  //limit the left turning angle 
    {
      pos1=180;
    }
  }
}

//**********************************************************/
// upper arm
void xiaobi()
{
    if(y1>1000) // if push the right joystick upward
  {
    pos2=pos2-1;
    myservo2.write(pos2); // the robot arm will lift
    delay(5);
    if(pos2<0)  // limit the lifting angle 
    {
      pos2=0;
    }
  }
  if(y1<50)  // if push the right joystick downward
  {
    pos2=pos2+1;  
    myservo2.write(pos2);  // the robot arm will go down
    delay(5);
    if(pos2>180)  // limit the declining angle 
    {
      pos2=180;
    }
  }
}

//*************************************************************/
// lower arm 
void dabi()
{
  if(y2>1000)  // if push the left joystick upward
  {
    pos3=pos3-1;
    myservo3.write(pos3);  // the lower arm will stretch out 
    delay(5);
    if(pos3<35)  // limit the stretched angle 
    {
      pos3=35;
    }
  }
  
  if(y2<50)  // if push the right joystick downward
  {
    pos3=pos3+1;
    myservo3.write(pos3);  // the lower arm will draw back
    delay(5);
    if(pos3>180)   // limit the retraction angle 
    {
      pos3=180;
    }
  }  
}


Test Result:
Wire it up, stack the shield onto REV4 board, upload the code. Powered on, press the key Z1 of right Joystick to save the angle value of 4 servos. Press down the key Z1 to memorize different postures, at most 10 postures in the code. If need to memorize more postures, can set it in the code.
When memorizing successfully, press down the key Z2 of left Joystick to make the robot arm carry out several postures stored successively, looping.
Long press the key Z1, 4DOF robot arm will exit the looping action. Press the key Z1 again, start to memorize the posture, after that, press the key Z2 to loop the memorized actions.


Project 4: Bluetooth Controlled Robot Arm

1) Principle of Bluetooth Control


Bluetooth, as the name implies, blue teeth, and he is not used to bite people, but a wireless data transmission method. Bluetooth technology is a wireless standard technology that enables short-range data exchange among fixed devices, mobile devices, and personal area networks of buildings (UHF radio waves in the ISM band of 2.4 to 2.485 GHz).
There are two kinds of commonly used Bluetooth module on the market, HC-05 and HC-06 models.
The difference between them is that the HC-05 is a master-slave one. It can not only make small reports to its own “master”, but also can receive the command given to it. The HC-06 can only work in slave mode, which can only accept the superior command. For instance, in many cases you may want to be an overbearing man, letting subordinates obey the order without any nonsense. In such situation, it is enough to use the HC-06 module.
Note: HC-06 Bluetooth module is only compatible with Android system, not support iOS.

thumb


Specification Parameters:

  • 1) Bluetooth Protocol: Bluetooth 2.1+ EDR Standard
  • 2) USB Protocol: USB v1.1/2.0
  • 3) Operating Frequency: 2.4GHz ISM Frequency Band
  • 4) Modulation Mode: Gauss Frequency Shift Keying
  • 5) Transmit Power: ≤ 4dBm, Second Stage
  • 6) Sensitivity: ≤-84dBm at 0.1% Bit Error Rate
  • 7) Transmission Speed: 2.1Mbps(Max)/160 kbps(Asynchronous)；1Mbps/1Mbps(Synchronous)
  • 8) Safety Feature: Authentication and Encryption
  • 9) Supported Configuration: Bluetooth Serial Port (major and minor)
  • 10) Supply Voltage: DC 5V
  • 11) Operating Temperature: -20 to 55℃


2)Bluetooth Control Key Test


Description:
Next, we are going to introduce the use method for HC-06 Bluetooth module. To easily use the HC-06 Bluetooth module to control the robot arm, we particularly design the APP control. Shown below.
thumb
You can download the APP from the link:
https://drive.google.com/open?id=10U7EWyCyb5bGUxWm3DXRXGKEm1UQGU4Q
There are 10 control keys on the App. When connect well the HC-06 Bluetooth module to Android phone using our APP, press the control key, Android phone will receive a corresponding value.
When programming, you can set the function for the corresponding value. So in the experiment, we will test each key to get the corresponding value.

Pay special attention to:
1) First should pair Bluetooth in the phone settings. Connect the Bluetooth to shield, powered up, LED on the Bluetooth module flashes, then open the mobile Bluetooth, search for a Bluetooth device.
If find a new address, pair and enter 1234, paired successfully, you should see the address name change to HC-06.
2) Must first upload the test program successfully, then connect the Bluetooth, pair and connect the App. Otherwise, fail to upload the program.
thumb


Hookup Guide:

thumb


Test Code 8:

void setup() 
{
  Serial.begin(9600);     // set the serial baud rate to 9600
}

void loop()
{
  char val;    // define a variable, used to receive the value read from Bluetooth.
  if(Serial.available())     // if receive the value 
  {
    val = Serial.read();       // assign the value read to val
    Serial.println(val);
  }
}


Test Result:
Upload the code, hookup well and power up. After connecting the Bluetooth APP, open the serial monitor and set the baud rate to 9600, press down the control key on the APP, you should see the corresponding value. Shown below.

key value
one 1
two 2
three 3
four 4
five 5
A A
B B
C C
D D
E E


3)Bluetooth Controlling Arm

Description:
In this experiment, we combine a Android APP and HC-06 Bluetooth module to control the robot arm.
The controlling method are shown below.

key Corresponding arm action
one The lower arm stretches out
two The clamp claw opens
three The clamp claw is closed
four The lower arm draws back
five -
A The upper arm lifts up
B The arm turns left
C The arm turns right
D The upper arm goes down
E -


Hookup Guide:

thumb thumb


Test Code 9:

#include <Servo.h>  // add the servo libraries
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;
int pos1=80, pos2=60, pos3=130, pos4=0;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)
char val;

void setup()
{
   // boot posture
  myservo1.write(pos1);  
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);

  Serial.begin(9600); //  set the baud rate to 9600
}

void loop() 
{
  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  // set the control pin of servo 2 to A0
  myservo3.attach(6);   // set the control pin of servo 3 to D6
  myservo4.attach(9);   // set the control pin of servo 4 to D9

  if(Serial.available())   // if receive the data
  {
    val=Serial.read();    // read the received data
    Serial.println(val);
   
    switch(val)   
    {
      case 'B':  T_left();  break;   // execute the corresponding function when receive the value 
      case 'C':  T_right();  break;
      case 'A':  RF();  break;
      case 'D':  RB();  break;
      case '2':  ZK();  break;
      case '3':  ZB();  break;
      case '1':  LF();  break;
      case '4':  LB();  break;
    }
  }
}
//**************************************************
// turn left
void T_left()
{
    pos1=pos1+1;
    myservo1.write(pos1);
    delay(5);
    if(pos1>180)
    {
      pos1=180;
    }
}
//turn right 
void T_right()
{
    pos1=pos1-1;
    myservo1.write(pos1);
    delay(5);
    if(pos1<1)
    {
      pos1=1;
    }
}
//********************************************
//open the claw
void ZK()
{
      pos4=pos4-2;
      Serial.println(pos4);
      myservo4.write(pos4);
      delay(5);
      if(pos4<2)
      {
        pos4=0;
      }
}
// close the claw
void ZB()
{
    pos4=pos4+8;
      Serial.println(pos4);
      myservo4.write(pos4);
      delay(5);
      if(pos4>108)
      {
        pos4=108;
      }
}

//******************************************
// the upper arm will lift up 
void RF()
{
    pos2=pos2-1;
    myservo2.write(pos2);
    delay(5);
    if(pos2<0)
    {
      pos2=0;
    }
}
// the upper arm will go down 
void RB()
{
    pos2=pos2+1;
    myservo2.write(pos2);
    delay(5);
    if(pos2>180)
    {
      pos2=180;
    }
}

//***************************************
// the lower arm will stretch out 
void LB()
{
  pos3=pos3+1;
    myservo3.write(pos3);
    delay(5);
    if(pos3>180)
    {
      pos3=180;
    }
}
// the lower arm will draw back 
void LF()
{
  pos3=pos3-1;
    myservo3.write(pos3);
    delay(5);
    if(pos3<35)
    {
      pos3=35;
    }
}


Test Result:
Upload the code, connect it up and power on, after connecting the Bluetooth APP, press the key to control the robot arm do commanded motions.


Project 5: PS2 Controlled Robot Arm (Extension)

1)PS2 Joypad Key Test

Description:
On the drive shield there is a PS2 Joystick connector, which is easy for you to control the 4DOF robot arm using the PS2 Joypad. But you need to purchase it by yourself because the PS2 Joypad is not included in the kit.
When use the PS2 Joypad to control the robot arm, first need to get the corresponding character of each key on the PS2 Joypad.
So this experiment will help you test the character of each key on the PS2 Joypad.
After connecting the Joypad, should upload the test program on Arduino IDE. But before testing, should place the PS2X_lib folder inside the libraries folder of Arduino IDE directory.
Uploading the code, open the serial monitor, connect the PS2 Joypad. When press down the key, you should see the corresponding character on the monitor.


Test Code 10:

 
#include <PS2X_lib.h>  //for v1.6

/******************************************************************
 * set pins connected to PS2 controller:
 *   - 1e column: original 
 *   - 2e colmun: Stef?
 * replace pin numbers by the ones you use
 ******************************************************************/
#define PS2_DAT        13  //14    
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
#define PS2_CLK        12  //17

/******************************************************************
 * select modes of PS2 controller:
 *   - pressures = analog reading of push-butttons 
 *   - rumble    = motor rumbling
 * uncomment 1 of the lines for each mode selection
 ******************************************************************/
//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning 
//you must always either restart your Arduino after you connect the controller, 
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

void setup(){
 
  Serial.begin(57600);
  
  delay(300);  //added delay to give wireless ps2 module some time to startup, before configuring it
   
  //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
  
  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
  
  if(error == 0){
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
	if (pressures)
	  Serial.println("true ");
	else
	  Serial.println("false");
	Serial.print("rumble = ");
	if (rumble)
	  Serial.println("true)");
	else
	  Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }  
  else if(error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
   
  else if(error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if(error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
  
//  Serial.print(ps2x.Analog(1), HEX);
  
  type = ps2x.readType(); 
  switch(type) {
    case 0:
      Serial.print("Unknown Controller type found ");
      break;
    case 1:
      Serial.print("DualShock Controller found ");
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
	case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
   }
}

void loop() {
  /* You must Read Gamepad to get new values and set vibration values
     ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
     if you don't enable the rumble, use ps2x.read_gamepad(); with no values
     You should call this at least once a second
   */  
  if(error == 1) //skip loop if no controller found
    return; 
  
  if(type == 2){ //Guitar Hero Controller
    ps2x.read_gamepad();          //read controller 
   
    if(ps2x.ButtonPressed(GREEN_FRET))
      Serial.println("Green Fret Pressed");
    if(ps2x.ButtonPressed(RED_FRET))
      Serial.println("Red Fret Pressed");
    if(ps2x.ButtonPressed(YELLOW_FRET))
      Serial.println("Yellow Fret Pressed");
    if(ps2x.ButtonPressed(BLUE_FRET))
      Serial.println("Blue Fret Pressed");
    if(ps2x.ButtonPressed(ORANGE_FRET))
      Serial.println("Orange Fret Pressed"); 

    if(ps2x.ButtonPressed(STAR_POWER))
      Serial.println("Star Power Command");
    
    if(ps2x.Button(UP_STRUM))          //will be TRUE as long as button is pressed
      Serial.println("Up Strum");
    if(ps2x.Button(DOWN_STRUM))
      Serial.println("DOWN Strum");
 
    if(ps2x.Button(PSB_START))         //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if(ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");
    
    if(ps2x.Button(ORANGE_FRET)) {     // print stick value IF TRUE
      Serial.print("Wammy Bar Position:");
      Serial.println(ps2x.Analog(WHAMMY_BAR), DEC); 
    } 
  }
  else { //DualShock Controller
    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
    
    if(ps2x.Button(PSB_START))         //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if(ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");      

    if(ps2x.Button(PSB_PAD_UP)) {      //will be TRUE as long as button is pressed
      Serial.print("Up held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    }
    if(ps2x.Button(PSB_PAD_RIGHT)){
      Serial.print("Right held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    }
    if(ps2x.Button(PSB_PAD_LEFT)){
      Serial.print("LEFT held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    }
    if(ps2x.Button(PSB_PAD_DOWN)){
      Serial.print("DOWN held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    }   

    vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
    if (ps2x.NewButtonState()) {        //will be TRUE if any button changes state (on to off, or off to on)
      if(ps2x.Button(PSB_L3))
        Serial.println("L3 pressed");
      if(ps2x.Button(PSB_R3))
        Serial.println("R3 pressed");
      if(ps2x.Button(PSB_L2))
        Serial.println("L2 pressed");
      if(ps2x.Button(PSB_R2))
        Serial.println("R2 pressed");
      if(ps2x.Button(PSB_TRIANGLE))
        Serial.println("Triangle pressed");        
    }

    if(ps2x.ButtonPressed(PSB_CIRCLE))               //will be TRUE if button was JUST pressed
      Serial.println("Circle just pressed");
    if(ps2x.NewButtonState(PSB_CROSS))               //will be TRUE if button was JUST pressed OR released
      Serial.println("X just changed");
    if(ps2x.ButtonReleased(PSB_SQUARE))              //will be TRUE if button was JUST released
      Serial.println("Square just released");     

    if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { //print stick values if either is TRUE
      Serial.print("Stick Values:");
      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX  
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_LX), DEC); 
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_RY), DEC); 
      Serial.print(",");
      Serial.println(ps2x.Analog(PSS_RX), DEC); 
    }     
  }
  delay(50);  
}


Test Result:
Stack the drive shield onto REV4 board and upload the code. Connecting the PS2 Joypad, open the serial monitor and set the baud rate to 57600. When press down the key or push the rocker, you should see the corresponding character showed on the monitor.


2)PS2 Joypad Control

Description:
In the previous section, we have showed how to use Joystick module to control the robot arm. It is almost the same for you to control the 4DOF robot arm using the PS2 Joypad.

Hookup Guide:

thumb

Test Code 11:

 
#include <PS2X_lib.h>

PS2X ps2x; // create PS2 Controller Class


//right now, the library does NOT support hot pluggable controllers, meaning 
//you must always either restart your Arduino after you connect the controller, 
//or call config_gamepad(pins) again after connecting the controller.
int error = 0; 
byte vibrate = 0;

#include <Servo.h>  // add the servo libraries 
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;

int pos1=80, pos2=60, pos3=130, pos4=0;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)

void setup(){
 Serial.begin(57600);

// boot posture
  myservo1.write(pos1);  
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);
  
 error = ps2x.config_gamepad(13,11,10,12);   //setup GamePad(clock, command, attention, data) pins, check for error
 
 if(error == 0){
   Serial.println("Found Controller, configured successful");
   Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
  Serial.println("holding L1 or R1 will print out the analog stick values.");
  Serial.println("Go to www.billporter.info for updates and to report bugs.");
 }
   
  else if(error == 1)
   Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
   
  else if(error == 2)
   Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
   
   //Serial.print(ps2x.Analog(1), HEX);
 
 
 ps2x.enableRumble();              //enable rumble vibration motors
 ps2x.enablePressures();           //enable reading the pressure values from the buttons. 
  

  
}

void loop(){
   /* You must Read Gamepad to get new values
   Read GamePad and set vibration values
   ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
   if you don't enable the rumble, use ps2x.read_gamepad(); with no values
   
   you should call this at least once a second
   */

  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  // set the control pin of servo 2 to A0
  myservo3.attach(6);   // set the control pin of servo 3 to D6
  myservo4.attach(9);   // set the control pin of servo 4 to D9
  
 if(error != 0)
  return; 
  
  ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed
  
  if(ps2x.Button(PSB_START))                   //will be TRUE as long as button is pressed
       Serial.println("Start is being held");
  if(ps2x.Button(PSB_SELECT))
       Serial.println("Select is being held");
       
       
   if(ps2x.Button(PSB_PAD_UP)) {         //will be TRUE as long as button is pressed
     Serial.print("Up held this hard: ");
     Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    }
    if(ps2x.Button(PSB_PAD_RIGHT)){
     Serial.print("Right held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    }
    if(ps2x.Button(PSB_PAD_LEFT)){
     Serial.print("LEFT held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    }
    if(ps2x.Button(PSB_PAD_DOWN)){
     Serial.print("DOWN held this hard: ");
   Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    }   

  
    vibrate = ps2x.Analog(PSAB_BLUE);        //this will set the large motor vibrate speed based on 
                                            //how hard you press the blue (X) button    
  
  if (ps2x.NewButtonState())               //will be TRUE if any button changes state (on to off, or off to on)
  {
      if(ps2x.Button(PSB_R3))
        
       Serial.println("R3 pressed");
     
       
      if(ps2x.Button(PSB_L3))
       Serial.println("L3 pressed");
      
      if(ps2x.Button(PSB_L2))
       Serial.println("L2 pressed");
      if(ps2x.Button(PSB_R2))
       Serial.println("R2 pressed");
      if(ps2x.Button(PSB_GREEN))
       Serial.println("Triangle pressed");
       
  }   
       
  
  if(ps2x.ButtonPressed(PSB_RED))             //will be TRUE if button was JUST pressed
       Serial.println("Circle just pressed");
       
  if(ps2x.ButtonReleased(PSB_PINK))             //will be TRUE if button was JUST released
       Serial.println("Square just released");     
  
  if(ps2x.NewButtonState(PSB_BLUE))            //will be TRUE if button was JUST pressed OR released
       Serial.println("X just changed");    

    //转动
    zhuandong();
    //爪子
    zhuazi();
    //大臂
    dabi();
    //小臂
    xiaobi();
    
  if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) // print stick values if either is TRUE
  {
      Serial.print("Stick Values:");
      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX  
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_LX), DEC); 
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_RY), DEC); 
      Serial.print(",");
      Serial.println(ps2x.Analog(PSS_RX), DEC); 
  }    
  delay(5);    
}
//********************************************************************
// turn
void zhuandong()
{
//turn right
   if(ps2x.Analog (PSS_RX) > 200)  // if push the right joystick to the right 
    {
      //Serial.println(ps2x.Analog(PSS_RX), DEC);
      pos1=pos1-1;    //pos1 subtracts 1
      myservo1.write(pos1);   // servo 1 executes the action, the arm will turn right.
      // delay(5);
      if(pos1<1)   // limit the right turning angle 
      {
        pos1=1;
      }
     }
      // turn left
     if(ps2x.Analog (PSS_RX) < 50)    // if push the right joystick to the left
     {
       //Serial.println(ps2x.Analog(PSS_RX), DEC);
       pos1=pos1+1;   //pos1 plus 1
       myservo1.write(pos1);     // the arm turns left 
       // delay(5);
       if(pos1>180)     // limit the left turning angle
       {
         pos1=180;
       }
     }
 }
 //**********************************************************************  
 // upper arm 
void xiaobi()
{
  //upper arm front
   if(ps2x.Analog(PSS_RY)<50)  // if push the right joystick upward
   {
      pos2=pos2-1;
      myservo2.write(pos2);    // the upper arm will lift
      delay(5);
      if(pos2<0)    // limit the lifting angle
      {
        pos2=0;
      }
    }
    //upper arm back
    if(ps2x.Analog(PSS_RY)>200)    // if push the right joystick downward
    {
      pos2=pos2+1;
      myservo2.write(pos2);   // the upper arm will go down
      delay(5);
      if(pos2>180)  // limit the declining angle
      {
        pos2=180;
      }
    }
}
//***************************************************************
void zhuazi()
{
// close the claw
   if(ps2x.Analog(PSS_LX)>220)   // if push the left joystick to the right
   {
      pos4=pos4-1;
      Serial.println(pos4);
      myservo4.write(pos4);  // servo 4 carries out the action and the claw is gradually closed.
      delay(5);
      if(pos4<0)   // if pos4 value subtracts to 37, the claw in 37 degrees we have tested is closed.）
      {
        pos4=0;
      }
    }
    // open the claw
    if(ps2x.Analog(PSS_LX)<10)   // if push the left joystick to the left
    {
      pos4=pos4+8;
      Serial.println(pos4);
      myservo4.write(pos4);   // servo 4 carries out the action and the claw is gradually opened
      delay(5);
      if(pos4>108)    // limit the maximum opening angle 
      {
        pos4=108;
      }
    }
} 
//*********************************************************
void dabi()
{
  // lower arm front
  if(ps2x.Analog(PSS_LY)>200)  // if push the left joystick upward
  {
    pos3=pos3+1;
    myservo3.write(pos3);  // the lower arm will stretch out
    delay(5);
    if(pos3>180)   // limit the stretched angle
    {
      pos3=180;
    }
   }
  if(ps2x.Analog(PSS_LY)<10)    //if push the left joystick downward
  {
    pos3=pos3-1;
    myservo3.write(pos3);   // the lower arm will draw back
    delay(5);
    if(pos3<35)    // limit the retracted angle
    {
      pos3=35;
    }
   }
}


Test Result:
Stack the shield onto REV4 board and upload the code. Powered on and connected the PS2 Joypad, you can use the PS2 Joypad to control the robot arm actions.



3)PS2 Controlling Posture Memory

Description:
In the previous experiment, we have showed how to use Joystick module to control the robot arm memorize several postures. Now we replace the joystick module with PS2 Joypad. The program thought is almost the same.


Hookup Guide:

thumb

Test Code 12:

#include <PS2X_lib.h>
#include <Servo.h>  // add the servo libraries
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;  

int pos1=80, pos2=60, pos3=130, pos4=0;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning 
//you must always either restart your Arduino after you conect the controller, 
//or call config_gamepad(pins) again after connecting the controller.
int error = 0; 
byte vibrate = 0;

int s1,s2,s3,s4;
int jiyi1[20];  // define four array, separately used to save the angle of 4 servos.
int jiyi2[20];
int jiyi3[20];
int jiyi4[20];
int i=0;
int j=0;

void setup()
{
 Serial.begin(57600);

 // boot posture
  myservo1.write(pos1);  
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);

  
 error = ps2x.config_gamepad(13,11,10,12);   //setup GamePad(clock, command, attention, data) pins, check for error
 
 if(error == 0){
   Serial.println("Found Controller, configured successful");
   Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
  Serial.println("holding L1 or R1 will print out the analog stick values.");
  Serial.println("Go to www.billporter.info for updates and to report bugs.");
 }
   
  else if(error == 1)
   Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
   
  else if(error == 2)
   Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
   
   //Serial.print(ps2x.Analog(1), HEX);
 
 
 ps2x.enableRumble();              //enable rumble vibration motors
 ps2x.enablePressures();           //enable reading the pressure values from the buttons. 
  

  
}

void loop()
{
  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  // set the control pin of servo 2 to A0
  myservo3.attach(6);   // set the control pin of servo 3 to D6
  myservo4.attach(9);   // set the control pin of servo 4 to D9
  
 if(error != 0)
  return; 
  
  ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed
  
  if(ps2x.Button(PSB_START))                   //will be TRUE as long as button is pressed
       Serial.println("Start is being held");
  if(ps2x.Button(PSB_SELECT))
       Serial.println("Select is being held");
       
       
   if(ps2x.Button(PSB_PAD_UP)) {         //will be TRUE as long as button is pressed
     Serial.print("Up held this hard: ");
     Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    }
    if(ps2x.Button(PSB_PAD_RIGHT)){
     Serial.print("Right held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    }
    if(ps2x.Button(PSB_PAD_LEFT)){
     Serial.print("LEFT held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    }
    if(ps2x.Button(PSB_PAD_DOWN)){
     Serial.print("DOWN held this hard: ");
   Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    }   

  
    vibrate = ps2x.Analog(PSAB_BLUE);        //this will set the large motor vibrate speed based on 
                                            //how hard you press the blue (X) button    
  
  if (ps2x.NewButtonState())               //will be TRUE if any button changes state (on to off, or off to on)
  {
      if(ps2x.Button(PSB_R3))
      {
        //Serial.println("R3 pressed");
        // record
        s1=myservo1.read();
        delay(100);
        Serial.println(s1);
        s2=myservo2.read();
        delay(100);
        Serial.println(s2);
        s3=myservo3.read();
        delay(100);
        Serial.println(s3);
        s4=myservo4.read();
        delay(100);
        Serial.println(s4);
      
        jiyi1[i]=s1;  // save the servo value read in the array sequentially
        jiyi2[i]=s2;
        jiyi3[i]=s3;
        jiyi4[i]=s4;
        i++;
        j=i;
       // delay(100);
       Serial.println(i);
      }
       
      if(ps2x.Button(PSB_L3))
      {
        //Serial.println("L3 pressed");
        i=0;
   //执行
       pos1 = myservo1.read();
       pos2 = myservo2.read();
       pos3 = myservo3.read();
       pos4 = myservo4.read();

      for(int k=0;k<j;k++)  //for loop, to execute all the stored actions
      { 
        if(pos1<jiyi1[k])  //if the current servo 1 angle is less than the value stored in array 1.
        {
          while(pos1<jiyi1[k])  //while loop, make servo turn to the position of value stored in the array.
          {
            myservo1.write(pos1);   // servo 1 executes the action
            delay(5);   // delay 5ms，controlling the rotating speed of servo
            pos1++;
            //Serial.println(pos1);
          }
        }
        else   //if the current servo 1 angle is greater than the value stored in array 1.
        {
         while(pos1>jiyi1[k])  //while loop, make servo turn to the position of value stored in the array.
          {  
            myservo1.write(pos1);   // servo 1 executes the action
            delay(5);    //delay 5ms，controlling the rotating speed of servo
            pos1--;
            //Serial.println(pos1);
          }
        
//**********************************************
// the same analysis as the previous servo
        if(pos2<jiyi2[k])
        {
          while(pos2<jiyi2[k])
          {
            myservo2.write(pos2);
            delay(5);
            pos2++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos2>jiyi2[k])
          {
            myservo2.write(pos2);
            delay(5);
            pos2--;
            //Serial.println(pos1);
          }
        }
 //*****************************************************
 //the same analysis 
        if(pos3<jiyi3[k])
        {
          while(pos3<jiyi3[k])
          {
            myservo3.write(pos3);
            delay(5);
            pos3++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos3>jiyi3[k])
          {
            myservo3.write(pos3);
            delay(5);
            pos3--;
            //Serial.println(pos1);
          }
        }
 //*****************************************************   
 //the same analysis
        if(pos4<jiyi4[k])
        {
          while(pos4<jiyi4[k])
          {
            myservo4.write(pos4);
            delay(5);
            pos4++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos4>jiyi4[k])
          {
            myservo4.write(pos4);
            delay(5);
            pos4--;
            //Serial.println(pos1);
          }
        }
      }
    }
      }
       
      
      if(ps2x.Button(PSB_L2))
       Serial.println("L2 pressed");
      if(ps2x.Button(PSB_R2))
       Serial.println("R2 pressed");
      if(ps2x.Button(PSB_GREEN))
       Serial.println("Triangle pressed");
       
  }   
       
  
  if(ps2x.ButtonPressed(PSB_RED))             //will be TRUE if button was JUST pressed
       Serial.println("Circle just pressed");
       
  if(ps2x.ButtonReleased(PSB_PINK))             //will be TRUE if button was JUST released
       Serial.println("Square just released");     
  
  if(ps2x.NewButtonState(PSB_BLUE))            //will be TRUE if button was JUST pressed OR released
       Serial.println("X just changed");    

    // turn
    zhuandong();
    // claw
    zhuazi();
    // lower arm
    dabi();
    // upper arm
    xiaobi();
  
  if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) // print stick values if either is TRUE
  {
      Serial.print("Stick Values:");
      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX  
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_LX), DEC); 
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_RY), DEC); 
      Serial.print(",");
      Serial.println(ps2x.Analog(PSS_RX), DEC); 
  } 
    
 delay(5);
}

//********************************************************************
// turn 
void zhuandong()
{
//turn right 
   if(ps2x.Analog (PSS_RX) > 200)  // if push the right joystick to the right
    {
      //Serial.println(ps2x.Analog(PSS_RX), DEC);
      pos1=pos1-1;    //pos1 subtracts 1
      myservo1.write(pos1);   // servo 1 carries out the action and the arm will turn right
      // delay(5);
      if(pos1<1)   // limit the right turning angle 
      {
        pos1=1;
      }
     }
      //左转
     if(ps2x.Analog (PSS_RX) < 50)    //if push the right joystick to the left
     {
       //Serial.println(ps2x.Analog(PSS_RX), DEC);
       pos1=pos1+1;   //pos1 plus 1
       myservo1.write(pos1);     // the arm will turn left
       // delay(5);
       if(pos1>180)     // limit the left turning angle 
       {
         pos1=180;
       }
     }
 }
 //**********************************************************************  
 // upper arm
void xiaobi()
{
  //upper arm front
   if(ps2x.Analog(PSS_RY)<50)  // if push the right joystick upward
   {
      pos2=pos2-1;
      myservo2.write(pos2);    // the upper arm will lift up
      delay(5);
      if(pos2<0)    // limit the lifting angle 
      {
        pos2=0;
      }
    }
    // upper arm back 
    if(ps2x.Analog(PSS_RY)>200)    //if push the right joystick downward
    {
      pos2=pos2+1;
      myservo2.write(pos2);   // the upper arm will go down 
      delay(5);
      if(pos2>180)  // limit the declining angle 
      {
        pos2=180;
      }
    }
}
//***************************************************************
void zhuazi()
{
// close the claw
   if(ps2x.Analog(PSS_LX)>220)   // if push the left joystick to the right 
   {
      pos4=pos4-1;
      Serial.println(pos4);
      myservo4.write(pos4);  // servo 4 carries out the action and the claw is gradually closed.
      delay(5);
      if(pos4<0)   // if pos4 value reduces to 37（the claw we test in 37degrees is closed）
      {
        pos4=0;
      }
    }
    // open the claw
    if(ps2x.Analog(PSS_LX)<10)   // if push the left joystick to the left
    {
      pos4=pos4+8;
      Serial.println(pos4);
      myservo4.write(pos4);   // servo 4 carries out the action and the claw is gradually opened
      delay(5);
      if(pos4>108)    // limit the maximum angle opened
      {
        pos4=108;
      }
    }
} 
//*********************************************************
void dabi()
{
  // lower arm front
  if(ps2x.Analog(PSS_LY)>200)  //  if push the left joystick upward
  {
    pos3=pos3+1;
    myservo3.write(pos3);  // the lower arm will stretch out
    delay(5);
    if(pos3>180)   // limit the stretched angle
    {
      pos3=180;
    }
   }
  if(ps2x.Analog(PSS_LY)<10)    // if push the left joystick downward
  {
    pos3=pos3-1;
    myservo3.write(pos3);   //the lower arm will retract
    delay(5);
    if(pos3<35)    // limit the retracted angle
    {
      pos3=35;
    }
   }
}


Test Result:
Stack the shield onto REV4 board and upload the code. Powered on and connected the PS2 Joypad, you can use the PS2 Joypad to control the robot arm memorize several postures.


4)PS2 Controlling Posture Memory And Loop

Description:
In the previous experiment, we have showed how to use Joystick module to control the robot arm memorize several postures and loop. Now we replace the Joystick module with the PS2 Joypad. The program is almost the same.

Hookup Guide:

thumb

Test Code 13:

 
#include <PS2X_lib.h>

#include <Servo.h>  // add the servo libraries
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;  

int pos1=80, pos2=60, pos3=130, pos4=0;  // define the variable of 4 servo angle and assign the initial value( that is the boot posture angle value)

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning 
//you must always either restart your Arduino after you conect the controller, 
//or call config_gamepad(pins) again after connecting the controller.
int error = 0; 
byte vibrate = 0;

int s1,s2,s3,s4;
int jiyi1[30];   //define four array, separately used to save the angle of 4 servos.
int jiyi2[30];
int jiyi3[30];
int jiyi4[30];
int i=0;
int j=0,tt=0;
void setup()
{
 Serial.begin(57600);

 // boot posture
  myservo1.write(pos1);  
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);

  
 error = ps2x.config_gamepad(13,11,10,12);   //setup GamePad(clock, command, attention, data) pins, check for error
 
 if(error == 0){
   Serial.println("Found Controller, configured successful");
   Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
  Serial.println("holding L1 or R1 will print out the analog stick values.");
  Serial.println("Go to www.billporter.info for updates and to report bugs.");
 }
   
  else if(error == 1)
   Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
   
  else if(error == 2)
   Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
   
   //Serial.print(ps2x.Analog(1), HEX);
 
 
 ps2x.enableRumble();              //enable rumble vibration motors
 ps2x.enablePressures();           //enable reading the pressure values from the buttons. 
}

void loop()
{
  myservo1.attach(A1);  // set the control pin of servo 1 to A1
  myservo2.attach(A0);  //set the control pin of servo 2 to A0
  myservo3.attach(6);   //set the control pin of servo 3 to D6
  myservo4.attach(9);   //set the control pin of servo 4 to D9
  
 if(error != 0)
  return; 
  
  ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed
  
  if(ps2x.Button(PSB_START))                   //will be TRUE as long as button is pressed
       Serial.println("Start is being held");
  if(ps2x.Button(PSB_SELECT))
       Serial.println("Select is being held");
       
       
   if(ps2x.Button(PSB_PAD_UP)) {         //will be TRUE as long as button is pressed
     Serial.print("Up held this hard: ");
     Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    }
    if(ps2x.Button(PSB_PAD_RIGHT)){
     Serial.print("Right held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    }
    if(ps2x.Button(PSB_PAD_LEFT)){
     Serial.print("LEFT held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    }
    if(ps2x.Button(PSB_PAD_DOWN)){
     Serial.print("DOWN held this hard: ");
   Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    }   

  
    vibrate = ps2x.Analog(PSAB_BLUE);        //this will set the large motor vibrate speed based on 
                                            //how hard you press the blue (X) button    
  
  if (ps2x.NewButtonState())               //will be TRUE if any button changes state (on to off, or off to on)
  {
      if(ps2x.Button(PSB_R3))
      {
        //Serial.println("R3 pressed");
        //record
        s1=myservo1.read();
        delay(100);
        Serial.println(s1);
        s2=myservo2.read();
        delay(100);
        Serial.println(s2);
        s3=myservo3.read();
        delay(100);
        Serial.println(s3);
        s4=myservo4.read();
        delay(100);
        Serial.println(s4);
      
        jiyi1[i]=s1;  //save the servo value read in the array sequentially
        jiyi2[i]=s2;
        jiyi3[i]=s3;
        jiyi4[i]=s4;
        i++;
        j=i;
       // delay(100);
       Serial.println(i);
      }
      
       // carry out
      if(ps2x.Button(PSB_L3))
      {
        //Serial.println("L3 pressed");
        i=0;
        tt=1;
   
       pos1 = myservo1.read();  // record the angle value of 4 servo posture
       pos2 = myservo2.read();
       pos3 = myservo3.read();
       pos4 = myservo4.read();

     while(tt==1)  // repeat the actions
     {
       for(int k=0;k<j;k++)   //for loop, to execute all the stored actions.
      {
        if(pos1<jiyi1[k])   // if the current servo 1 angle is less than the value stored in array 1. 
        {
          while(pos1<jiyi1[k])    //while loop, make servo turn to the position of value stored in the array.
          {
            myservo1.write(pos1);   //servo 1 executes the action
            delay(5);     //delay 5ms，controlling the rotating speed of servo.
            pos1++;
            //Serial.println(pos1);
          }
        }
        else     //if the current servo 1 angle is greater than the value stored in array 1.
        {
         while(pos1>jiyi1[k])     //while loop, make servo turn to the position of value stored in the array.
          {
            myservo1.write(pos1);    //servo 1 executes the action
            delay(5);           //delay 5ms，controlling the rotating speed of servo.
            pos1--;
            //Serial.println(pos1);
          }
        
//**********************************************
// the same analysis as the previous servo
        if(pos2<jiyi2[k])
        {
          while(pos2<jiyi2[k])
          {
            myservo2.write(pos2);
            delay(5);
            pos2++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos2>jiyi2[k])
          {
            myservo2.write(pos2);
            delay(5);
            pos2--;
            //Serial.println(pos1);
          }
        }
 //*****************************************************
// the same analysis as the previous servo
        if(pos3<jiyi3[k])
        {
          while(pos3<jiyi3[k])
          {
            myservo3.write(pos3);
            delay(5);
            pos3++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos3>jiyi3[k])
          {
            myservo3.write(pos3);
            delay(5);
            pos3--;
            //Serial.println(pos1);
          }
        }
 //*****************************************************   
// the same analysis as the previous servo
        if(pos4<jiyi4[k])
        {
          while(pos4<jiyi4[k])
          {
            myservo4.write(pos4);
            delay(5);
            pos4++;
            //Serial.println(pos1);
          }
        }
        else
        {
         while(pos4>jiyi4[k])
          {
            myservo4.write(pos4);
            delay(5);
            pos4--;
            //Serial.println(pos1);
          }
        }
      }
    }
    //*******************************************************
    // exit the looping
     ps2x.enableRumble();              //enable rumble vibration motors
     ps2x.enablePressures();
     ps2x.read_gamepad(false, vibrate); 
     vibrate = ps2x.Analog(PSAB_BLUE);
      if (ps2x.NewButtonState())               //will be TRUE if any button changes state (on to off, or off to on)
      {
        if(ps2x.Button(PSB_R3))
        {
          tt=0;
          i=0;
          break;
        }
      }
   //*********************************************************
   }
  }

      if(ps2x.Button(PSB_L2))
       Serial.println("L2 pressed");
      if(ps2x.Button(PSB_R2))
       Serial.println("R2 pressed");
      if(ps2x.Button(PSB_GREEN))
       Serial.println("Triangle pressed");
       
  }   
       
  
  if(ps2x.ButtonPressed(PSB_RED))             //will be TRUE if button was JUST pressed
       Serial.println("Circle just pressed");
       
  if(ps2x.ButtonReleased(PSB_PINK))             //will be TRUE if button was JUST released
       Serial.println("Square just released");     
  
  if(ps2x.NewButtonState(PSB_BLUE))            //will be TRUE if button was JUST pressed OR released
       Serial.println("X just changed");    

    //turn
    zhuandong();
    //claw
    zhuazi();
    //lower arm
    dabi();
    //upper arm
    xiaobi();
  
  if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) // print stick values if either is TRUE
  {
      Serial.print("Stick Values:");
      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX  
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_LX), DEC); 
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_RY), DEC); 
      Serial.print(",");
      Serial.println(ps2x.Analog(PSS_RX), DEC); 
  } 
    
 delay(5);
}

//********************************************************************
// turn 
void zhuandong()
{
// turn right
   if(ps2x.Analog (PSS_RX) > 200)  // if push the right joystick to the right
    {
      //Serial.println(ps2x.Analog(PSS_RX), DEC);
      pos1=pos1-1;    //pos1 subtracts 1
      myservo1.write(pos1);   // servo 1 carries out the action, the robot arm turns right.
      // delay(5);
      if(pos1<1)   // limit the right turning angle
      {
        pos1=1;
      }
     }
      // turn left
     if(ps2x.Analog (PSS_RX) < 50)    // if push the right joystick to the left
     {
       //Serial.println(ps2x.Analog(PSS_RX), DEC);
       pos1=pos1+1;   //pos1 plus 1
       myservo1.write(pos1);     // the robot arm turns left 
       // delay(5);
       if(pos1>180)     // limit the left turning angle
       {
         pos1=180;
       }
     }
 }
 //**********************************************************************  
 // the upper arm 
void xiaobi()
{
  // upper arm front
   if(ps2x.Analog(PSS_RY)<50)  // if push the right joystick upward
   {
      pos2=pos2-1;
      myservo2.write(pos2);    // the upper arm will lift up
      delay(5);
      if(pos2<0)    // limit the lifting angle 
      {
        pos2=0;
      }
    }
    // upper arm back
    if(ps2x.Analog(PSS_RY)>200)    //if push the right joystick to downward
    {
      pos2=pos2+1;
      myservo2.write(pos2);   // the robot arm will go down
      delay(5);
      if(pos2>180)  // limit the declining angle
      {
        pos2=180;
      }
    }
}
//***************************************************************
void zhuazi()
{
// close the claw
   if(ps2x.Analog(PSS_LX)>220)   // if push the left joystick to the right  
   {
      pos4=pos4-1;
      Serial.println(pos4);
      myservo4.write(pos4);  // servo 4 carries out the action and claw is gradually closed 
      delay(5);
      if(pos4<0)   // if pos4 value subtracts to 37, the claw in 37 degrees we have tested is closed.）
      {
        pos4=0;
      }
    }
    // open the claw
    if(ps2x.Analog(PSS_LX)<10)   //  if push the left joystick to the left
    {
      pos4=pos4+8;
      Serial.println(pos4);
      myservo4.write(pos4);   //  servo 4 carries out the action and claw is gradually opened
      delay(5);
      if(pos4>108)    // limit the maximum angle opened
      {
        pos4=108;
      }
    }
} 
//*********************************************************
void dabi()
{
  // lower arm front
  if(ps2x.Analog(PSS_LY)>200)  // if push the left joystick upward
  {
    pos3=pos3+1;
    myservo3.write(pos3);  //  the lower arm will stretch out
    delay(5);
    if(pos3>180)   // limit the stretched angle 
    {
      pos3=180;
    }
   }
  if(ps2x.Analog(PSS_LY)<10)    // if push the left joystick downward
  {
    pos3=pos3-1;
    myservo3.write(pos3);   // the lower arm will draw back 
    delay(5);
    if(pos3<35)    //  limit the retracted angle 
    {
      pos3=35;
    }
   }
}


Test Result:
Stack the shield onto REV4 board and upload the code. Powered on and connected the PS2 Joypad, you can use the PS2 Joypad to control the robot arm memorize several postures, looping.


More Resources

Download libraries, software, app and test code:

https://fs.keyestudio.com/KS0198

ARDUINO Software: https://www.arduino.cc/en/Main/OldSoftwareReleases#1.5.x


Our Tutorial

This tutorial is designed for everyone to DIY robot arm. You will learn all the basic information about how to control this robot arm with Joystick and Bluetooth. Just enjoy your time!
Great! It's just the beginning of ARDUINO programming journey. There are more and more awesome projects for you to explore. Furthermore, our KEYESTUDIO research and development team will continue to explore on this path, walking you through the basics up to complex projects. Hope that you can enjoy our works!


About keyestudio

Located in Shenzhen, the Silicon Valley of China, KEYES DIY ROBOT CO.,LTD is a thriving technology company dedicated to open-source hardware research and development, production and marketing. Keyestudio is a best-selling brand owned by KEYES Corporation, our product lines range from Arduino boards, shields, sensor modules, Raspberry Pi, micro:bit extension boards and smart car to complete starter kits designed for customers of any level to learn Arduino knowledge.
All of our products comply with international quality standards and are greatly appreciated in a variety of different markets throughout the world. For more details of our products, you can check it from the links below.
Official Website: http://www.keyestudio.com/

US Amazon storefront: http://www.amazon.com/shops/A26TCVWBQE4D9T
CA Amazon storefront: http://www.amazon.ca/shops/A26TCVWBQE4D9T
UK Amazon storefront: http://www.amazon.co.uk/shops/A39F7KX4U3W9JH
DE Amazon storefront: http://www.amazon.de/shops/A39F7KX4U3W9JH
FR Amazon storefront: http://www.amazon.de/shops/A39F7KX4U3W9JH
ES Amazon storefront: http://www.amazon.de/shops/A39F7KX4U3W9JH
IT Amazon storefront: http://www.amazon.de/shops/A39F7KX4U3W9JH
US Amazon storefront: http://www.amazon.com/shops/APU90DTITU5DG
CA Amazon storefront: http://www.amazon.ca/shops/APU90DTITU5DG
JP Amazon storefront: http://www.amazon.jp/shops/AE9VWCCXQIC6J


Customer Service

As a continuous and fast growing technology company, we keep striving our best to offer you excellent products and quality service as to meet your expectation. We look forward to hearing from you and any of your critical
comment or suggestion would be much valuable to us.
You can reach out to us by simply drop a line at keyestudio@126.com
Thank you in advance.


Buy From

Retrieved from "http://wiki.keyestudio.com/index.php?title=Ks0198_keyestudio_4DOF_Robot_Mechanical_Arm_Kit_for_Arduino_DIY&oldid=32420"