Projectgroep 20 Biorobotics / Mbed 2 deprecated rki_psuedoinverse

sexy script

Dependencies:   MODSERIAL mbed

Files at this revision

API Documentation at this revision

Revision 2:935a0c78bc68, committed 2017-11-03

Comitter:
Miriam
Date:
Fri Nov 03 00:08:57 2017 +0000
Parent:
1:f7c0b8de5cbb
Commit message:
extra kinematica doneeee (zonder positie, maar met snelheidsbepaling)

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r f7c0b8de5cbb -r 935a0c78bc68 main.cpp
--- a/main.cpp	Thu Nov 02 23:37:30 2017 +0000
+++ b/main.cpp	Fri Nov 03 00:08:57 2017 +0000
@@ -13,9 +13,12 @@
 double vy;
 double q1_dif;
 double q2_dif;
-double q1=0;
-double q2=0;
+double q1=1;
+double q2=1;
 double T=0.01; 
+double p;
+double q1set;
+double q2set;
 DigitalIn Knopje1 (PTA4);       //tijdelijk
 DigitalIn Knopje2 (PTC6);       //tijdelijk
 
@@ -37,8 +40,10 @@
         vy = 0;}
     else
     {}
+    
+p = q1+q2;
 Ox = -(0.38*cos(q1+q2) + 0.3*cos(q1));
-Oy = -0.38*sin(q1+q2) -0.3*sin(q1);
+Oy = -0.38*sin(q1+q2) - 0.3*sin(q1);
 DetJaq = Oy*0.38*cos(q1+q2) + 0.38*sin(q1+q2)*Ox;
 Jaqr1k1 = (0.38*cos(q1+q2)*vx)/DetJaq;
 Jaqr1k2 = (0.38*sin(q1+q2)*vy)/DetJaq;
@@ -54,5 +59,6 @@
 {
     Treecko.attach(&IKHAATROBOTS, T);
     pc.baud(115200);
-    pc.printf("vx = %f, vy = %f, q1 = %f, q2 = %f \n\r",vx, vy, q1, q2);
+    while (true){
+    pc.printf("vx = %f, vy = %f, q1 = %f, q2 = %f \n\r",vx, vy, q1, q2);}
 }
\ No newline at end of file