main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "HIDScope.h"
00004 
00005 // Definiëren van alle poorten
00006 
00007 DigitalIn encoder1A (D13); //Channel A van Encoder 1
00008 DigitalIn encoder1B (D12); //Channel B van Encoder 1
00009 DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15
00010 DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
00011 DigitalOut motor1 (D7);
00012 PwmOut pwm_motor1 (D6);
00013 DigitalOut motor2 (D4);
00014 PwmOut pwm_motor2 (D5);
00015 HIDScope    scope( 4 );
00016 Ticker      sample_timer;
00017 
00018 double t;
00019 float pi=3.14159;
00020 float Aout;
00021 float PWMAout;
00022 int counts1;
00023 int counts2;
00024 const int cw=0;
00025 const int ccw=1;
00026 
00027 void sample()
00028 {
00029     scope.set(0, counts1);
00030     scope.set(1, counts2);
00031     scope.set(2, Aout);
00032     scope.set(3, PWMAout);
00033     scope.send();
00034 }
00035 
00036 
00037 int main() {
00038     const double frequency_pwm = 100.0;
00039     pwm_motor1.period(1.0/frequency_pwm);
00040     pwm_motor2.period(1.0/frequency_pwm);
00041     QEI Encoder1(D12, D13, NC, 32); //maakt de encoder aan, NC niet aanwezig, 32 bits
00042     QEI Encoder2(D10, D11, NC, 32); //maakt de encoder aan, NC niet aanwezig, 32 bits
00043     sample_timer.attach(&sample, 0.001);
00044  
00045     while(true){
00046       for (t=0; t<10; t=t+0.05) {
00047                 Aout=1*sin(2*pi*t);
00048                 PWMAout=(Aout+1)/2;
00049             if (Aout >=0) //clockwise rotation
00050                 {motor1=cw;        
00051             }
00052         else    //counterclockwise rotation 
00053             {motor1=ccw;         
00054             }
00055         pwm_motor1.write(PWMAout);
00056     
00057         if (Aout >=0) //clockwise rotation
00058             {motor2=cw;        
00059             }
00060         else    //counterclockwise rotation 
00061             {motor2=ccw;         
00062             }
00063         pwm_motor2.write(PWMAout);
00064     
00065         counts1 = Encoder1.getPulses();
00066         counts2 = Encoder2.getPulses(); 
00067      }
00068             
00069     
00070     }
00071  }
00072 
00073