Diff: main.cpp

Revision:

3:de8d3ca44a3e

Parent:

2:0a7a3c0c08d3

Child:

4:49dfbfcd3577

--- a/main.cpp	Mon Oct 29 14:02:47 2018 +0000
+++ b/main.cpp	Mon Oct 29 15:01:52 2018 +0000
@@ -4,13 +4,13 @@
 #include "MODSERIAL.h"
 #define PI 3.14159265
 
-MODSERIAL pc(USBTX, USBRX);
-DigitalIn button1(SW3);
-DigitalIn button2(SW2);
+MODSERIAL pc(USBTX, USBRX);  // connecting to pc
+DigitalIn button1(SW3);      
+DigitalIn button2(SW2);     
 DigitalOut LED(LED1);
 
 //Joe dit zijn de inputsignalen
-double theta1; double theta4; double emg1; double emg2; double emg3; 
+double theta1 = PI*0.5; double theta4 = PI*0.5; double emg1; double emg2; double emg3; 
 // Joe dit zijn de outputsignalen
 double x; double y; 
 
@@ -21,14 +21,14 @@
 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
 //First define the position equation of x
 double xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
-double xendsqrt1 = 2*sqrt(xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
+double xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
 double xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
 double xend = (xendsum + xendsqrt1/xendsqrt2)/2;
 //Now define the pos. eq. of y
-double yendsum = le;
+double yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
 double yendsqrt1 = (-xbase/ll + cos(theta1)+cos(theta4))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1)+cos(theta4))- ll*(1+cos(theta1+theta4)))); 
-double yendsqrt2 = ll/5*(sin(theta1)+sin(theta4))+ sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
-float yend = (xendsum + xendsqrt1/xendsqrt2);
+double yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
+double yend = (yendsum + yendsqrt1/yendsqrt2);
 
 
 //Hier definieren we de functies
@@ -37,13 +37,15 @@
 
 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
 void xcor(){
+    if (button1 == 0){
                             //als emg1==voorbij treshold, 
-    theta1 = PI*0.33;        //double theta1-> plus een paar counts (emg*richting)
+    theta1 = PI*(theta1/PI + 0.1);        //double theta1-> plus een paar counts (emg*richting)
     theta4 = PI*0.5;          //double theta4-> plus een paar counts (emg*richting)
     xend = (xendsum + xendsqrt1/xendsqrt2)/2;
     //default = als x = xbase/2... break
     //end
     }
+}
 void ycor(){
     //als emg2 == voorbij treshold,
     //double theta1 -> plus counts (emg*richting)
@@ -58,12 +60,14 @@
 {
     
     pc.baud(115200);
-    pc.printf("%f\n", xend);
     //default = theta1 = theta4 = pi/2 
     emgcheck.attach(xcor, 0.001);
     emgcheck.attach(ycor, 0.001);
     emgcheck.attach(flip, 0.001);
-    
+    while(true){
+        pc.printf("%f %f \n", xend,yend);
+        wait(1);
+        }