Diff: main.cpp

Revision:

13:c60942a1da8e

Parent:

12:dcf90cafb2a5

Child:

14:b9c925a8176a

--- a/main.cpp	Wed Sep 30 19:25:46 2015 +0000
+++ b/main.cpp	Wed Oct 07 13:30:46 2015 +0000
@@ -1,16 +1,16 @@
 #include "mbed.h"
-#include "QEI.h"
+#include "encoder.h"
 #include "math.h"
 #include "HIDScope.h"
 
 DigitalOut Direction(D4); //1 = CCW - 0 = CW
 PwmOut PowerMotor(D5); //van 0 tot 1
-DigitalIn Button(D1);
+DigitalIn Button(D3);
 AnalogIn PotMeter(A1);
-QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
+Encoder MotorEncoder(D12,D13); //Encoder
 Serial pc(USBTX, USBRX);
 Ticker Pot;
-HIDScope scope(2);
+HIDScope scope(3);
 
 double z=0; //initiele waarde potmeter
 const double twopi = 6.2831853071795;
@@ -20,14 +20,12 @@
 double Rotatieup; //aantal graden dat de motor is gedraaid in een bereik van n*pi
 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
 double Error = 0;
-double Errorv = 0;
-double Kp = 2; //Moet berekend worden aan de hand van Control concept slides
-double Kd = 0.1;
+double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
 double v = 0; //snelheid van de motor (0-0.1
 double upperlimit; //max aantal rotaties
 bool Excecute = false;
 
-double n = 2;
+double n = 8;
 
 void readpot()
 {
@@ -45,28 +43,28 @@
             Excecute =! Excecute;
         }
         while (Excecute ) {
-            Pulses = Encoder.getPulses();
+            Pulses = MotorEncoder.getPosition();
             Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
             Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
             pc.printf ("Potmeter = %f\n", z);
             pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
             Goal = z*upperlimit; // Het doel waar hij naar toe moet
             Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
-            Errorv = 0-v;
             if (Error >= 0) { //Bepalen van de rotatie richting
+                Direction =1;
+            } else {
                 Direction =0;
-            } else {
-                Direction =1;
             }
             pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
-            v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor
+            v = Kp*fabs(Error); // Snelheid van motor
             PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1).
-            if (Error<=0.001 && Error>=-0.001) {
-                PowerMotor.write(0);
-                Excecute=false;
-            }
+            //if (Error<=0.001 && Error>=-0.001) {
+//                PowerMotor.write(0);
+//                Excecute=false;
+            //}
             scope.set(0,Goal);
             scope.set(1,Rotatieup);
+            scope.set(2,Error);
             scope.send();
         }
     }