-
Notifications
You must be signed in to change notification settings - Fork 0
/
Roboarm.ino
267 lines (203 loc) · 7.24 KB
/
Roboarm.ino
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
/*
Robot arm control using hacked servos and a master arm with pots as control in
master-slave system.
+y is vertical, +x is to the right
drawing line/circle:
http://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino
inverse kinematics:
http://www.micromegacorp.com/downloads/documentation/AN044-Robotic%20Arm.pdf
http://www.learnaboutrobots.com/inverseKinematics.htm
*/
#include <AFMotor.h>
#include <math.h>
#include <PID_v1.h>
#include <IVSense.h>
// Instantiate both motors
AF_DCMotor shoulder(3);
AF_DCMotor elbow(4);
// declare pins
int ElbowPin = A1; // to potentiometer on elbow motor
int ShoulderPin = A0; // to potentiometer on shoulder motor
// INITIALIZE CONSTANTS
double Kp_Elbow = 20; // this is the proportional gain
double Kp_Shoulder = 20;
double Ki_Elbow = 0.1; // this is the integral gain
double Ki_Shoulder = 0.1;
double Kd_Elbow = 1.0; // this is the derivative gain
double Kd_Shoulder = 0.75;
double Elbow_neg = 970; // joint limits of robotic arm using right hand rule for sign
double Elbow_pos = 31;
double Shoulder_neg = 210;
double Shoulder_pos = 793;
const double a1 = 200; // shoulder-to-elbow "bone" length (mm)
const double a2 = 220; // elbow-to-wrist "bone" length (mm) - longer c bracket
double highY = 350; // line drawing targets
double lowY = 250;
double constantX = 200;
boolean elbowup = false; // true=elbow up, false=elbow down
// // VOLTAGE & CURRENT SENSE CONSTANTS/VARIABLES // //
const int analogElbowVoltagePin = A4;
const int analogShoulderVoltagePin = A5;
const int analogElbowCurrentPin = A2;
const int analogShoulderCurrentPin = A3;
const int rScale = 1;
IVSense elbow_sensor(analogElbowCurrentPin, analogElbowVoltagePin, rScale);
IVSense shoulder_sensor(analogShoulderCurrentPin, analogShoulderVoltagePin, rScale);
// // // // // // // // // // // // // // // // // //
// VARIABLES
double rawElbowAngle = 0.0; // initialize all angles to 0
double rawShoulderAngle = 0.0;
double elbowAngle = 0.0; // initialize all angles to 0
double shoulderAngle = 0.0;
double theta1 = 0.0; // target angles as determined through IK
double theta2 = 0.0;
double actualX = 0.0;
double actualY = 0.0;
double y = 0.0;
double c2 = 0.0; // is btwn -1 and 1
double s2 = 0.0;
double pwmTemp = 0.0;
double pwmElbow = 50.0; // between 0 to 255
double pwmShoulder = 50.0;
//Syntax: PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
PID elbowPID(&elbowAngle, &pwmElbow, &theta2, Kp_Elbow, Ki_Elbow, Kd_Elbow, DIRECT);
PID shoulderPID(&shoulderAngle, &pwmShoulder, &theta1, Kp_Shoulder, Ki_Shoulder, Kd_Shoulder, DIRECT);
int highV = 0;
void setup() { //happens once
Serial.begin(115200); // set up Serial library
elbowPID.SetSampleTime(5); // (ms) how often new PID calc is performed, default is 1000
shoulderPID.SetSampleTime(5);
elbow.setSpeed(pwmElbow); // set the speed to pwmElbow
shoulder.setSpeed(pwmShoulder); // set the speed to pwmElbow
elbowPID.SetMode(AUTOMATIC);
shoulderPID.SetMode(AUTOMATIC);
elbowPID.SetOutputLimits(-255,255);
shoulderPID.SetOutputLimits(-255,255);
move_to_start(); //get to starting position
//line_y();
}
void loop() { // happens forever
line_y();
}
void move_to_start() {
do {
get_angles(constantX, lowY);
drive_joints(); // drive joints until actual equals expected
}
while( abs(theta1 - shoulderAngle) > 2 && abs(theta2 - elbowAngle) > 2 ); // bail when it's close
}
void line_y() {
for(y = lowY; y < highY; y += 1) { // draw straight line up
get_angles(constantX,y);
drive_joints();
}
for(y = highY; y > lowY; y -= 1) { // draw straight line down
get_angles(constantX, y);
drive_joints();
}
}
void get_xy() {
actualX = degrees(a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2)));
actualY = degrees(a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2)));
}
// Given target(Px, Py) solve for theta1, theta2 (inverse kinematics)
void get_angles(double Px, double Py) {
// first find theta2 where c2 = cos(theta2) and s2 = sin(theta2)
c2 = (pow(Px,2) + pow(Py,2) - pow(a1,2) - pow(a2,2))/(2*a1*a2); // is btwn -1 and 1
if (elbowup == false) {
s2 = sqrt(1 - pow(c2,2)); // sqrt can be + or -, and each corresponds to a different orientation
}
else if (elbowup == true) {
s2 = -sqrt(1 - pow(c2,2));
}
theta2 = degrees(atan2(s2,c2)); // solves for the angle in degrees and places in correct quadrant
// now find theta1 where c1 = cos(theta1) and s1 = sin(theta1)
theta1 = degrees(atan2(-a2*s2*Px + (a1 + a2*c2)*Py, (a1 + a2*c2)*Px + a2*s2*Py));
}
void drive_joints() {
// read the actual values from all the pots
rawElbowAngle = analogRead(ElbowPin);
rawShoulderAngle = analogRead(ShoulderPin);
// constrain robot arm to ignore out of range values
elbowAngle = constrain(rawElbowAngle, Elbow_pos, Elbow_neg);
shoulderAngle = constrain(rawShoulderAngle, Shoulder_neg, Shoulder_pos);
// now map the angles so they correspond correctly
elbowAngle = map(elbowAngle, Elbow_neg, Elbow_pos, -110.0, 85.0);
shoulderAngle = map(shoulderAngle, Shoulder_neg, Shoulder_pos, 5.0, 135.0);
elbowPID.Compute();
shoulderPID.Compute();
// DRIVE ELBOW: CW is forward, CCW is backward
pwmTemp = abs(pwmElbow);
elbow.setSpeed(pwmTemp); // now use the new PID output to set the speed
if (pwmElbow < 0) {
elbow.run(FORWARD); // turn it on
}
else if ( pwmElbow > 0) {
elbow.run(BACKWARD); // turn it on
}
else elbow.run(RELEASE); // stopped
elbow_sensor.readData(); // elbow joint sensing
// DRIVE SHOULDER: CCW is forward, CW is backward
pwmTemp = abs(pwmShoulder);
shoulder.setSpeed(pwmTemp); // set the speed
if (pwmShoulder < 0) {
shoulder.run(BACKWARD); // turn it on
}
else if (pwmShoulder > 0) {
shoulder.run(FORWARD); // turn it on
}
else shoulder.run(RELEASE); // stopped
shoulder_sensor.readData(); // shoulder joint sensing
//highV = analogRead(HighPin);
//get_xy();
// send current data serially
Serial.print( millis() );
Serial.print(',');
/*
Serial.print(elbowAngle);
Serial.print(',');
Serial.print(shoulderAngle);
Serial.print(',');
Serial.print(pwmElbow);
Serial.print(',');
Serial.print(pwmShoulder);
Serial.print(',');
*/
Serial.print(elbow_sensor.getCurrent(), BIN);
Serial.print(',');
Serial.print(elbow_sensor.getVoltage(), BIN);
Serial.print(',');
Serial.print(shoulder_sensor.getCurrent(), BIN);
Serial.print(',');
Serial.println(shoulder_sensor.getVoltage(), BIN);
// need to know time, angle for velocity calcs
/*
Serial.print(millis());
Serial.print(" , ");
Serial.print("Elbow: ");
Serial.print(" , ");
Serial.print(elbowAngle);
Serial.print(" , ");
Serial.print(theta2);
Serial.print(" , ");
Serial.print("Shoulder: ");
Serial.print(" , ");
Serial.print(shoulderAngle);
Serial.print(" , ");
Serial.print(theta1);
Serial.print(" , ");
Serial.print("Voltage:");
Serial.print(" , ");
Serial.print(highV);
Serial.print(" , ");
Serial.print("X:");
Serial.print(" , ");
Serial.print(actualX);
Serial.print(" , ");
Serial.print("Y: ");
Serial.print(" , ");
Serial.print(y);
Serial.print(" , ");
Serial.println(actualY);
*/
}