Diagonaal berekenen lukt, meerdere outputs nog niet

Dependencies:   mbed FastPWM HIDScope MODSERIAL QEI

Files at this revision

API Documentation at this revision

Comitter:
sjoerdbarts
Date:
Thu Oct 20 12:07:30 2016 +0000
Parent:
5:a9da7bc89b24
Commit message:
Initial working code

Changed in this revision

BiQuad.cpp Show annotated file Show diff for this revision Revisions of this file
BiQuad.h Show annotated file Show diff for this revision Revisions of this file
FastPWM.lib Show annotated file Show diff for this revision Revisions of this file
HIDScope.lib Show annotated file Show diff for this revision Revisions of this file
MODSERIAL.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r a9da7bc89b24 -r ee243782bf51 BiQuad.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad.cpp	Thu Oct 20 12:07:30 2016 +0000
@@ -0,0 +1,160 @@
+#include "BiQuad.h"
+
+BiQuad::BiQuad() {
+    resetStateOnGainChange = true;
+    set( 1.0, 0.0, 0.0, 0.0, 0.0 );
+}
+
+BiQuad::BiQuad(double b0, double b1, double b2, double a1, double a2) {
+    resetStateOnGainChange = true;
+    set( b0, b1, b2, a1, a2 );
+}
+
+BiQuad::BiQuad(double b0, double b1, double b2, double a0, double a1, double a2) {
+    resetStateOnGainChange = true;
+    set( b0/a0, b1/a0, b2/a0, a1/a0, a2/a0 );
+}
+
+void BiQuad::PIDF( double Kp, double Ki, double Kd, double N, double Ts  ) {
+
+    double b0, b1, b2, bd, a1, a2;
+
+    a1 = -4.0/(N*Ts+2.0);
+    a2 = -(N*Ts-2.0)/(N*Ts+2.0);
+
+    bd = ( N*Ts+2.0 );
+
+    b0 = ( 4.0*Kp + 4.0*Kd*N + 2.0*Ki*Ts + 2.0*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
+    b1 = ( Ki*N*Ts*Ts - 4.0*Kp - 4.0*Kd*N )/bd;
+    b2 = ( 4.0*Kp + 4.0*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
+
+    set( b0, b1, b2, a1, a2 );
+
+};
+
+void BiQuad::set(double b0, double b1, double b2, double a1, double a2) {
+
+    B[0] = b0; B[1] = b1; B[2] = b2;
+    A[0] = a1; A[1] = a2;
+
+    if( resetStateOnGainChange )
+        wz[0] = 0; wz[1] = 0;
+
+}
+
+double BiQuad::step(double x) {
+
+    double y,w;
+
+    /* Direct form II */
+    w =      x - A[0]*wz[0] - A[1]*wz[1];
+    y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];
+
+    /* Shift */
+    wz[1] = wz[0];
+    wz[0] = w;
+
+    return y;
+
+}
+
+std::vector< std::complex<double> > BiQuad::poles() {
+
+    std::vector< std::complex<double> > poles;
+
+    std::complex<double> b2(A[0]*A[0],0);
+    std::complex<double> ds = std::sqrt( b2-4*A[1] );
+
+    poles.push_back( 0.5*(-A[0]+ds) );
+    poles.push_back( 0.5*(-A[0]-ds) );
+
+    return poles;
+
+}
+
+std::vector< std::complex<double> > BiQuad::zeros() {
+
+    std::vector< std::complex<double> > zeros;
+
+    std::complex<double> b2(B[1]*B[1],0);
+    std::complex<double> ds = std::sqrt( b2-4*B[0]*B[2] );
+
+    zeros.push_back( 0.5*(-B[1]+ds)/B[0] );
+    zeros.push_back( 0.5*(-B[1]-ds)/B[0] );
+
+    return zeros;
+
+}
+
+bool BiQuad::stable() {
+    bool stable = true;
+    std::vector< std::complex<double> > ps = poles();
+    for( size_t i = 0; i < ps.size(); i++ )
+        stable = stable & ( std::abs( ps[i] ) < 1 );
+    return stable;
+}
+
+void BiQuad::setResetStateOnGainChange( bool v ){
+    resetStateOnGainChange = v;
+}
+
+BiQuadChain &BiQuadChain::add(BiQuad *bq) {
+    biquads.push_back( bq );
+    return *this;
+}
+
+BiQuadChain operator*( BiQuad &bq1, BiQuad &bq2 ) {
+    BiQuadChain bqc;
+    bqc.add( &bq1 ).add( &bq2 );
+    return bqc;
+}
+
+double BiQuadChain::step(double x) {
+
+    int i;
+    size_t bqs;
+
+    bqs = biquads.size();
+
+    for( i = 0; i < bqs; i++ )
+        x = biquads[i]->step( x );
+
+    return x;
+}
+
+std::vector< std::complex<double> > BiQuadChain::poles_zeros( bool zeros ) {
+
+    std::vector< std::complex<double> > chain, bq;
+    int i;
+    size_t bqs;
+
+    bqs = biquads.size();
+
+    for( i = 0; i < bqs; i++ ){
+        bq = ( zeros ) ? biquads[ i ]->zeros() : biquads[ i ]->poles();
+        chain.insert( chain.end(), bq.begin(), bq.end() );
+    }
+
+    return chain;
+
+}
+
+std::vector< std::complex<double> > BiQuadChain::poles() {
+    return poles_zeros( false );
+}
+
+std::vector< std::complex<double> > BiQuadChain::zeros() {
+    return poles_zeros( true );
+}
+
+bool BiQuadChain::stable() {
+    bool stable = true;
+    for( size_t i = 0; i < biquads.size(); i++ )
+        stable = stable & biquads[i]->stable();
+    return stable;
+}
+
+BiQuadChain& BiQuadChain::operator*( BiQuad& bq ) {
+    add( &bq );
+    return *this;
+}
\ No newline at end of file
diff -r a9da7bc89b24 -r ee243782bf51 BiQuad.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad.h	Thu Oct 20 12:07:30 2016 +0000
@@ -0,0 +1,219 @@
+#ifndef BIQUAD_BIQUAD_H
+#define BIQUAD_BIQUAD_H
+
+#include <vector>
+#include <complex>
+
+class BiQuadChain;
+
+/** BiQuad class implements a single filter
+ *
+ * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl>
+ *
+ * Filters that - in the z domain - are the ratio of two quadratic functions. The general form is:
+ *
+ *        b0 + b1 z^-1 + b2 z^-2
+ * H(z) = ----------------------
+ *        a0 + a1 z^-1 + a2 z^-2
+ *
+ * Which is often normalized by dividing all coefficients by a0.
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include <complex>
+ *
+ * // Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist)
+ * BiQuad bq1( 4.16599e-04, 8.33198e-04, 4.16599e-04, -1.47967e+00, 5.55822e-01 );
+ * BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.70096e+00, 7.88500e-01 );
+ *
+ * BiQuadChain bqc;
+ *
+ * int main() {
+ *
+ *    // Add the biquads to the chain
+ *    bqc.add( &bq1 ).add( &bq2 );
+ *
+ *    // Find the poles of the filter
+ *    std::cout << "Filter poles" << std::endl;
+ *    std::vector< std::complex<double> > poles = bqc.poles();
+ *    for( size_t i = 0; i < poles.size(); i++ )
+ *        std::cout << "\t"  << poles[i] << std::endl;
+ *
+ *    // Find the zeros of the filter
+ *    std::cout << "Filter zeros" << std::endl;
+ *    std::vector< std::complex<double> > zeros = bqc.zeros();
+ *    for( size_t i = 0; i < poles.size(); i++ )
+ *        std::cout << "\t" << zeros[i] << std::endl;
+ *
+ *    // Is the filter stable?
+ *    std::cout << "This filter is " << (bqc.stable() ? "stable" : "instable") << std::endl;
+ *
+ *    // Output the step-response of 20 samples
+ *  std::cout << "Step response 20 samples" << std::endl;
+ *  for( int i = 0; i < 20; i++ )
+ *      std::cout << "\t" << bqc.step( 1.0 ) << std::endl;
+ * }
+ * @endcode
+ *
+ * https://github.com/tomlankhorst/biquad
+ *
+ */
+class BiQuad {
+
+private:
+
+    double B[3];
+    double A[2];
+    double wz[2];
+
+    bool resetStateOnGainChange;
+
+    /**
+     * Sets the gain parameters
+     */
+    void set( double b0, double b1, double b2, double a1, double a2 );
+
+public:
+
+    /**
+     * Initialize a unity TF biquad
+     * @return BiQuad instance
+     */
+    BiQuad( );
+
+    /**
+     * Initialize a normalized biquad filter
+     * @param b0
+     * @param b1
+     * @param b2
+     * @param a1
+     * @param a2
+     * @return BiQuad instance
+     */
+    BiQuad( double b0, double b1, double b2, double a1, double a2 );
+
+    /**
+     * Initialize a biquad filter with all six coefficients
+     * @param b0
+     * @param b1
+     * @param b2
+     * @param a0
+     * @param a1
+     * @param a2
+     * @return BiQuad instance
+     */
+    BiQuad( double b0, double b1, double b2, double a0, double a1, double a2 );
+
+    /**
+     * Initialize a PIDF biquad.
+     * Based on Tustin-approx (trapezoidal) of the continous time version.
+     * Behaviour equivalent to the PID controller created with the following MATLAB expression:
+     *
+     * C = pid( Kp, Ki, Kd, 1/N, Ts, 'IFormula', 'Trapezoidal', 'DFormula', 'Trapezoidal' );
+     *
+     * @param Kp    Proportional gain
+     * @param Ki    Integral gain
+     * @param Kd    Derivative gain
+     * @param N     Filter coefficient ( N = 1/Tf )
+     * @param Ts    Timestep
+     */
+    void PIDF( double Kp, double Ki, double Kd, double N, double Ts  );
+
+    /**
+     * Execute one digital timestep and return the result...
+     * @param x input of the filer
+     * @return output of the filter
+     */
+    double step( double x );
+
+    /**
+     * Return poles of the BiQuad filter
+     * @return vector of std::complex poles
+     */
+    std::vector< std::complex<double> > poles( );
+
+    /**
+     * Return zeros of the BiQuad filter
+     * @return vector of std::complex zeros
+     */
+    std::vector< std::complex<double> > zeros( );
+
+    /**
+     * Is this biquad stable?
+     * Checks if all poles lie within the unit-circle
+     * @return boolean whether the filter is stable or not
+     */
+    bool stable ();
+
+    /**
+     * Determines if the state variables are reset to zero on gain change.
+     * Can be used for changing gain parameters on the fly.
+     * @param v Value of the reset boolean
+     */
+    void setResetStateOnGainChange( bool v );
+
+};
+
+/**
+ * The BiQuadChain class implements a chain of BiQuad filters
+ */
+class BiQuadChain {
+
+private:
+    std::vector< BiQuad* > biquads;
+    std::vector< std::complex<double> > poles_zeros( bool zeros = false );
+
+public:
+
+    /**
+     * Add a BiQuad pointer to the list: bqc.add(&bq);
+     * @param bq Pointer to BiQuad instance
+     * @return Pointer to BiQuadChain
+     */
+    BiQuadChain &add( BiQuad *bq );
+
+    /**
+     * Execute a digital time step cascaded through all bq's
+     * @param x Input of the filter chain
+     * @return Output of the chain
+     */
+    double step(double x);
+
+    /**
+     * Return poles of the BiQuad filter
+     * @return vector of std::complex poles
+     */
+    std::vector< std::complex<double> > poles( );
+
+    /**
+     * Return zeros of the BiQuad filter
+     * @return vector of std::complex zeros
+     */
+    std::vector< std::complex<double> > zeros( );
+
+    /**
+     * Is this biquad-chain stable?
+     * Checks if all poles lie within the unit-circle
+     * @return boolean whether the chain is stable or not
+     */
+    bool stable ();
+
+    /**
+     * Appends a BiQuad to the chain
+     * Shorthand for .add(&bq)
+     * @param bq BiQuad
+     * @return Pointer to BiQuadChain
+     */
+    BiQuadChain &operator*( BiQuad& bq );
+
+};
+
+/**
+ * Multiply two BiQuads
+ * ... which in fact means appending them into a BiQuadChain
+ * @return BiQuadChain of the two BiQuads
+ */
+BiQuadChain operator*( BiQuad&, BiQuad& );
+
+#endif //BIQUAD_BIQUAD_H
\ No newline at end of file
diff -r a9da7bc89b24 -r ee243782bf51 FastPWM.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Thu Oct 20 12:07:30 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/sjoerdbarts/code/FastPWM/#ff245801a00d
diff -r a9da7bc89b24 -r ee243782bf51 HIDScope.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Thu Oct 20 12:07:30 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/Biorobotics_group_2/code/HIDScope/#eade4ec5282b
diff -r a9da7bc89b24 -r ee243782bf51 MODSERIAL.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Thu Oct 20 12:07:30 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/Sissors/code/MODSERIAL/#4737f8a5b018
diff -r a9da7bc89b24 -r ee243782bf51 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Oct 20 12:07:30 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r a9da7bc89b24 -r ee243782bf51 main.cpp
--- a/main.cpp	Mon Oct 17 14:25:12 2016 +0000
+++ b/main.cpp	Thu Oct 20 12:07:30 2016 +0000
@@ -1,230 +1,315 @@
 #include "mbed.h"
-#include "math.h"
-#include "iostream"
+#include "FastPWM.h"
+#include "HIDScope.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
 #define SERIAL_BAUD 115200
-
-Serial pc(USBTX,USBRX);
-InterruptIn Button1(PTC6);          // DIT WORDT EEN ANDERE BUTTON
-
-// tickers
-Ticker TickerCaseBepalen;
-Ticker TickerVeranderenXY;
-Ticker TickerAfstandBerekenen;
-Ticker TickerMotorHoekBerekenen;
-Ticker TickerBerekenenBewegingsHoek;
-Ticker TickerFlipperBewegen;
-
-// in te voeren x en y waardes 
-volatile float x = 0.0;    
-volatile float y = 305.5; 
-volatile float vorigex = 0.0;
-volatile float vorigey = 0.0;
+ 
+MODSERIAL pc(USBTX,USBRX);
+// Set button pins
+InterruptIn button1(PTB9);
+InterruptIn button2(PTA1);
 
-// waardes
-volatile const float a = 50.0;       // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)   KEERTJE NAMETEN
-volatile const float Bar = 200.0;    // lengte van de armen                                                                     KEERTJE NAMETEN
-volatile const float pi = 3.14159265359; 
-volatile float VorigeHoek1 = 1.0/3.0*pi;     // begin hoek van 90 graden
-volatile float VorigeHoek2 = 1.0/3.0*pi; 
-volatile float Step = 1;    // EEN GOK VOOR HET STAPJE EN MOET DUS NOG AANGEPAST
-volatile float MaxHoek = pi - 30*pi/180;   // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel)
+// Set AnalogIn potmeters
+AnalogIn pot1(A3);
+AnalogIn pot2(A4);
 
-volatile float Dia1 = 0.0;
-volatile float Dia2 = 0.0;
-volatile float MotorHoek1 = 0.0;
-volatile float MotorHoek2 = 0.0;
-volatile float BewegingsHoek1 = 0.0;
-volatile float BewegingsHoek2 = 0.0;
-volatile bool Richting1 = true;     // cw = true, ccw = false
-volatile bool Richting2 = true;     
-volatile bool FlipperAan = false;   // als true dan flipper aanzetten
-bool BezigMetCalibreren = true;     // we beginnen met calibreren
-bool BeginnenMetFlippen = false;    // om de flipper aan te zetten
-bool BezigMetFlippen = false;       // om verandering in x en y tegen te houden tijdens het flippen. DIT WORDT VERANDERD IN SJOERDS DEEL (NAAM CONTROLEREN)
+// Set motor Pinouts
+DigitalOut motor1_dir(D4);
+FastPWM motor1_pwm(D5);
+DigitalOut motor2_dir(D7);
+FastPWM motor2_pwm(D6);
 
-// waardes die komen van Charlotte: true is aangespannen, false is niet aangespannen    (NAAM CONTROLEREN)
-volatile bool BicepsLinks = false;
-volatile bool BicepsRechts = true;
-volatile bool Been = false;
+// Set LED pins
+DigitalOut led(LED_RED);
 
-// nodig voor het switchstatement
-enum states {Flipper, Links, Rechts, Voor, Achter, GeenSignaal};
-states MyState = Links;
+// Set HID scope
+HIDScope    scope(2);
 
-// het bepalen van welke case we hebben
-void CaseBepalen () {
-    if (BicepsLinks==true && BicepsRechts==true && Been==true && BezigMetFlippen == false) {   
-        MyState = Flipper;      // flipper bewegen
-    }
-    else if (BicepsLinks==true && BicepsRechts==false && Been==false && BezigMetFlippen == false) {
-        MyState = Links;        // naar links bewegen
-    }
-    else if (BicepsLinks==false && BicepsRechts==true && Been==false && BezigMetFlippen == false) {
-        MyState = Rechts;       // naar rechts bewegen
-    }
-    else if (BicepsLinks==true && BicepsRechts==true && Been==false && BezigMetFlippen == false) {
-        MyState = Voor;         // naar voren bewegen
-    }
-    else if (BicepsLinks==false && BicepsRechts==true && Been==true && BezigMetFlippen == false) {
-        MyState = Achter;       // naar achter bewegen
-    }
-    else if (BicepsLinks==false && BicepsRechts==false && Been==false || BezigMetFlippen==true) {
-        MyState = GeenSignaal;
-    }
-}    
+// Set Encoders for motors
+QEI Motor1_ENC_CW(D11,D10,NC,32);
+QEI Motor1_ENC_CCW(D10,D11,NC,32);
+QEI Motor2_ENC_CW(D12,D13,NC,32);
+QEI Motor2_ENC_CCW(D13,D12,NC,32);
+
+// Constants
+volatile double pwm = 0.0;
+volatile double pwm_new = 0;
+volatile double timer = 0.0;
+volatile bool button1_value = false;
+volatile bool button2_value = false;
 
-// het berekenen van de nieuwe x en y en zorgen dat deze niet buiten het bereik komen
-void VeranderenXY () {
-        // bepalen hoe de x en y waardes gaan veranderen
-    switch(MyState) {
-        case Flipper :
-            BeginnenMetFlippen = true;     // we zijn aan het flipper dus geen andere dingen doen
-            break;
-        case Links : 
-            vorigex = x;    // ervoor zorgen dat als de motor te ver draait de x niet verder telt
-            x = x - Step;
-            break;
-        case Rechts :
-            vorigex = x;
-            x = x + Step;
-            break;
-        case Voor :
-            vorigey = y;    // ervoor zorgen dat als de motor te ver draait de y niet verder telt
-            y = y + Step;
-            break;
-        case Achter :
-            vorigey = y;
-            y = y - Step;
-            break;
-        case GeenSignaal :
-            break;
-        default :
-            pc.printf("Er is geen verandering in x en y\r\n");
-    }
-            
-    // Grenswaardes     LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
-    if (x > 200) {
-        x = 200;
-    }
-    else if (x < -200) {
-        x = -200;
-    }
-    if (y > 306) {
-        y = 306;
-    }
-    else if (y < 50) {
-        y = 50;         // GOKJE, UITPROBEREN
-    }
-    pc.printf("x = %f, y = %f\r\n", x, y);
+// Ticker
+Ticker SinTicker;
+Ticker PotControlTicker;
+Ticker SendZerosTicker;
+
+void SwitchLed()
+{
+    led = not led;
+}
+
+void button1_switch()
+{
+    button1_value = not button1_value;
+}
+
+void button2_switch()
+{
+    button2_value = not button2_value;
 }
 
-// functie om D1 en D2 te berekenen
-void AfstandBerekenen (){
-    float BV1 = sqrt(pow((a+x),2) + pow(y,2));  // diagonaal (afstand van armas tot locatie) berekenen
-    Dia1 = pow(BV1,2)/(2*BV1);     // berekenen van de afstand oorsprong tot diagonaal
-    
-    float BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm 
-    Dia2 = pow(BV2,2)/(2*BV2); 
-    
-    //pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2);
+void SendZeros()
+{
+    int zero=0;
+    scope.set(0,zero);
+    scope.set(1,zero);
+    scope.send();
 }
 
-// functie om de motorhoek te berekenen
-void MotorHoekBerekenen (){
-    // eerst if loop doorlopen voor motor 1
-    if (x > -a) {
-        MotorHoek1 = pi - atan(y/(x+a)) - acos(Dia1/Bar);
-    }
-    else if (x > -a) {
-        MotorHoek1 = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
-    }
-    else {  // als x=-a
-        MotorHoek1 = 0.5f*pi - acos(Dia1/Bar);
-    }
-    // twee if loop doorlopen voor motor 2
-    if (x < a) {
-        MotorHoek2 = pi + atan(y/(x-a)) - acos(Dia2/Bar);
-    }
-    else if (x > a) {
-        MotorHoek2 = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
-    }
-    else {  // als x=a
-        MotorHoek2 = 0.5f*pi - acos(Dia1/Bar);
-    }
-    
-    // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
-    if (MotorHoek1 >= MaxHoek) {    // is de maximale hoek in radialen
-        MotorHoek1 = VorigeHoek1;
-        x = vorigex;
-        y = vorigey;
-    }    
-    if (MotorHoek2 >= MaxHoek) {
-        MotorHoek2 = VorigeHoek2;
-        x = vorigex;
-        y = vorigey;
+void SetMotor(int motor_number, double MotorValue)
+{
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range
+    if (motor_number == 1)
+    {
+        if (MotorValue >=0)
+        {
+            motor1_dir=1;
+        }
+        else
+        {
+            motor1_dir=0;
+        }
+        if (fabs(MotorValue)>1){
+            motor1_pwm.write(1);
+        }
+        else
+        {
+            motor1_pwm.write(abs(MotorValue));
+        }
     }
-    //pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2);
-}        
-
-// het verschil uitrekenen tussen waar we zijn en waar we heen willen       OVERLAP MET SJOERD
-void BerekenenBewegingsHoek () {
-    // Hoek 1: grootte beweging en richting
-    BewegingsHoek1 = VorigeHoek1 - MotorHoek1;
-    VorigeHoek1 = MotorHoek1;
-    if (BewegingsHoek1 > 0) {
-        Richting1 = false;      // ccw
-    }
-    else {
-        Richting1 = true;       // cw
+    else
+    {  
+        if (MotorValue >=0)
+        {
+            motor1_dir=1;
+        }
+        else
+        {
+            motor1_dir=0;
+        }
+        if (fabs(MotorValue)>1){
+            motor1_pwm.write(1);
+        }
+        else
+        {
+            motor1_pwm.write(abs(MotorValue));
+        }
     }
-    
-    // Hoek 2: grootte beweging en richting
-    BewegingsHoek2 = VorigeHoek2 - MotorHoek2;
-    VorigeHoek2 = MotorHoek2;
-    if (BewegingsHoek2 > 0) {
-        Richting2 = true;       // cw
-    }
-    else {
-        Richting2 = false;      // ccw
+        
+}
+
+double GetPosition(int motor_number)
+{
+    const int   COUNTS_PER_REV = 8400;
+    const float DEGREES_PER_PULSE = 8400 / 360;
+    if (motor_number == 1)
+    {
+        int countsCW = Motor1_ENC_CW.getPulses();
+        int countsCCW= Motor1_ENC_CCW.getPulses();
+        int net_counts = countsCW-countsCCW;
+        double Position=(net_counts*360.0f)/COUNTS_PER_REV;
+        return Position;
     }
-    //pc.printf("Beweging 1 = %f, Beweging 2 = %f\r\n", BewegingsHoek1, BewegingsHoek2);
-}  
-
-// als BeginnenMetFlippen aan staat voeren we deze functie uit. Zolang deze bezig is is BezigMetFlippen true
-void FlipperBewegen () {
-    if (BeginnenMetFlippen == true) {
-        // SJOERDS DEEL 
-    }
-    else {      // als BezigMetFlippen == false dan doe niks
+    else
+    {
+        int countsCW = Motor2_ENC_CW.getPulses();
+        int countsCCW= Motor2_ENC_CCW.getPulses();
+        int net_counts = countsCW-countsCCW;
+        double Position=(net_counts*360.0f)/COUNTS_PER_REV;
+        return Position;
     }
 }
 
-// als de button ingedrukt wordt dan stoppen we met calibreren        
-void ButtonIndrukken () {
-        BezigMetCalibreren = false;
-}        
-        
+void MultiSin_motor1()
+{
+    double pwm = pwm_new;
+    double f1 = 0.2f;
+    double f2 = 1.0f;
+    double f3 = 5.0f;
+    double f4 = 20.0f;
+    double f5 = 100.0f;
+    const double pi = 3.141592653589793238462;
+    // read encoder
+    double motor1_position = GetPosition(1);
+    // set HIDSCOPE values, position, pwm*volt
+    scope.set(0, motor1_position);
+    scope.set(1, pwm);
+    // sent HIDScope values
+    scope.send();
+    // calculate the new multisin
+    double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
+    // devide by the amount of sin waves
+    pwm_new=multisin / 5;
+    // write the motors
+    SetMotor(1, multisin);
+    // Increase the timer
+    timer = timer + 0.001;
+    if (timer >= (20.0-0.001))
+    {
+        SinTicker.detach();
+        timer = 0;
+    }
+}
+
+void MultiSin_motor2()
+{      
+    double pwm = pwm_new;
+    double f1 = 0.2f;
+    double f2 = 1.0f;
+    double f3 = 5.0f;
+    double f4 = 20.0f;
+    double f5 = 100.0f;
+    const double pi = 3.141592653589793238462;
+    // read encoder
+    double motor2_position = GetPosition(2);
+    // set HIDSCOPE values, position, pwm*volt
+    scope.set(0, motor2_position);
+    scope.set(1, pwm);
+    // sent HIDScope values
+    scope.send();
+    // calculate the new multisin
+    double multisin = sin(2*pi*f1*timer)+sin(2*pi*f2*timer)+sin(2*pi*f3*timer)+sin(2*pi*f4*timer)+sin(2*pi*f5*timer);
+    // devide by the amount of sin waves
+    pwm_new=multisin / 5;
+    // write the motor
+    SetMotor(2, multisin);
+    // Increase the timer
+    timer = timer + 0.001;
+    if (timer >= (20.0-0.001))
+    {
+        SinTicker.detach();
+        timer = 0;
+    }
+}
+
+void PotControl()
+{
+    double Motor1_Value = (pot1.read() - 0.5)/2.0;
+    double Motor2_Value = (pot2.read() - 0.5)/2.0;
+    //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
+    //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
+    scope.set(0, Motor1_Value);
+    scope.set(1, Motor2_Value);
+    scope.send();
+    // Write the motors
+    SetMotor(1, Motor1_Value);
+    SetMotor(2, Motor2_Value);
+}
+
+void SinControl1()
+{
+    SinTicker.attach(&MultiSin_motor1, 0.001);
+}
+
+void SinControl2()
+{
+    SinTicker.attach(&MultiSin_motor2, 0.001);
+}
+
+void HIDScope_Send()
+{
+    scope.send();
+}
+
+
+
 int main() {
     pc.baud(SERIAL_BAUD);
-    pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
+    pc.printf("\r\n ***THERMONUCLEAR WARFARE HAS COMMENCED*** \r\n");
+    
+    // Set LED
+    led=0;
+    Ticker LedTicker;
+    LedTicker.attach(SwitchLed,0.5f);    
+    
+    // Interrupt for buttons
+    button1.fall(button1_switch);
+    button2.fall(button2_switch);
+    
+    pc.printf("\r\n ***************** \r\n");
+    pc.printf("\r\n Press button 1 to start positioning the arms \r\n");
+    pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r\n");
+    pc.printf("\r\n ***************** \r\n");
+    while (button1_value == false)
+    {
+        // just wait
+    }
+    pc.printf("\r\n ***************** \r\n");
+    pc.printf("\r\n Use potentiometers to control the motor arms \r\n");
+    pc.printf("\r\n Pot 1 for motor 1 \r\n");
+    pc.printf("\r\n Pot 2 for motor 2 \r\n");
+    pc.printf("\r\n ***************** \r\n");
+    pc.printf("\r\n Press button 2 to start the rest of the program \r\n");
+    pc.printf("\r\n ***************** \r\n");
+    // Start potmeter control
+    PotControlTicker.attach(&PotControl,0.01f);
     
-    // calibreren gebeurt hier, zodat de tickers nog niet lopen en dit dus geen rekenkracht kost en je niet gebonden bent aan tijden
-    // we gaan door met calibreren tot we de button indrukken
-    while (BezigMetCalibreren == true) {
-        // potmeter dingen aanpassen
-        
-        pc.printf("calibreren is bezig\r\n");
-        Button1.fall(&ButtonIndrukken);
+    while (button2_value == false)
+    {
+        // just wait   
+    }
+    PotControlTicker.detach();
+    SendZerosTicker.attach(&SendZeros,0.001);
+    
+    // Program startup   
+    pc.printf("\r\n Starting multisin on motor 1 in: \r\n");
+    pc.printf("\r\n 10 \r\n");
+    wait(5);
+    pc.printf("\r\n 5 \r\n");
+    wait(1);
+    pc.printf("\r\n 4 \r\n");
+    wait(1);
+    pc.printf("\r\n 3 \r\n");
+    wait(1);
+    pc.printf("\r\n 2 \r\n");
+    wait(1);
+    pc.printf("\r\n 1 \r\n");
+    wait(1);
+    pc.printf("\r\n 0 \r\n");
+    pc.printf("\r\n Wait for the signal to complete \r\n");
+    pc.printf("\r\n Press button 2 too continue afterwards \r\n");
+    SendZerosTicker.detach();
+    SinControl1();
+
+    
+    while (button2_value == true)
+    {
+        // just wait   
     }
     
-    TickerCaseBepalen.attach(&CaseBepalen, 1);                      // ticker om te bepalen welke case we gaan gebruiken
-    TickerVeranderenXY.attach(&VeranderenXY,1);                     // ticker voor het veranderen van de x en y waardes
-    TickerAfstandBerekenen.attach(&AfstandBerekenen,1);             // ticker om de waardes van dia1 en dia 2 te berekenen
-    TickerMotorHoekBerekenen.attach(&MotorHoekBerekenen,1);         // ticker om de motorhoek te berekenen
-    TickerBerekenenBewegingsHoek.attach(&BerekenenBewegingsHoek,1); // ticker om grote en richting vd beweging te berekenen
-    TickerFlipperBewegen.attach(&FlipperBewegen,1);                 // ticker om de flipper te laten bewegen
     
-    while (true) {
-        
-    }
+    SendZerosTicker.attach(&SendZeros,0.001);
+    pc.printf("\r\n Starting multisin on motor 2 in: \r\n");
+    pc.printf("\r\n 10 \r\n");
+    wait(5);
+    pc.printf("\r\n 5 \r\n");
+    wait(1);
+    pc.printf("\r\n 4 \r\n");
+    wait(1);
+    pc.printf("\r\n 3 \r\n");
+    wait(1);
+    pc.printf("\r\n 2 \r\n");
+    wait(1);
+    pc.printf("\r\n 1 \r\n");
+    wait(1);
+    pc.printf("\r\n 0 \r\n");
+    SendZerosTicker.detach();
+    SinControl2();
+    wait(23);
+    pc.printf("\r\n Program Finished, press reset to restart \r\n");
+    while (true) {}
 }
diff -r a9da7bc89b24 -r ee243782bf51 mbed.bld
--- a/mbed.bld	Mon Oct 17 14:25:12 2016 +0000
+++ b/mbed.bld	Thu Oct 20 12:07:30 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/aae6fcc7d9bb
\ No newline at end of file