Coding help for Implementing formulas etc.

khordi · February 5, 2018, 5:48pm

Hi, so first of im not the best at coding and not sure where to go from here, if you guys could help that would be great!

I made some equations on MATLAB where it had alot of variables and equations, how do implement this so the two variable i get out that control two servos, use them to move? Any help on where to go from here would be awesome

cheers,

here is the code:

%=========== Scientific Constants ===========
g = 9.81; % m.s^-2
%============================================
%%========= Spring Constants (g/mm) =========
k1 = 338.28;
k2 = 338.28;
%============================================
%%============== Masses (kg)=================
S2 = 0.135;
L = 50;
m1 = 192.14;
m2 = 164.44;
m3 = 89.5;
m4 = 39.62;
m5 = 80; % Is a spring
m6 = 80; % Is a spring
%============================================
%%== Angles (would be sampled from servos) ==
theta1 = pi () / 3;
theta2 = pi () / 6;
%============================================
%%=============== Arm Lenghts ===============
r1 = 224.5;
r2 = 370;
r3 = 210;
r4 = 70;
A1A3 = 69.5; % Line from joint A1 to A3 (intersection of r1 and r4)
R1_1 = 75; % Length of arm r1 beyond point A1
%============================================
%%============ Calculated Angles ============
sinT1 = sin (theta1);
sinT2 = sin (theta2);
%============================================
%%======== Calculated Arm Lengths ===========
b1 = R1_1;
b2 = r4;
R1 = 0.5 * r1 - 0.5 * R1_1;
R2 = 0.5 * r2 - 0.5 * r4;
%=========== Calculated Moments =============
%-------------------------------------
ML1 = L * r2 * sinT2;
ML2 = L * r2 * sinT2 + L * r1 * sinT1; %Moments around Load
%-------------------------------------
MAR1 = m1 * R1 * sinT1; %Moments around ARM R1
%-------------------------------------
MR21 = m2 * r1 * sinT1 + m2 * R2 * sinT2; %Moments around ARM R2
MR22 = m2 * r2 * sinT2;
%-------------------------------------
MR31 = m3 * 0.5 * r3 * sinT1 - m3 * r4 * sinT2; %Moments around ARM R3
MR32 = m3 * 0.5 * r3 * sinT1 - m3 * r4 * sinT2 - m3 * r1 * sinT1;
%------------------------------------- %Moments around ARM R4
MR41 = m4 * A1A3 * sinT1 - m4 * 0.5 * r4 * sinT2;
MR42 = m4 * A1A3 * sinT1 - m4 * 0.5 * r4 * sinT2 - m4 * r1 * sinT1;
%------------------------------------- %Moments around ARM R5
MR51 = m5 * 0.5 * R1_1 * sinT1;
%------------------------------------- %Moments around ARM R6
MR61 = -m6 * 0.5 * r4 * sinT2;
MR62 = -m6 * 0.5 * r4 * sinT2 - m6 * r1 * sinT1;
%------------------------------------- %Moments around Servo 2
MRS = S2 * r1 * sinT1;
%=========== Spring Equations ==============
C1 = ((ML1+MAR1+MR21+MR31+MR41+MR51+MR61+MRS)/(k1b1sinT1))

C2 = ((ML2+MR22+MR32+MR42+MR61+MR62)/(k2b2sinT2))
%===========================================
disp(C1)
disp(C2)

RayLivingston · February 5, 2018, 6:01pm

I hope you don't need to move fast, because all those calculations will take a LOOOOOOOONG time on an Arduino....

Regards,
Ray L.

khordi · February 5, 2018, 6:04pm

Its for a robotic arm, do you have a solution on how to code this in a more simplistic way?

thanks,

mark

johnwasser · February 5, 2018, 6:39pm

Step 1 is to change the "%" at the start of each comment to "//" to make them C++ comments

Step 2 is to declare the type of each variable. Many will be 'float'. Some appear to be 'int'.

Step 3: Change 'pi()' to PI

