File content as of revision 30:356902e752e4:

#include "mbed.h" //standaard bieb mbed
#include "QEI.h"  //bieb voor encoderfuncties in c++
#include "MODSERIAL.h" //bieb voor modserial
InterruptIn sw3(SW3);
DigitalIn sw2(SW2);
DigitalIn encoder1A(D13);
DigitalIn encoder1B(D12);
DigitalIn encoder2A(D11); 
DigitalIn encoder2B(D10);
DigitalIn button_cw(D9);
DigitalIn button_ccw(PTC12);
MODSERIAL pc(USBTX, USBRX);
DigitalOut richting_motor1(D4);
PwmOut pwm_motor1(D5);
DigitalOut richting_motor2(D7);
PwmOut pwm_motor2(D6);



int n = 0; //start van de teller wordt op nul gesteld
 
void SwitchN() {                        // maakt simpele functie die 1 bij n optelt
    n++;
    }
     
//constanten voor de encoder
const int CW = 2.5; //definitie rechtsom 'lage waarde'
const int CCW =-1; //definitie linksom 'hoge waarde'
const float gearboxratio=131.25; // gearboxratio van encoder naar motor
const float rev_rond=64.0;        // aantal revoluties per omgang van de encoder

volatile double current_position_motor1 = 0;                // current position is 0 op het begin
volatile double previous_position_motor1 = 0;               // previous position is 0 op het begin                    
volatile double current_position_motor2 = 0;                
volatile double previous_position_motor2 = 0;                                   
volatile bool tickerflag;                           //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false)
volatile double snelheid_motor1;                    // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s
volatile double snelheid_motor2;                    // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s
double ticker_TS=0.025;                              // zorgt voor een tijdstap van de ticker van 0.1 seconde
volatile double timepassed=0;                       //de waarde van hoeveel tijd er verstreken is
Ticker t;                                           // maakt ticker t aan
volatile double value1_resetbutton = 0;             // deze value wordt gebruikt zodat als er bij de reset button na het bereiken van de waarde nul. De motor stopt met draaien.
volatile double value2_resetbutton = 0;

void tickerfunctie()                // maakt een ticker functie aan
{
tickerflag = 1;                     // het enige wat deze functie doet is zorgen dat tickerflag 1 is
}
 
