Skip to content

Commit f093390

Browse files
author
Alex
committed
Reduce Global variables
1 parent 2417583 commit f093390

3 files changed

Lines changed: 109 additions & 92 deletions

File tree

‎uarm_calibration.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ void CalibrationClass::calibrationServo(byte servo_num)
3131
double l_angle_analog;
3232
double arr_real[16];
3333
double arr_input[16];
34-
int l_address = uarm.kAddrServo + (servo_num - 1) * 6;
34+
int l_address = OFFSET_START_ADDRESS + (servo_num - 1) * 6;
3535
Serial.print("l_address: ");
3636
Serial.println(l_address);
3737
Serial.print("servo ");
@@ -127,7 +127,7 @@ void CalibrationClass::cleanEEPROM(){
127127
}
128128

129129
void CalibrationClass::cleanOFFSETS(){
130-
for(int q = uarm.kAddrOffset; q<uarm.kAddrOffset+8;q++){
130+
for(int q = LINEAR_START_ADDRESS; q<LINEAR_START_ADDRESS+8;q++){
131131
EEPROM.write(q,0);
132132
}
133133
}
@@ -203,7 +203,7 @@ void CalibrationClass::saveOffsetValue(double value, byte servo_num)
203203
value *= 10;
204204
MSBs = (int) value;
205205

206-
EEPROM.write( uarm.kAddrOffset + (servo_num - 1)*2, abs(LSBs));
207-
EEPROM.write( uarm.kAddrOffset + (servo_num - 1)*2 +1, abs(MSBs));
206+
EEPROM.write( LINEAR_START_ADDRESS + (servo_num - 1)*2, abs(LSBs));
207+
EEPROM.write( LINEAR_START_ADDRESS + (servo_num - 1)*2 +1, abs(MSBs));
208208

209209
}

‎uarm_library.cpp‎

Lines changed: 72 additions & 63 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ int angleR;
1818
int angleL;
1919
int angleBottom;
2020
int l_movementTrigger = 0;
21+
double g_interpol_val_arr[50];
2122

