Zoek mij ewout

Dependencies:   QEI mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #define SERIAL_BAUD 115200 
00004 
00005 DigitalOut gpo(D0);
00006 DigitalOut led(LED_RED);
00007 Serial pc(USBTX,USBRX);
00008 
00009 int counts;
00010 int counts2;
00011 float potr = 0;
00012 float potr2;
00013 float tspeed = 0.01;
00014 int desiredcount = 0;
00015 int angleerror = 500;
00016 int temp= 0;
00017 
00018 
00019 QEI Encoder(D12,D13,NC,32);
00020 DigitalOut motor2_direction(D7);
00021 PwmOut motor2_speed(D6);
00022 DigitalIn button1(PTA4);
00023 DigitalIn button2(PTC6);
00024 DigitalIn but1(D8);
00025 AnalogIn pot1(A1);
00026 AnalogIn pot2(A0);
00027 
00028 
00029 void move(int angle){
00030      desiredcount = int(angle*11.65);
00031      if(counts < desiredcount - angleerror){
00032         motor2_direction = false;   
00033         if(counts > desiredcount- (10*angleerror) && counts < desiredcount - angleerror){
00034             motor2_speed = 0.15;
00035             } else {
00036         motor2_speed = tspeed;
00037         }
00038         
00039         }
00040     else if (counts > desiredcount + angleerror){
00041         motor2_direction = true;  
00042         if(counts < desiredcount + (10*angleerror) && counts > desiredcount +angleerror){
00043             motor2_speed = 0.15;
00044             }else {
00045         motor2_speed = tspeed;
00046         }
00047          
00048         }            
00049     else {
00050         motor2_speed = 0;
00051         }
00052     }
00053     
00054 
00055 
00056 int main()
00057 {
00058             pc.baud(SERIAL_BAUD);
00059             motor2_speed.period(1/500);
00060 
00061 while (true){
00062     potr2 = pot1.read();
00063     counts = Encoder.getPulses();
00064     
00065     if(potr != potr2){
00066         potr = potr2;   
00067         tspeed = potr;    
00068         pc.printf("speed = %f \r\n", tspeed);
00069         } 
00070     
00071 
00072     if(button1==0 && button2 !=0){  //button 1
00073             move(0);
00074                   
00075     }       
00076     if(button2==0 && button1!=0){    //button2
00077         move(7200)  ;      
00078     }   
00079     if(button2==0 && button1==0){   //allebij
00080         move(2160);
00081         }
00082         
00083     if(button1!=0 && button2!=0) { // knopjes
00084         temp = int(360*pot2.read());
00085         move(temp);
00086         
00087     }
00088     if(but1==0){
00089         Encoder.reset();
00090         motor2_speed = 0;
00091         }
00092     if(counts!= counts2){
00093         counts2=counts;
00094         
00095     }
00096 }
00097 }