int main()
{
  pc.baud(115200);                      // zorgt voor de link voor putty, 115200 is snelheid
  QEI Encoder2(D12,D13, NC, rev_rond,QEI::X4_ENCODING);  // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen. 
  QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
    float counts_encoder1;                  //variabele counts aanmaken
    float rev_counts_motor1;                //variabele motor rondjes aanmaken in radialen!!
    float counts_encoder2;
    float rev_counts_motor2;
t.attach(&tickerfunctie,ticker_TS);             // attacht de ticker met een tick van 0.1 seconde (gelijk aan de tijdstap)
volatile float voltage_motor1=0.18; //pwm is de pulse with tussen geen ampere en wel ampere motor 1
volatile float voltage_motor2=1;//pwm is de pulse with tussen geen ampere en wel ampere motor 1



        
        
 
  while (true) {                        // zorgt er voor dat de code oneindig doorgelopen wordt
    
    sw3.fall(&SwitchN);                  // zorgt er voor dat void switch wordt gedaan als switch 3 wordt ingedrukt
    
    while(sw2==0 && rev_counts_motor1<-0.1 && value1_resetbutton >= 0){ // zorgt er voor dat de loop wordt doorlopen als switch 2 wordt ingedrukt, aantal rondejs van de motor kleiner is dan 0 en de value resetbutton groter of gelijk is aan 0
        pc.printf("switch 2 is ingedrukt \r\n");
            pc.printf("aantal rondjes motor 1 is kleiner dan 0\r\n");
            richting_motor1 = 1;                                        // zorgt er voor dat de motor tegenovergestelde richting gaat draaien zodat het totaal aantal rondjes gedraaid op 0 komt te staan
             pwm_motor1 = 0.1;                               // zorgt er voor dat er een ampere is voor de motor om te draaien
             pc.printf("richting motor is %f \r\n", richting_motor1);
             pc.printf("voltage motor 1 is %f \r\n", voltage_motor1);
             counts_encoder1 = Encoder1.getPulses();                   // zorgt er voor dat het aantal rondjes geteld worden
             rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); // zorgt er voor dat rev counts niet het aantal pulsen is maar het aantal rondjes
             pc.printf("rondjes motor is %f \r\n", rev_counts_motor1);
             value1_resetbutton = 1;                                    // veranderd de value1_resetbutton in 1 hierdoor kan er niet meer in de andere loop gekomen worden maar nog wel in dezelfde. dit voorkomt dat als de loop 0 heeft bereikt die daar blijft staan en niet oneindig om nul blijft draaien.
            }
       while (sw2==0 && rev_counts_motor1>0.1 && value1_resetbutton <=0){ // werkt het zelfde als de vorige loop maar dan met tegengestelde richting.
            pc.printf("aantal rondjes motor 1 is groter dan 0 \r\n");
            richting_motor1 = 0;
             pwm_motor1 = 0.1;
             pc.printf("richting motor is %f \r\n", richting_motor1);
             pc.printf("voltage motor 1 is %f \r\n", voltage_motor1);
             counts_encoder1 = Encoder1.getPulses();
             rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
              pc.printf("rondjes motor is %f \r\n", rev_counts_motor1);  
              value1_resetbutton = -1;
            }
    while(sw2==0 && rev_counts_motor2<-0.1 && value2_resetbutton >= 0){ // werkt het zelfde maar dan voor motor2
            pc.printf("aantal rondjes motor 2 is kleiner dan 0\r\n");
            richting_motor2 = 0;
             pwm_motor2 = 0.1;
             pwm_motor1 = 0;
             pc.printf("richting motor 2 is %f \r\n", richting_motor2);
             pc.printf("voltage motor 2 is %f \r\n", voltage_motor2);
             counts_encoder2 = Encoder2.getPulses();  
             rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
             pc.printf("rondjes motor 2is %f \r\n", rev_counts_motor2);
             value2_resetbutton = 1;    
            }
       while (sw2==0 && rev_counts_motor2>0.1 && value2_resetbutton <=0){
            pc.printf("aantal rondjes motor 2 is groter dan 0\r\n");
            richting_motor2 = 1;
             pwm_motor2 = 0.1;
             pwm_motor1=0;
             pc.printf("richting motor 2is %f \r\n", richting_motor2);
             pc.printf("voltage motor 2 is %f \r\n", voltage_motor2);
             counts_encoder2 = Encoder2.getPulses();
             rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
              pc.printf("rondjes motor2 is %f \r\n", rev_counts_motor2);  
              value2_resetbutton = -1;
            }
    
    if (button_cw==0)                           // als s ingedrukt wordt gebeurd het volgende
    {
    if ( tickerflag == 1)                        // als de ticker flag 1 is gaat dit loopje lopen. 
       {
         if (n%2==0)                     // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
         {
           pc.printf("n is even \n\r"); // print lijn "n is even"
           pc.printf("up \n\r");        // print lijn "up"   
           richting_motor1 = 1;         // geeft richting van motor 1 aan
           pwm_motor1 = voltage_motor1; // geeft pulse lengte tussen ampere uit en aan dit bepaald de snelheid
           counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in  
        rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);              //berekenen van het aantal rondjes van motor. Gedeeld door gearboxratio en rev rond, om naar motorrondjes te gaan in plaats van pulsen van encoder!
        pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1);                  //weergeven
          value1_resetbutton = 0;       // reset de value van de resetbutton zodat als je hebt gereset en dan weer gaat draaien dat je gewoon normaal kan resetten.
         } 
         
         else                           // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
         {
           pc.printf("n is odd \n\r");  // print lijn "n is odd"
           pc.printf("left \n\r");      // print lijn "left"
           richting_motor2 = 1;         // motor 2 richting
           pwm_motor2 = voltage_motor2; // geeft pulse lengte tussen ampere uit en aan dit bepaald de snelheid
           counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in  
        rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);            //weergeven van het aantal rondjes
        pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor2);   
        value2_resetbutton = 0;
         }      
         
        previous_position_motor1 = current_position_motor1; // zorgt er voor dat de huidige positie wordt gedefineerd als de vorige positie is
        current_position_motor1 = rev_counts_motor1;        // zorgt dat de huidige positie wordt gedefineerd als het huidige aantal rondejs dat gedraaid is
        previous_position_motor2 = current_position_motor2; // zelfde maar dan voor motor2
        current_position_motor2 = rev_counts_motor2;
        
        snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS; // berekend de snelheid van de motor door te kijken naar het aantal rondjes dat gedraaid is in radialen (dus *2pi) delen door de tijd waarin dat gedraaid is (die is altijd 0.1 want de ticker laat het script iedere 0.1 seconde lopen).
        pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1);
        
        snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS; // doet het zelfde voor motor 2
        pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2);
        
        if (abs(snelheid_motor1) > 3.0){ // zorgt dat als de absolute van de snelheid van motor 1 boven de target snelheid zit ( in dit geval 3.0 rad/s) dat er in dit loopje gelopen wordt
        voltage_motor1 = voltage_motor1-0.005; // zorgt er voor dat de pwm verlaagd word hierdoor word de puls lengte kleiner en zal de motor langzamer gaan draaien.
        pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor1);
        }
        else if (abs(snelheid_motor1) < 3.0 && snelheid_motor1 != 0){ // doet het zelfde maar dan als de snelheid lager is dan de target speed
        voltage_motor1 = voltage_motor1+0.005;                        // verhoogd dus ook de pwm
        pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor1);
        }
         if (abs(snelheid_motor2) > 5.0){// zelfde maar dan voor motor 2
        voltage_motor2 = voltage_motor2-0.005;
        pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor2);
        }
        else if (abs(snelheid_motor2) < 5.0 && snelheid_motor2 != 0){
        voltage_motor2 = voltage_motor2+0.005;
        pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor2);
        }
        tickerflag = 0; // reset de tickerflag weer op 0 zodat het loopje niet wordt doorlopen tot de volgende tick zo kan de tijd tussen het lopen van ieder loopje gecontroleerd worden
        }
    }
    else if (button_ccw==0)                     // als d ingedrukt wordt gebeurd het volgende        // dit doet het zelfde als bovenstaande alleen dan voor het andere knopje.
    {
   if ( tickerflag == 1)                        // als de ticker flag 1 is gaat dit loopje lopen. 
       {
         if  (n%2==0)                     // als d is ingedrukt en n is even dan gebeurd het volgende
         {
           pc.printf("n is even \n\r"); // print lijn "n is even"
           pc.printf("down \n\r");      // print lijn "down"   
           richting_motor1 = 0;
           pwm_motor1 = voltage_motor1;
           counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in  
        rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);            //weergeven van het aantal rondjes
        pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1);   
        value1_resetbutton = 0;
       
         
           
         } 
         else                           // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
         {
           pc.printf("n is odd \n\r"); // print lijn "n is odd"
           pc.printf("right \n\r");    // print lijn "right"
           richting_motor2 = 0;
           pwm_motor2 = voltage_motor2;
           counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in  
        rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);            //weergeven van het aantal rondjes
        pc.printf("motor rondjes naar rechts: %f \r\n", rev_counts_motor2);  
        value2_resetbutton = 0; 
        
         }  
        previous_position_motor1 = current_position_motor1;
        current_position_motor1 = rev_counts_motor1;
        previous_position_motor2 = current_position_motor2;
        current_position_motor2 = rev_counts_motor2;
        
        snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
        pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1);
        
        snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS;
        pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2);
        
        if (abs(snelheid_motor1) > 3.0){
        voltage_motor1 = voltage_motor1-0.005;
        pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor1);
        }
        else if (abs(snelheid_motor1) < 3.0 && snelheid_motor1 != 0){
        voltage_motor1 = voltage_motor1+0.005;
        pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor1);
        }
         if (abs(snelheid_motor2) > 5.0){
        voltage_motor2 = voltage_motor2-0.005;
        pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor2);
        }
        else if (abs(snelheid_motor2) < 5.0 && snelheid_motor2 != 0){
        voltage_motor2 = voltage_motor2+0.005;
        pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor2);
        }
        
        tickerflag = 0;
        }
    }   
    else{
     pc.printf("rondjes motor1 is %f:",rev_counts_motor1);
     pc.printf("rondjes motor2 is %f: \n\r",rev_counts_motor2);
     counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in  
    rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);  
     counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in  
    rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);  
    pwm_motor2=0;
    pwm_motor1=0;
    }              
   if (rev_counts_motor1 <-3.4 && richting_motor1 == 0){
      pwm_motor1=0;
      }
    else if(rev_counts_motor1 >3.4 && richting_motor1 == 1){
    pwm_motor1 =0;
    }
    
  if (rev_counts_motor2 < -2.0 && richting_motor2 == 1){
        pc.printf("rondjes motor kleinder dan -2.0 dus ik stop \r\n");
        pwm_motor2=0;
        richting_motor2=0;
           }
    else if (rev_counts_motor2 > 2.0 &&  richting_motor2 == 0){
           pc.printf("rondjes motor groter dan 2.0 dus ik stop \r\n");
           pwm_motor2=0;
           richting_motor2=1;
           }
           
}
}