Step 4: Add the required setup() and loop() functions.

Step 5: Decide what the undeclared 'disp()' function is supposed to do and declare it.

That is where I get stuck. Here is everything up to Step 5:

//=========== Scientific Constants ===========
const float g = 9.81;                                      // m.s ^ -2
//============================================
//========= Spring Constants (g/mm) =========
const float k1 = 338.28;
const float k2 = 338.28;
//============================================
//============== Masses (kg)=================
const float S2 = 0.135;
const int L = 50;
const float m1 = 192.14;
const float m2 = 164.44;
const float m3 = 89.5;
const float m4 = 39.62;
const int m5 = 80;                                       // Is a spring
const int m6 = 80;                                        // Is a spring
//============================================
// == Angles (would be sampled from servos) ==
const float theta1 = PI / 3;
const float theta2 = PI / 6;
//============================================
//=============== Arm Lenghts ===============
const float r1 = 224.5;
const int r2 = 370;
const int r3 = 210;
const int r4 = 70;
const float A1A3 = 69.5;                        // Line from joint A1 to A3 (intersection of r1 and r4)
const int R1_1 = 75;                                      // Length of arm r1 beyond point A1
//============================================
//============ Calculated Angles ============
const float sinT1 = sin (theta1);
const float sinT2 = sin (theta2);
//============================================
//======== Calculated Arm Lengths ===========
const int b1 = R1_1;
const int b2 = r4;
const float R1 = 0.5 * r1 - 0.5 * R1_1;
const float R2 = 0.5 * r2 - 0.5 * r4;
//=========== Calculated Moments =============
//-------------------------------------
const float ML1 = L * r2 * sinT2;
const float ML2 = L * r2 * sinT2 + L * r1 * sinT1;          // Moments around Load
//-------------------------------------
const float MAR1 = m1 * R1 * sinT1;                         // Moments around ARM R1
//-------------------------------------
const float MR21 = m2 * r1 * sinT1 + m2 * R2 * sinT2;       // Moments around ARM R2
const float MR22 = m2 * r2 * sinT2;
//-------------------------------------
const float MR31 = m3 * 0.5 * r3 * sinT1 - m3 * r4 * sinT2;  // Moments around ARM R3
const float MR32 = m3 * 0.5 * r3 * sinT1 - m3 * r4 * sinT2 - m3 * r1 * sinT1;
//-------------------------------------          // Moments around ARM R4
const float MR41 = m4 * A1A3 * sinT1 - m4 * 0.5 * r4 * sinT2;
const float MR42 = m4 * A1A3 * sinT1 - m4 * 0.5 * r4 * sinT2 - m4 * r1 * sinT1;
//-------------------------------------          // Moments around ARM R5
const float MR51 = m5 * 0.5 * R1_1 * sinT1;
//-------------------------------------          // Moments around ARM R6
const float MR61 = -m6 * 0.5 * r4 * sinT2;
const float MR62 = -m6 * 0.5 * r4 * sinT2 - m6 * r1 * sinT1;
//-------------------------------------          // Moments around Servo 2
const float MRS = S2 * r1 * sinT1;
//=========== Spring Equations ==============
const float C1 = ((ML1 + MAR1 + MR21 + MR31 + MR41 + MR51 + MR61 + MRS) / (k1*b1*sinT1));


const float C2 = ((ML2 + MR22 + MR32 + MR42 + MR61 + MR62) / (k2*b2*sinT2));
//===========================================


void setup() {
  disp(C1);
  disp(C2);
}
void loop() {}

el_supremo · February 5, 2018, 6:43pm

If those calculations would have enough precision with single precision floating point, you could use a Teensy 3.6 which has hardware single-precision floating point.

Pete

khordi · February 5, 2018, 11:42pm

johnwasser:
Step 1 is to change the "%" at the start of each comment to "//" to make them C++ comments

Step 2 is to declare the type of each variable. Many will be 'float'. Some appear to be 'int'.

Step 3: Change 'pi()' to PI

Step 4: Add the required setup() and loop() functions.

