Robotarm

We've programmed and build a robot arm that's able to self regulate its height and maintain a given height.
The desired height is controlled by the speed knob of a hacked Logitech wingman extreme digital joystick from the 80's.
This joystick also controls the arm and elbow movement of the robot arm.
The shoulder movement is controlled by an HC-RS04 sonar.
6 buttons from the joystick is also available for use, as it is wired and programmed.

All this is programmed in Arduino Uno.

/*
Hacking an old joystik an controlling a robotarm.
*/
/* To understand servo og ping control, check servo.h/newping.h libraries */
#include
#include
#define TRIGGER_PIN 12 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 13 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 400 /* Maximum distance we want to ping for (in centimeters).
Maximum sensor distance is rated at 400-500cm.*/
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
Servo servo_arm;
Servo servo_skulder;
Servo servo_albue;
int temp_val;
int last_X = 25;
int last_Y = 25;
int last_Servo = 25;
int desiret_height = 3;
void setup() {
pinMode(0, INPUT_PULLUP); //Setup for pins til inputs
pinMode(6, INPUT_PULLUP);
pinMode(2, INPUT_PULLUP);
pinMode(3, INPUT_PULLUP);
pinMode(4, INPUT_PULLUP);
pinMode(5, INPUT_PULLUP);
servo_arm.attach(9);
servo_skulder.attach(10);
servo_albue.attach(11);
}
void loop()
{
////////////////////////////////// Sonar //////////////////////////////////////////
int Sonar = sonar.ping();
delay(15);
/* Ved ikke om denne kode skal bruges endnu */
// if (Sonar <= 183){ // Sonar = 183;
// }

////////////////////////////////// Joystik //////////////////////////////////////////

int sensorValue = analogRead(A0);
delay(15);
int X = sensorValue;
sensorValue = analogRead(A1);
delay(15);
int Y = sensorValue;
sensorValue = analogRead(A2);
delay(15);
int Height = sensorValue;

if (digitalRead(0) == LOW) {
/* Put code here for button action on */
}
else {
/* Put code here for button action off */
}

if (digitalRead(2) == LOW) {
/* same as line 67 and 70*/
}
else {
/* same as line 67 and 70*/
}
if (digitalRead(6) == LOW) {
/* same as line 67 and 70*/
}
else {
/* same as line 67 and 70*/
}

if (digitalRead(3) == LOW) {
/* same as line 67 and 70*/
}
else {
/* same as line 67 and 70*/
}
if (digitalRead(4) == LOW) {
/* same as line 67 and 70*/
}
else {
/* same as line 67 and 70*/
}
if (digitalRead(5) == LOW) {
/* same as line 67 and 70*/
}
else {
/* same as line 67 and 70*/
}
// delay(500); // used to se serial output, slow it down.
////////////////////////////////// Servo control & Mapping //////////////////////////////////////////
Height = map(Height, 0, 1023, 3, 50); // Check https://www.arduino.cc/ for map function
desiret_height = Height;

temp_val = map(X, 900, 45, 0, 180);
servo_arm.write(temp_val);

temp_val = map(Y, 90, 950, 0, 65);
servo_skulder.write(temp_val);

temp_val = map(Sonar, 200, 1400, 0, 55);
if (temp_val < desiret_height)
{
last_Servo -= 1;
servo_albue.write(last_Servo);
}
else if (temp_val > desiret_height)
{
last_Servo += 1;
servo_albue.write(last_Servo);
}
}