Diff: main.cpp

Revision:

13:f77c5f196161

Parent:

12:8d3bc1fa2321

Child:

14:e3fe54f0a4b4

diff -r 8d3bc1fa2321 -r f77c5f196161 main.cpp
--- a/main.cpp	Wed Oct 31 16:05:53 2018 +0000
+++ b/main.cpp	Wed Oct 31 17:36:26 2018 +0000
@@ -1,16 +1,21 @@
+//Libraries 
 #include "mbed.h"
 #include <math.h>
 #include <cmath>
 #include "MODSERIAL.h"
+#include "BiQuad.h"
+#include <algorithm>
 #define PI 3.14159265
-
 MODSERIAL pc(USBTX, USBRX);  // connecting to pc
-DigitalIn button1(SW3);      // defining testbutton NEEDS TO BE REMOVED
-DigitalOut ledr(LED1);      // Only for testing
-//DigitalOut led2(LED2);      // Only for testing
-InterruptIn button2(SW2);   //Only for testing
+
+
+
+
 
 // nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
+double omega1;
+double omega4;
+
 
 //Joe dit zijn de inputsignalen (en tussenvariabelen)
 //vorige theta
@@ -20,11 +25,10 @@
 bool emg2;
 bool emg3;
 double thetaflip = 0;
-double omega1;
-double omega4;
+
 double prefx;
 double prefy;
-double deltat = 0.01;
+double deltat = 0.01;                   //tijdstap(moet nog aangepast worden)
 //Joe dit zijn de constantes
 double ll = 200.0;
 double lu = 170.0;
@@ -42,7 +46,7 @@
 double xend;
 double jacobiana;
 double jacobianc;
-//Now define the pos. eq. of y
+//Now define the position equation of y
 double yendsum;
 double yendsqrt1;
 double yendsqrt2;
@@ -51,10 +55,18 @@
 double jacobiand;
 
 
-//Hier definieren we de functies en tickers
-Ticker emgcheck;
-Ticker emgcheck2;
-Ticker rekenen;
+//Timers and Tickers
+Ticker kin;                                                     //Timer for calculating x,y,theta1,theta4
+Ticker simulateval;                                             //Timer that prints the values for x,y, and angles
+Ticker rekenen;                                                 //Not used right now
+Ticker ReadUseEMG0_timer;                                       //Timer to read, filter and use the EMG
+Ticker EMGCalibration0_timer;                                   //Timer for the calibration of the EMG   
+Ticker FindMax0_timer;                                          //Timer for finding the max muscle
+Ticker ReadUseEMG1_timer;                                       //Timer to read, filter and use the EMG
+Ticker EMGCalibration1_timer;                                   //Timer for the calibration of the EMG   
+Ticker FindMax1_timer;                                          //Timer for finding the max muscle
+Ticker SwitchState_timer;                                       //Timer to switch from the Calibration to the working mode
+
 
 
 //dit wordt aangeroepen in de tickerfunctie
@@ -228,7 +240,10 @@
 
 
 //tot aan hier overslaan
+void printvalue(){
+    pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend);           // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
 
+}
 
 
 int main()
@@ -239,16 +254,19 @@
     theta4 = PI*0.49;
     pc.baud(115200);
     //default = theta1 = theta4 = pi/2, 
-    emgcheck.attach(kinematics, 0.01); // roep de ticker aan (
-
-
+    kin.attach(kinematics, 0.01); // roep de ticker aan (
+    simulateval.attach(printvalue, 1);
+    greenled = 1;                                               //First turn the LEDs off
+    blueled = 1;
+    redled = 1;
+    filter0.add(&Notch50_0).add(&High0);                        //Make filter chain for the first EMG
+    filter1.add(&Notch50_1).add(&High1);                        //Make filter chain for the second EMG
+    button.rise(StopProgram);                                   //If the button is pressed, stop program
 
     pc.printf("%f", theta1);
     while(true) {
 
-
-        pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend); // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
-        wait(0.5);//anders krijg je DOEZEND waardes...
+        StateMachine();                                                          //Start the state machine
 
 
     }