2223
uArmClass::uArmClass()
2324
{
@@ -65,15 +66,15 @@ void uArmClass::writeAngle(byte servo_rot_angle, byte servo_left_angle, byte ser
6566
int servo_2_angle_execute = inputToReal(SERVO_LEFT_NUM,round(servo_left_angle));
6667
int servo_3_angle_execute = inputToReal(SERVO_RIGHT_NUM,round(servo_right_angle));
6768
int servo_4_angle_execute = inputToReal(SERVO_HAND_ROT_NUM,round(servo_hand_rot_angle));
68-
69+
6970
if(servo_2_angle_execute < 10) servo_2_angle_execute = 10;
7071
if(servo_2_angle_execute > 120) servo_2_angle_execute = 120;
7172
if(servo_3_angle_execute < 10) servo_2_angle_execute = 10;
7273
if(servo_3_angle_execute > 110) servo_2_angle_execute = 110;
7374

7475

7576
if(servo_2_angle_execute + servo_3_angle_execute > 150)
76-
{servo_3_angle_execute = 150 - servo_2_angle_execute;}
77+
{servo_3_angle_execute = 150 - servo_2_angle_execute;}
7778

7879
g_servo_rot.write(servo_1_angle_execute);
7980
g_servo_left.write(servo_2_angle_execute);
@@ -118,9 +119,9 @@ double uArmClass::readServoOffset(byte servo_num)
118119

119120
if ((servo_num == 1)||(servo_num == 2)||(servo_num == 3))
120121
{
121-
g_servo_offset = (EEPROM.read(kAddrOffset + (servo_num-1)*2 +1))/10.00;
122+
g_servo_offset = (EEPROM.read(LINEAR_START_ADDRESS + (servo_num-1)*2 +1))/10.00;
122123

123-
if (EEPROM.read(kAddrOffset + (servo_num-1)*2 ) == 0)
124+
if (EEPROM.read(LINEAR_START_ADDRESS + (servo_num-1)*2 ) == 0)
124125
{g_servo_offset = - g_servo_offset;}
125126

126127
return g_servo_offset;
@@ -177,7 +178,7 @@ void uArmClass::saveDataToRom(double data, int address)
177178

178179
double uArmClass::readToAngle(double input_analog, byte servo_num, byte trigger)
179180
{
180-
int address = 60+(servo_num-1)*6;
181+
int address = OFFSET_START_ADDRESS +(servo_num-1)*6;
181182

182183
for (int i = 0; i<6;i++){
183184

@@ -210,19 +211,19 @@ double uArmClass::readAngle(byte servo_num)
210211
{
211212
case SERVO_ROT_NUM:
212213

213-
return readToAngle(analogRead(kServoRotReadPin),SERVO_ROT_NUM,0);
214+
return readToAngle(analogRead(SERVO_ROT_ANALOG_PIN),SERVO_ROT_NUM,0);
214215
break;
215216

216217
case SERVO_LEFT_NUM:
217-
return readToAngle(analogRead(kServoLeftReadPin),SERVO_LEFT_NUM,0);
218+
return readToAngle(analogRead(SERVO_LEFT_ANALOG_PIN),SERVO_LEFT_NUM,0);
218219
break;
219220

220221
case SERVO_RIGHT_NUM:
221-
return readToAngle(analogRead(kServoRightReadPin),SERVO_RIGHT_NUM,0);
222+
return readToAngle(analogRead(SERVO_RIGHT_ANALOG_PIN),SERVO_RIGHT_NUM,0);
222223
break;
223224

224225
case SERVO_HAND_ROT_NUM:
225-
return readToAngle(analogRead(kServoHandRotReadPin),SERVO_HAND_ROT_NUM,0);
226+
return readToAngle(analogRead(SERVO_HAND_ROT_ANALOG_PIN),SERVO_HAND_ROT_NUM,0);
226227
break;
227228

228229
default:
@@ -240,19 +241,19 @@ double uArmClass::readAngle(byte servo_num, byte trigger)
240241
switch (servo_num)
241242
{
242243
case SERVO_ROT_NUM:
243-
return readToAngle(analogRead(kServoRotReadPin),SERVO_ROT_NUM,trigger);
244+
return readToAngle(analogRead(SERVO_ROT_ANALOG_PIN),SERVO_ROT_NUM,trigger);
244245
break;
245246

246247
case SERVO_LEFT_NUM:
247-
return readToAngle(analogRead(kServoLeftReadPin),SERVO_LEFT_NUM,trigger);
248+
return readToAngle(analogRead(SERVO_LEFT_ANALOG_PIN),SERVO_LEFT_NUM,trigger);
248249
break;
249250

250251
case SERVO_RIGHT_NUM:
251-
return readToAngle(analogRead(kServoRightReadPin),SERVO_RIGHT_NUM,trigger);
252+
return readToAngle(analogRead(SERVO_RIGHT_ANALOG_PIN),SERVO_RIGHT_NUM,trigger);
252253
break;
253254

254255
case SERVO_HAND_ROT_NUM:
255-
return readToAngle(analogRead(kServoHandRotReadPin),SERVO_HAND_ROT_NUM,trigger);
256+
return readToAngle(analogRead(SERVO_HAND_ROT_ANALOG_PIN),SERVO_HAND_ROT_NUM,trigger);
256257
break;
257258

258259
default:
@@ -279,11 +280,19 @@ void uArmClass::calAngles(double x, double y, double z)
279280
z = MATH_L1 - MATH_L4 + BottomOffset;
280281
}
281282

282-
283-
g_y_in = (-y-MATH_L2)/MATH_L3;
284-
g_z_in = (z - MATH_L1) / MATH_L3;
285-
g_right_all = (1 - g_y_in*g_y_in - g_z_in*g_z_in - MATH_L43*MATH_L43) / (2 * MATH_L43);
286-
g_sqrt_z_y = sqrt(g_z_in*g_z_in + g_y_in*g_y_in);
283+
double x_in = 0.0;
284+
double y_in = 0.0;
285+
double z_in = 0.0;
286+
double right_all = 0.0;
287+
double right_all_2 = 0.0;
288+
double sqrt_z_x = 0.0;
289+
double sqrt_z_y = 0.0;
290+
double phi = 0.0;
291+
292+
y_in = (-y-MATH_L2)/MATH_L3;
293+
z_in = (z - MATH_L1) / MATH_L3;
294+
right_all = (1 - y_in*y_in - z_in*z_in - MATH_L43*MATH_L43) / (2 * MATH_L43);
295+
sqrt_z_y = sqrt(z_in*z_in + y_in*y_in);
287296

288297

289298
if (x == 0)
@@ -292,17 +301,17 @@ void uArmClass::calAngles(double x, double y, double z)
292301
g_theta_1 = 90;
293302

294303
// Calculate value of theta 3
295-
if (g_z_in == 0) {
296-
g_phi = 90;
304+
if (z_in == 0) {
305+
phi = 90;
297306
}
298307

299308
else {
300-
g_phi = atan(-g_y_in / g_z_in)*MATH_TRANS;
309+
phi = atan(-y_in / z_in)*MATH_TRANS;
301310
}
302311

303-
if (g_phi > 0) g_phi = g_phi - 180;
312+
if (phi > 0) phi = phi - 180;
304313

305-
g_theta_3 = asin(g_right_all / g_sqrt_z_y)*MATH_TRANS - g_phi;
314+
g_theta_3 = asin(right_all / sqrt_z_y)*MATH_TRANS - phi;
306315
if(g_theta_3<0)
307316
{
308317
g_theta_3 = 0;
@@ -332,19 +341,19 @@ void uArmClass::calAngles(double x, double y, double z)
332341

333342
// Calculate value of theta 3
334343

335-
g_x_in = (-x / cos(g_theta_1 / MATH_TRANS) - MATH_L2) / MATH_L3;
344+
x_in = (-x / cos(g_theta_1 / MATH_TRANS) - MATH_L2) / MATH_L3;
336345

337-
if (g_z_in == 0){ g_phi = 90; }
346+
if (z_in == 0){ phi = 90; }
338347

339-
else{ g_phi = atan(-g_x_in / g_z_in)*MATH_TRANS; }
348+
else{ phi = atan(-x_in / z_in)*MATH_TRANS; }
340349

341-
if (g_phi > 0) {g_phi = g_phi - 180;}
350+
if (phi > 0) {phi = phi - 180;}
342351

343-
g_sqrt_z_x = sqrt(g_z_in*g_z_in + g_x_in*g_x_in);
352+
sqrt_z_x = sqrt(z_in*z_in + x_in*x_in);
344353

345-
g_right_all_2 = -1 * (g_z_in*g_z_in + g_x_in*g_x_in + MATH_L43*MATH_L43 - 1) / (2 * MATH_L43);
346-
g_theta_3 = asin(g_right_all_2 / g_sqrt_z_x)*MATH_TRANS;
347-
g_theta_3 = g_theta_3 - g_phi;
354+
right_all_2 = -1 * (z_in*z_in + x_in*x_in + MATH_L43*MATH_L43 - 1) / (2 * MATH_L43);
355+
g_theta_3 = asin(right_all_2 / sqrt_z_x)*MATH_TRANS;
356+
g_theta_3 = g_theta_3 - phi;
348357

349358
if (g_theta_3 <0 ) {
350359
g_theta_3 = 0;
@@ -353,7 +362,7 @@ void uArmClass::calAngles(double x, double y, double z)
353362

354363
// Calculate value of theta 2
355364

356-
g_theta_2 = asin(g_z_in + MATH_L43*sin(abs(g_theta_3 / MATH_TRANS)))*MATH_TRANS;
365+
g_theta_2 = asin(z_in + MATH_L43*sin(abs(g_theta_3 / MATH_TRANS)))*MATH_TRANS;
357366

358367
}
359368

@@ -409,12 +418,12 @@ void uArmClass::interpolation(double init_val, double final_val)
409418
void uArmClass::calXYZ(double theta_1, double theta_2, double theta_3)
410419
{
411420

412-
g_l3_1 = MATH_L3 * cos(theta_2 / MATH_TRANS);
413-
g_l4_1 = MATH_L4*cos(theta_3 / MATH_TRANS);
414-
g_l5 = (MATH_L2 + MATH_L3*cos(theta_2 / MATH_TRANS) + MATH_L4*cos(theta_3 / MATH_TRANS));
421+
// g_l3_1 = MATH_L3 * cos(theta_2 / MATH_TRANS);
422+
// g_l4_1 = MATH_L4*cos(theta_3 / MATH_TRANS);
423+
double l5 = (MATH_L2 + MATH_L3*cos(theta_2 / MATH_TRANS) + MATH_L4*cos(theta_3 / MATH_TRANS));
415424

416-
g_cal_x = -cos(abs(theta_1 / MATH_TRANS))*g_l5;
417-
g_cal_y = -sin(abs(theta_1 / MATH_TRANS))*g_l5;
425+
g_cal_x = -cos(abs(theta_1 / MATH_TRANS))*l5;
426+
g_cal_y = -sin(abs(theta_1 / MATH_TRANS))*l5;
418427
g_cal_z = MATH_L1 + MATH_L3*sin(abs(theta_2 / MATH_TRANS)) - MATH_L4*sin(abs(theta_3 / MATH_TRANS));
419428

420429
}
@@ -486,9 +495,9 @@ void uArmClass::moveTo(double x, double y, double z, int relative, double time_s
486495
double z_arr[50];
487496

488497
calXYZ();
489-
g_current_x = g_cal_x;
490-
g_current_y = g_cal_y;
491-
g_current_z = g_cal_z;
498+
double current_x = g_cal_x;
499+
double current_y = g_cal_y;
500+
double current_z = g_cal_z;
492501

493502
if ((relative !=0)&&(relative != 1))
494503
{
@@ -502,23 +511,23 @@ void uArmClass::moveTo(double x, double y, double z, int relative, double time_s
502511

503512
// }
504513

505-
interpolation(g_current_x, g_current_x*relative+x);
514+
interpolation(current_x, current_x*relative+x);
506515

507516
for (byte i = 0; i < 50; i++){
508517

509518
x_arr[i] = g_interpol_val_arr[i];
510519

511520
}
512521

513-
interpolation(g_current_y, g_current_y*relative+y);
522+
interpolation(current_y, current_y*relative+y);
514523

515524
for (byte i = 0; i < 50; i++){
516525

517526
y_arr[i] = g_interpol_val_arr[i];
518527

519528
}
520529

521-
interpolation(g_current_z, g_current_z*relative+z);
530+
interpolation(current_z, current_z*relative+z);
522531

523532
for (byte i = 0; i < 50; i++){
524533

@@ -549,16 +558,16 @@ void uArmClass::moveToAtOnce(double x, double y, double z, int relative, double
549558

550559

551560
calXYZ();
552-
g_current_x = g_cal_x;
553-
g_current_y = g_cal_y;
554-
g_current_z = g_cal_z;
561+
double current_x = g_cal_x;
562+
double current_y = g_cal_y;
563+
double current_z = g_cal_z;
555564

556565
if ((relative !=0)&&(relative != 1))
557566
{
558567
relative = 0;
559568
}
560569

561-
calAngles(g_current_x*relative+x,g_current_y*relative+y,g_current_z*relative+z);
570+
calAngles(current_x*relative+x,current_y*relative+y,current_z*relative+z);
562571
l_movementTrigger = 1;
563572
uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3, servo_4_angle);
564573

@@ -575,9 +584,9 @@ void uArmClass::moveTo(double x, double y, double z, int relative, double time_s
575584

576585

577586
calXYZ();
578-
g_current_x = g_cal_x;
579-
g_current_y = g_cal_y;
580-
g_current_z = g_cal_z;
587+
double current_x = g_cal_x;
588+
double current_y = g_cal_y;
589+
double current_z = g_cal_z;
581590

582591
if ((relative !=0)&&(relative != 1))
583592
{
@@ -594,26 +603,26 @@ void uArmClass::moveTo(double x, double y, double z, int relative, double time_s
594603
time_spend = abs(time_spend);
595604
}
596605

597-
interpolation(g_current_x, g_current_x*relative+x);
606+
interpolation(current_x, current_x*relative+x);
598607

599608
for (byte i = 0; i < 50; i++){
600609

601610
x_arr[i] = g_interpol_val_arr[i];
602611

603612
}
604613

605-
interpolation(g_current_y, g_current_y*relative+y);
614+
interpolation(current_y, current_y*relative+y);
606615

607616
for (byte i = 0; i < 50; i++){
608617

609618
y_arr[i] = g_interpol_val_arr[i];
610619

611620
}
612621

613-
if ( g_current_z*relative+z>25)
614-
{ interpolation(g_current_z, 25); }
622+
if ( current_z*relative+z>25)
623+
{ interpolation(current_z, 25); }
615624
else
616-
{ interpolation(g_current_z, g_current_z*relative+z); }
625+
{ interpolation(current_z, current_z*relative+z); }
617626

618627
for (byte i = 0; i < 50; i++){
619628

@@ -651,9 +660,9 @@ void uArmClass::drawCur(double length_1, double length_2, int angle, double time
651660
double l_yp;
652661

653662
calXYZ();
654-
g_current_x = g_cal_x;
655-
g_current_y = g_cal_y;
656-
g_current_z = g_cal_z;
663+
double current_x = g_cal_x;
664+
double current_y = g_cal_y;
665+
double current_z = g_cal_z;
657666

658667
interpolation(0, angle/MATH_TRANS);
659668

@@ -663,7 +672,7 @@ void uArmClass::drawCur(double length_1, double length_2, int angle, double time
663672
l_xp = length_1 * cos(g_interpol_val_arr[i]);
664673
l_yp = length_2 * sin(g_interpol_val_arr[i]);
665674

666-
calAngles( l_xp + g_current_x - length_1 , l_yp+ g_current_y , g_current_z);
675+
calAngles( l_xp + current_x - length_1 , l_yp+ current_y , current_z);
667676
l_movementTrigger = 1;
668677
uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3,0);
669678

@@ -677,11 +686,11 @@ double uArmClass::calYonly(double theta_1, double theta_2, double theta_3)
677686

678687
{
679688

680-
g_l3_1_2 = MATH_L3 * cos(theta_2 / MATH_TRANS);
681-
g_l4_1_2 = MATH_L4*cos(theta_3 / MATH_TRANS);
682-
g_l5_2 = (MATH_L2 + MATH_L3*cos(theta_2 / MATH_TRANS) + MATH_L4*cos(theta_3 / MATH_TRANS));
689+
//g_l3_1_2 = MATH_L3 * cos(theta_2 / MATH_TRANS);
690+
//g_l4_1_2 = MATH_L4*cos(theta_3 / MATH_TRANS);
691+
double l5_2 = (MATH_L2 + MATH_L3*cos(theta_2 / MATH_TRANS) + MATH_L4*cos(theta_3 / MATH_TRANS));
683692

684-
return -sin(abs(theta_1 / MATH_TRANS))*g_l5_2;
693+
return -sin(abs(theta_1 / MATH_TRANS))*l5_2;
685694

686695
}
687696

0 commit comments

Comments
 (0)