Revision 17:f44f41cab151, committed 2016-10-19

Comitter:

sivuu

Date:

Wed Oct 19 15:31:31 2016 +0000

Parent:

16:135908f85971

Child:

18:cd292bac4524

Child:

19:58c24260e08d

Commit message:

werkende motoren en alle shit

Changed in this revision

--- a/main.cpp	Tue Oct 11 14:09:58 2016 +0000
+++ b/main.cpp	Wed Oct 19 15:31:31 2016 +0000
@@ -2,14 +2,12 @@
 #include "QEI.h"  //bieb voor encoderfuncties in c++
 #include "MODSERIAL.h" //bieb voor modserial
 InterruptIn sw3(SW3);
-DigitalIn encoder1A(D13); 
+DigitalIn encoder1A(D13);
 DigitalIn encoder1B(D12);
-DigitalIn button_cw(D11);
-DigitalIn encoder2A(D15);
-DigitalIn encoder2B(D14);
-DigitalIn button_ccw(D9);
-DigitalOut ledcw(D10);
-DigitalOut ledccw(D2);
+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);
@@ -24,25 +22,42 @@
     n++;
     }
     
+
+    
 //constanten voor de encoder
 const int CW = 2.5; //definitie rechtsom 'lage waarde'
-const int CCW =0; //definitie linksom 'hoge waarde'
-const float gearboxratio=131; // gearboxratio van encoder naar motor
+const int CCW =-1; //definitie linksom 'hoge waarde'
+const float gearboxratio=131.25; // gearboxratio van encoder naar motor
 const int rev_rond=32;        // 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;
+volatile double snelheid_motor2;
+double ticker_TS=0.01;                              // zorgt voor een tijdstap van de ticker van 0.01 seconde
+volatile double timepassed=0;                       //de waarde van hoeveel tijd er verstreken is
+Ticker t;                                           // maakt ticker t aan
+
+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 Encoder1(D12,D13, NC, rev_rond);  // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen. 
-  QEI Encoder2(D15,D14, NC, rev_rond);
+  QEI Encoder2(D12,D13, NC, rev_rond);  // 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);
     float counts_encoder1;                  //variabele counts aanmaken
     float rev_counts_motor1;                   //variabele motor rondjes aanmaken in radialen!!
     float counts_encoder2;
     float rev_counts_motor2;
-             
- const float a=1.0; //voltingang motor 1
- const float b=10.0;//voltingang motor 2
+t.attach(&tickerfunctie,ticker_TS);             // attacht de ticker met een tick van 0.01 seconde (gelijk aan de tijdstap)
+ const float a=12.0; //voltingang motor 1
+ const float b=12.0;//voltingang motor 2
  
   while (true) {                        // zorgt er voor dat de code oneindig doorgelopen wordt
     
@@ -52,13 +67,14 @@
    
     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;
            pwm_motor1 = a; 
-           ledcw=1; ledccw=0;
            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
@@ -71,23 +87,35 @@
            pc.printf("left \n\r");      // print lijn "left"
            richting_motor2 = 1;
            pwm_motor2 = b;
-           ledcw=1;
-           ledccw=1;
            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);   
          }      
-              
+        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);
+        
+        
+        tickerflag = 0;
+        }
     }
     else if (button_ccw==0)                     // als d ingedrukt wordt gebeurd het volgende
     {
+   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 = a;
-           ledccw=1; ledcw=0;
            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);   
@@ -100,18 +128,29 @@
            pc.printf("right \n\r");    // print lijn "right"
            richting_motor2 = 0;
            pwm_motor2 = b;
-           ledccw=1; ledcw=0;
            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);   
         
          }  
+        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);
+        
+        tickerflag = 0;
+        }
     }   
     else{
       // pc.printf("motor staat stil \n\r");
     pwm_motor2=0;
     pwm_motor1=0;
-    ledccw=0; ;ledcw=0;
     }