Diff: main.cpp

Revision:

7:b59b762c537e

Parent:

6:59744dfe8ea7

Child:

8:697aa3c94209

diff -r 59744dfe8ea7 -r b59b762c537e main.cpp
--- a/main.cpp	Mon Oct 29 16:00:08 2018 +0000
+++ b/main.cpp	Tue Oct 30 10:15:03 2018 +0000
@@ -1,16 +1,18 @@
 #include "mbed.h"
 #include <math.h>
 #include <cmath>
-#include "MODSERIAL.h"
+#include "MODSERIAL.h"  
 #define PI 3.14159265
 
 MODSERIAL pc(USBTX, USBRX);  // connecting to pc
 DigitalIn button1(SW3);      
 DigitalIn button2(SW2);     
-DigitalOut LED(LED1);
+DigitalOut ledr(LED1);
+DigitalOut led2(LED2);
 
 //Joe dit zijn de inputsignalen
-double theta1 = PI*0.5; double theta4 = PI*0.5; double emg1; double emg2; double emg3; 
+double theta1 = PI*0.5; double theta4 = PI*0.5; double emg1; double emg2; double emg3; double thetaflip = 0;
+double omega1; double omega4;
 // Joe dit zijn de outputsignalen
 double x; double y; 
 
@@ -33,13 +35,15 @@
 
 //Hier definieren we de functies
 Ticker emgcheck;
+Ticker emgcheck2;
 
 
 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
-void xcor(){
-    
+void forward(){        //dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
+    // hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
+    //maar daar moet eerst inverse kinematics voor gebeuren.
     if (button1 == 0){                          //als emg1==voorbij treshold, 
-        theta1 = PI*(theta1/PI - 0.001);          //double theta1-> plus een paar counts (emg*richting)
+        theta1 = PI*(theta1/PI + 0.1);          //double theta1-> plus een paar counts (emg*richting)
                                                 //double theta4-> plus een paar counts (emg*richting)
     //default = als x = xbase/2... break, okee dit moet hier niet
     }
@@ -54,29 +58,52 @@
     yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
     yend = (yendsum + yendsqrt1/yendsqrt2);
 }
-void ycor(){}
+void demomode(){}   //Alleen nodig in de DEMOMODE
                    //als emg2 == voorbij treshold,
             //double theta1 -> plus counts (emg*richting)
                     //double theta4 -> plus counts (emg*richting)
                     //reken y door
                     //default = als y = default... break
                     //end
-    
-void flip(){}
+void flip(){
+    if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);}
+}
+
+void inverse(){
+    /*
+    We willen ergens beginnen
+    Dan de functie qi+1 = qi + (jacobian *(qi - qi-1)/deltaT)deltaT
+    if
+    */
+    theta1-1 = PI*0.5
+    theta4-1 = PI*0.5
+
+    theta11 = 
+    theta14 = 
+
+    theta1+1
+    theta4+1
+   
+}
 
 int main()
 {
     
     pc.baud(115200);
     //default = theta1 = theta4 = pi/2 
-    emgcheck.attach(xcor, 0.1);
+    emgcheck.attach(forward, 0.1);
+    emgcheck2.attach(flip, 0.1);
     while(true){
-              
+        if(xend !=xend){ledr = 0;}
+        else{ledr=1;}
+           
+        if(button2==0){pc.printf("Draai! Hij is plat!");}
+    
        if (button1 == 0){
-        pc.printf("\n\r %f %f \n\r", xend,yend);
-        pc.printf(" %f\n\r", theta1);
-        wait(1.5);
-        }
+     pc.printf("\n\r %f %f \n\r", xend,yend);
+       pc.printf(" %f\n\r", theta1);
+       wait(1.5);
+      }
         }