Step 5: Decide what the undeclared 'disp()' function is supposed to do and declare it.

That is where I get stuck. Here is everything up to Step 5:
//=========== Scientific Constants ===========

const float g = 9.81; // m.s ^ -2
//============================================
//========= Spring Constants (g/mm) =========
const float k1 = 338.28;
const float k2 = 338.28;
//============================================
//============== Masses (kg)=================
const float S2 = 0.135;
const int L = 50;
const float m1 = 192.14;
const float m2 = 164.44;
const float m3 = 89.5;
const float m4 = 39.62;
const int m5 = 80; // Is a spring
const int m6 = 80; // Is a spring
//============================================
// == Angles (would be sampled from servos) ==
const float theta1 = PI / 3;
const float theta2 = PI / 6;
//============================================
//=============== Arm Lenghts ===============
const float r1 = 224.5;
const int r2 = 370;
const int r3 = 210;
const int r4 = 70;
const float A1A3 = 69.5; // Line from joint A1 to A3 (intersection of r1 and r4)
const int R1_1 = 75; // Length of arm r1 beyond point A1
//============================================
//============ Calculated Angles ============
const float sinT1 = sin (theta1);
const float sinT2 = sin (theta2);
//============================================
//======== Calculated Arm Lengths ===========
const int b1 = R1_1;
const int b2 = r4;
const float R1 = 0.5 * r1 - 0.5 * R1_1;
const float R2 = 0.5 * r2 - 0.5 * r4;
//=========== Calculated Moments =============
//-------------------------------------
const float ML1 = L * r2 * sinT2;
const float ML2 = L * r2 * sinT2 + L * r1 * sinT1; // Moments around Load
//-------------------------------------
const float MAR1 = m1 * R1 * sinT1; // Moments around ARM R1
//-------------------------------------
const float MR21 = m2 * r1 * sinT1 + m2 * R2 * sinT2; // Moments around ARM R2
const float MR22 = m2 * r2 * sinT2;
//-------------------------------------
const float MR31 = m3 * 0.5 * r3 * sinT1 - m3 * r4 * sinT2; // Moments around ARM R3
const float MR32 = m3 * 0.5 * r3 * sinT1 - m3 * r4 * sinT2 - m3 * r1 * sinT1;
//------------------------------------- // Moments around ARM R4
const float MR41 = m4 * A1A3 * sinT1 - m4 * 0.5 * r4 * sinT2;
const float MR42 = m4 * A1A3 * sinT1 - m4 * 0.5 * r4 * sinT2 - m4 * r1 * sinT1;
//------------------------------------- // Moments around ARM R5
const float MR51 = m5 * 0.5 * R1_1 * sinT1;
//------------------------------------- // Moments around ARM R6
const float MR61 = -m6 * 0.5 * r4 * sinT2;
const float MR62 = -m6 * 0.5 * r4 * sinT2 - m6 * r1 * sinT1;
//------------------------------------- // Moments around Servo 2
const float MRS = S2 * r1 * sinT1;
//=========== Spring Equations ==============
const float C1 = ((ML1 + MAR1 + MR21 + MR31 + MR41 + MR51 + MR61 + MRS) / (k1b1sinT1));

const float C2 = ((ML2 + MR22 + MR32 + MR42 + MR61 + MR62) / (k2b2sinT2));
//===========================================

void setup() {
disp(C1);
disp(C2);
}
void loop() {}

Awesome thanks, maybe just do a serial.print so i can verify i get the correct output. thanks for your help and time

Topic		Replies	Views
beginner code needs a bit of work please Syntax & Programs	12	3478	May 6, 2021
Control 2 servos with a matlab script Interfacing w/ Software on the Computer	2	701	May 6, 2021
mathematics to servo Programming	9	1639	May 5, 2021
Arrays and Servos Programming	8	1546	May 6, 2021
need help with code I try to modify...aint working Syntax & Programs	2	789	May 6, 2021

Coding help for Implementing formulas etc.

Related topics