RobotArm_IK/RobotArm_IK.ino at master · JesperKrogPoulsen/RobotArm_IK · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
#include <Servo.h>

//Constants (robot dimensions in mm)
int baseHeight = 50;
int humerus = 50;
int ulna = 50;
int hand = 80;
const float pi = 3.141593;

float shoulder;
float elbow;
float wristAngle;
int setHeight;
int setDepth;
int setAngle;

int rotationStartDepth; //Initial depth value before compensating for rotation
int setRotation;  //
int rotationDepth;  //Depth value after rotation

//PINOUT
int servoPin[] = {5,6,7,8,9};
int gripperButton = 12;

//Servo object declaration using <Servo.h>
Servo myservo;  //A-1 (Base)
Servo myservo2; //A-2 (Shoulder)
Servo myservo3; //A-3 (Elbow)
Servo myservo4; //A-4 (Wrist)
Servo myservo5; //A-5 (Gripper)

void setup(){
    Serial.begin(115200);
    myservo.attach(servoPin[0]);  //A-1 (Base)
    myservo2.attach(servoPin[1]); //A-2 (Shoulder)
    myservo3.attach(servoPin[2]); //A-3 (Elbow)
    myservo4.attach(servoPin[3]); //A-4 (Wrist)
    myservo5.attach(servoPin[4]); //A-5 (Gripper)
    pinMode(gripperButton, INPUT_PULLUP);
}

void loop() {
    serialDebug();
    setHeight = map(analogRead(A1),0,1024,10,200);
    setRotation = map(analogRead(A0),0,1024,-90,90);
    rotationStartDepth = map(analogRead(A2),0,1024,10,200);
    setAngle = map(analogRead(A3),0,1024,-90,90);
    rotationDepth = constrain(rotationStartDepth/cos(setRotation*0.0174532925),10,200);
    findAngles(setHeight,rotationDepth,setAngle); //End effector angle locked to 0!
    if (shoulder != 0) {
        myservo.write(90+setRotation);  //Base.Neutral position = 100 100+setRotation
        myservo2.write(shoulder);     //Shoulder. Vertical = 90.
        myservo3.write(55+elbow);  //Elbow. Vertical up = 175.
        myservo4.write(90+wristAngle);  //Wrist. Vertical = 90.
        if (digitalRead(gripperButton) == 1) {
            myservo5.write(48);  //Less is more closed
        }
        else {
            myservo5.write(115);  //Less is more closed
        }
    }
    else Serial.println("OUTSIDE AREA OF OPERATION");
}

void serialDebug() {
    Serial.print(analogRead(A0));
    Serial.print("\t");
    Serial.print(analogRead(A1));
    Serial.print("\t");
    Serial.print(analogRead(A2));
    Serial.print("\t");
    Serial.print(analogRead(A3));
    Serial.print("\t");
    Serial.print(setHeight); //LEFT RIGHT(1024)
    Serial.print("\t");
    Serial.print(setDepth); //DOWN UP(1024)
    Serial.print("\t");
    Serial.print(setAngle);//LEFT RIGHT(1024)
    Serial.print("\t");
    Serial.print(shoulder);
    Serial.print("\t");
    Serial.print(elbow);
    Serial.print("\t");
    Serial.print(setRotation);
    Serial.print("\t");
    Serial.print(rotationDepth);
    Serial.print("\n");
    delay(30);
}

void findAngles(float localHeight, float localDepth, float wristAngleToGround){
    Serial.print(localHeight); Serial.print("\t"); Serial.print(localDepth); Serial.print("\t"); Serial.print(wristAngleToGround);  Serial.print("\n");
    float offsetsHeight = sin(radians(wristAngleToGround))*hand;
    float offsetsDepth = cos(radians(wristAngleToGround))*hand;
    float wristHeight = localHeight-offsetsHeight-baseHeight;
    float wristDepth = localDepth-offsetsDepth;
    float shoulderToWrist = sqrt((wristHeight*wristHeight)+(wristDepth*wristDepth));
    float angle1 = atan2(wristHeight,wristDepth);
    float angle2 = acos(((humerus*humerus)-(ulna*ulna)+(shoulderToWrist*shoulderToWrist))/((2*humerus)*shoulderToWrist));
    shoulder = int((angle1+angle2)*180/pi);
    elbow = int(-(180-(acos(((humerus*humerus)+(ulna*ulna)-(shoulderToWrist*shoulderToWrist))/((2*humerus)*ulna)))*180/pi));
    wristAngle = wristAngleToGround-elbow-shoulder;
    Serial.print(offsetsHeight); Serial.print("\t");
    Serial.print(offsetsDepth); Serial.print("\t");
    Serial.print(wristHeight); Serial.print("\t");
    Serial.print(wristDepth); Serial.print("\t");
    Serial.print(shoulderToWrist); Serial.print("\t");
    Serial.print(angle1); Serial.print("\t");
    Serial.print(angle2); Serial.print("\t");
    Serial.print(shoulder); Serial.print("\t");
    Serial.print(elbow); Serial.print("\t");
    Serial.print(wristAngle); Serial.print("\n");
}