Diff: main.cpp

Revision:

9:6ce7218be0ee

Parent:

8:5896424eb539

--- a/main.cpp	Tue Nov 06 14:45:19 2018 +0000
+++ b/main.cpp	Tue Nov 06 15:11:31 2018 +0000
@@ -37,9 +37,9 @@
 
 const double sampleTime = 0.001;
 const double PI = 3.141592653589793238463;
-const double  L1 = 0.328;
-const double L2 = 0.218;
-double T1[3][3] {
+const double  L1 = 0.328;                       // Length of Arm 1
+const double L2 = 0.218;                        // Length of Arm 2
+double T1[3][3] {                               // Twist of joints in reference configuration
     {0, -1, 0},
     {1, 0, 0,},
     {0, 0, 0,}
@@ -73,7 +73,7 @@
 FastPWM pwmpin4(A5);
 DigitalOut directionpin4(D9);
 
-double setPointX = 0.3;
+double setPointX = 0.3;                                     // Declaring intial setpoint that robot moves to in the moving state where EMG can then be used
 double setPointY = 0.28;
 double qRef1;
 double qRef2;
@@ -82,7 +82,7 @@
 
 double C[5][5];
 
-double Kp1 = 10;
+double Kp1 = 10;                                            // PID control values of motors 1 and 2 that actuate two different arms
 double Ki1 = 0;
 double Kd1 = 2;
 double Kp2 = 10;
@@ -212,7 +212,7 @@
 }
 
 template<std::size_t N, std::size_t M, std::size_t P>
-void mult(double A[N][M], double B[M][P])
+void mult(double A[N][M], double B[M][P])               // Matrix multiplication function calls matrix sizes and matrixs A and B 
 {
 
     for( int n =0; n < 5; n++) {