main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "HIDScope.h"
00004 #include "QEI.h"
00005  
00006 Serial pc(USBTX, USBRX); // tx, rx
00007 DigitalOut led(LED_RED);
00008 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
00009 PwmOut motor2speed(D5);
00010 DigitalIn button1(SW3);
00011 DigitalIn EncoderA(D3);
00012 DigitalIn EncoderB(D2);
00013 QEI Encoder(D3, D2, NC, 32);
00014 HIDScope scope(3);
00015 Ticker ScopeTime;
00016 float Aantal_Degs;
00017 float Aantal_pulses;
00018 float Error;
00019 float refference;
00020 const float Kp = 0.005;
00021 
00022 
00023 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
00024 {
00025     scope.set(0, motor2direction.read());
00026     scope.set(1, motor2speed.read());
00027     scope.set(2, Aantal_Degs);
00028     Aantal_Degs = Encoder.getPulses()*360/31/131;
00029 
00030     scope.send();
00031     
00032 }
00033 
00034 
00035 int main()
00036 {
00037     motor2direction = 0;
00038     motor2speed = 0;
00039     led = 1;
00040     pc.baud(115200);
00041     float refference = 0;
00042     pc.printf("Tot aan loop werkt\n");
00043     ScopeTime.attach_us(&ScopeSend, 10e4);
00044     
00045     
00046     while (true) 
00047     {
00048         
00049         char c = pc.getc();   
00050         if(c == 'r')
00051         {
00052             refference = refference + 10;
00053             pc.printf("rx \n");
00054             Error = refference - Aantal_Degs;
00055             while(abs(Error) > 2)
00056                 {
00057                     Error = refference - Aantal_Degs;
00058                     motor2speed = Kp*abs(Error);
00059                     pc.printf("reffence = %f,error = %f \n",refference,Error);
00060                     if(Error > 0)
00061                     {
00062                         motor2direction = 0;
00063                     }
00064                     else
00065                     {
00066                     motor2direction = 1;
00067                     }
00068 
00069                 }
00070         }
00071 
00072         
00073 
00074 
00075     }
00076 }