Download Arduino's sketch file.

Plug the USB cable on your computer's USB port and upload the code. Uploading the code takes some time. You can use that time to give a 'like' and 'share' this tutorial while you wait! :D

After the upload was complete, unplug the USB cable, connect the power supply and turn on the power button. The code will start running imediatly.

Warning: when the code starts running, the robotic arm will move really fast to it's initial position. Be careful not to get hurt or damage nearby equipment during startup!

You'll possibly have to replace the starting angle of each servomotor depending on how your servos where mounted.

Code explained:

Before the setup, the code imports the libraries used on the sketch (nunchuk.h, wire.h and servo.h).

Pins to be used are defined, and global variables are declared. The angle# integer variables store the initial position for each servo. If you want your robot to start at a different position, change the values on those variables.

servo#_speed variables define the speed of the movement of each servo. If you want a specific servo to move faster, increase its value. angle#min and angle#max variables are used to limit the maximun and minimun angle for each servo. You can set those variables in order to avoid colisions between consecutive joints of the robot.

//Include libraries #include <nunchuk.h> #include <wire.h> #include <servo.h> //define variables #define SERV1 8 //servo 1 on digital port 8 #define SERV2 9 //servo 2 on digital port 9 #define SERV3 10 //servo 3 on digital port 10 #define SERV4 11 //servo 4 on digital port 11 #define SERV5 12 //servo 5 on digital port 12 #define SERV6 13 //servo 6 on digital port 13 Servo s1; //servo 1 Servo s2; //servo 2 Servo s3; //servo 3 Servo s4; //servo 4 Servo s5; //servo 5 Servo s6; //servo 6 //define starting angle for each servo //choose a safe position to start from //it will try to move instantaniously to that position when powered up! //those angles will depend on the angle of each servo during the assemble int angle1 = 90; //servo 1 current angle int angle2 = 30; //servo 2 current angle int angle3 = 0; //servo 3 current angle int angle4 = 90; //servo 4 current angle int angle5 = 90; //servo 5 current angle int angle6 = 45; //servo 6 current angle int servo1_speed = 3; //servo 1 speed int servo2_speed = 3; //servo 2 speed int servo3_speed = 3; //servo 3 speed int servo4_speed = 1; //servo 4 speed int servo5_speed = 1; //servo 5 speed //define restrictions for each servo //those angles will depend on the angle of each servo during the assemble int angle1min = 0; //servo 1 minimum angle int angle1max = 180; //servo 1 maximum angle int angle2min = 0; //servo 2 minimum angle int angle2max = 180; //servo 2 maximum angle int angle3min = 0; //servo 3 minimum angle int angle3max = 180; //servo 3 maximum angle int angle4min = 0; //servo 4 minimum angle int angle4max = 180; //servo 4 maximum angle int angle5min = 0; //servo 5 minimum angle int angle5max = 180; //servo 5 maximum angle int angle6min = 0; //servo 6 minimum angle int angle6max = 180; //servo 6 maximum angle boolean display_angles = true; //boolean used to update the angle of each servo on Serial Monitor

During the setup, each servo is attached to an especific pin, and its position is started.

Serial communication (to Serial monitor) and I2C communication with the Nunchuk are also started here.

//SETUP void setup() { //attach each servo to a pin and start its position s1.attach(SERV1); s1.write(angle1); s2.attach(SERV2); s2.write(angle2); s3.attach(SERV3); s3.write(angle3); s4.attach(SERV4); s4.write(angle4); s5.attach(SERV5); s5.write(angle5); s6.attach(SERV6); s6.write(angle6); //start serial communication Serial.begin(9600); //start Nunchuk Wire.begin(); nunchuk_init(); }

The main loop is repeated over and over. Nunchuk status is read at each cycle. Depending on the readings, different commands are performed.

void loop() { //read Nunchuk sensors if (nunchuk_read()) { int x = nunchuk_joystickX(); //joystick X position int y = nunchuk_joystickY(); //joystick Y position boolean z = nunchuk_buttonZ(); //z button status boolean c = nunchuk_buttonC(); //c button status float pitch = nunchuk_pitch(); //pitch angle float roll = nunchuk_roll(); //roll angle

Joystick X will be used to move the servo #1.

The following block of code was used. First it checks if the value of the joystick is large enough. This way, noise and small variations are disregarded. If the value meet the requirements, the angle of the servo will be increased/decreased at a given speed.

//Turn left/right (at a fixed speed) //Turn left if (x > 90) { angle1 -= servo1_speed; display_angles = true; if (angle1 < angle1min) { angle1 = angle1min; } } //Turn right if (x < -90) { angle1 += servo1_speed; display_angles = true; if (angle1 > angle1max) { angle1 = angle1max; } } s1.write(angle1); //update servo position

A similar block is used for joystick y. It's used for changing the angle of servo #3. Servo #2 is kept fixed in this code.

Rotation of the gripper is given by the roll and pitch angles of the controller, measured by it's accelerometer. In order to make the control of the arm easier, the angle of the gripper is only updated when C or Z buttons are pressed.

When only C button is pressed, the code reads roll angle and use it as a set point. servo #5 is rotated until it reaches the set point. It's speed is proportional to the error between actual and desired position. A similar code is used for servo #4, which tracks Nunchuk's pitch angle.

// Enable accelerometer only when the buttons are pressed // Rotate gripper (only Z button pressed) if (c && !z) { roll = roll * 57.0 + 90.0; //convert do degrees servo5_speed = abs(angle5 - roll)/10 + 1; //speed proportional do the error between actual and desired angle if (roll > angle5) { angle5 += servo5_speed; display_angles = true; } if (roll < angle5) { angle5 -=servo5_speed; display_angles = true; } s5.write(angle5); //update servo position }

The gripper is closed whenever both C and Z buttons are pressed. When any of those buttons is released, the robot will open its gripper.

//Open/close gripper (both buttons pressed) if(z && c) { s6.write(90); //close gripper display_angles = true; } else { s6.write(45); //open gripper }

There's a block of code by the end of the sketch. It will display on the Serial Monitor the actual angle of each servomotor. It might be helpfull for choosing the starting angle of each motor.