Revision 6:ee243782bf51, committed 2016-10-20

Comitter:

sjoerdbarts

Date:

Thu Oct 20 12:07:30 2016 +0000

Parent:

5:a9da7bc89b24

Commit message:

Initial working code

Changed in this revision

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