Diff: main.cpp

Revision:

13:51ae2da8da55

Parent:

10:b8c60fd468f1

Child:

14:4cf17b10e504

--- a/main.cpp	Sun Oct 06 13:14:40 2019 +0000
+++ b/main.cpp	Mon Oct 07 13:11:24 2019 +0000
@@ -2,6 +2,7 @@
 #include "MODSERIAL.h"
 #include "FastPWM.h"
 #include "QEI.h"
+#define PI 3.14159265
 
 Serial pc(USBTX, USBRX);            //verbinden met pc
 DigitalOut motor2_direction(D4);    //verbinden met motor 2 op board (altijd d4)
@@ -125,7 +126,74 @@
     
 }
 
+void kinematics1() {                                                //
+    double length_1;
+    volatile double theta;
+    volatile double length_2;
+    
+    class H_matrix {                                                //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
+        public:
+        int h1_1_1 = 1; 
+        int h1_1_2 = 0;
+        float h1_1_3 = sin(theta*PI/180)*(length_1+length_2);          //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
+        int h1_2_1 = 0;
+        int h1_2_2 = 1;
+        float h1_2_3 = cos(theta*PI/180)*(length_1+length_2);  
+        int h1_3_1 = 0;
+        int h1_3_2 = 0;
+        int h1_3_3 = 1;
+                }
+                H_matrix H;
+                
+    class Position_vector1 {                                        //positie vector gezien vanuit het uiteinde van de complete arm (coördinatiestelsel draait niet mee met de arm)
+        public:
+        float p1_1_1
+        float p1_2_1
+        float p1_3_1
+                        }
+    class Position_vector2 {                                         
+        public:
+        float p2_1_1 = h1_1_1*p1_1_1 + h1_1_2*p1_2_1 + h1_1_3*p1_3_1
+        float p2_2_1 = h1_2_1*p1_1_1 + h1_2_2*p1_2_1 + h1_2_3*p1_3_1
+        float p2_3_1 = h1_3_1*p1_1_1 + h1_3_2*p1_2_1 + h1_3_3*p1_3_1
+        }                      
+                    }
 
+
+void kinematics2() {                                                //
+    double length_1;
+    volatile double theta;
+    volatile double length_2;
+    
+    class H_matrix2 {                                                   //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
+        public:
+        int h2_1_1 = 1; 
+        int h2_1_2 = 0;
+        float h2_1_3 = -sin(theta*PI/180)*(length_1+length_2);         //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
+        int h2_2_1 = 0;
+        int h2_2_2 = 1;
+        float h2_2_3 = -cos(theta*PI/180)*(length_1+length_2);  
+        int h2_3_1 = 0;
+        int h2_3_2 = 0;
+        int h2_3_3 = 1;
+                }
+                H_matrix H;
+                
+    class Position_vector3 {                                        //positie vector gezien vanuit het onderste draaipunt
+        public:
+        float p3_1_1
+        float p3_2_1
+        float p3_3_1
+                        }
+    class Position_vector4 {                                         
+        public:
+        float p4_1_1 = h2_1_1*p3_1_1 + h2_1_2*p3_2_1 + h2_1_3*p3_3_1
+        float p4_2_1 = h2_2_1*p1_1_1 + h2_2_2*p1_2_1 + h2_2_3*p3_3_1
+        float p4_3_1 = h2_3_1*p1_1_1 + h2_3_2*p1_2_1 + h2_3_3*p3_3_1  
+                            }
+                            H_matrix2 H2
+                            }
+                            
 void rotate_cw() {
     if (state_changed) {
         pc.printf("State changed to CW\r\n");