Diff: main.cpp

Revision:

2:f832050b1b4a

Parent:

1:363c5230fe25

Child:

3:ecd394ae8118

--- a/main.cpp	Fri Oct 04 09:26:44 2019 +0000
+++ b/main.cpp	Mon Oct 21 12:28:08 2019 +0000
@@ -5,66 +5,89 @@
 
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "QEI.h"
 
+QEI Encoder(D12,D13,NC,32);
+
+DigitalOut M1(D4);
+DigitalOut M2(D7);
 
-DigitalOut M1(D4);  //Direction control
-DigitalOut M2(D7);  //Direction control
-PwmOut E1(D5);      //Speed control
-PwmOut E2(D6);      //Speed control
-InterruptIn Btn1(A0);  //Potentionmeter
-InterruptIn Btn2(A1);  //Potentionmeter
-DigitalIn M1A(D2);  //Encoder
-DigitalIn M1B(D3);  //Encoder
+PwmOut E1(D5);
+PwmOut E2(D6);
+
+AnalogIn Pot1(A0);
+AnalogIn Pot2(A1);
 
-//float potVal1;
-//float potVal2;
-int EncA1;
-int EncB1;
-int counts = 0;
-int speedCount = 0;
+float potVal1;
+float potVal2;
 
-//Ticker readEnc
+float pi = 3.14159265359;
+
 
 MODSERIAL pc(USBTX, USBRX);
 
-void speedUp(){
-    speedCount++;
-}
-void speedDown(){
-    speedCount--;
-}
+
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
 
 
 // main() runs in its own thread in the OS
-int main(){
-    pc.baud(9600);
-    Btn1.rise(&speedUp);
-    Btn2.rise(&speedDown);
-    while(true){
-        if(speedCount > 0){
-            M1 = 1;
-            E1 = speedCount/10;
-            }
-        else{
-            M1 = 0;
-            E1 = -speedCount/10;
-            }                
-        /*potVal1 = Pot1.read();
-        potVal2 = Pot2.read();
-        if (potVal1 > 0.5){
-            M1 = 1;
-            E1 = potVal1-0.5;
-        }else{
-            M1 = 0;
-            E1 = -(potVal1-0.5);
+int main()
+{
+    float steps = 100;
+    pc.baud(115200);
+    while (true){
+        for(int i = 0; i < steps; i++){
+            moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
+            //pc.printf("step: %f\n\r", float(i)/steps);
         }
-        if (potVal2 > 0.5){
-            M2 = 1;
-            E2 = potVal2-0.5;
-        }else{
-            M2 = 0;
-            E2 = -(potVal2-0.5);
-        }
-        pc.printf("Pot1: %f \t Pot2: %f \n\r", potVal1, potVal2);*/
+        for(int i = steps; i > 0; i--){
+            moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
+        }            
     }
 }
+
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
+{
+    float pErrorC;
+    float pErrorP = 0;
+    float iError = 0;
+    float dError;
+
+    float Kp = 5;
+    float Ki = 0.01;
+    float Kd = 0.7;
+
+    float rotC = Enc->getPulses()/(32*131.25);
+    float rotP = 0;
+    float MotorPWM;
+
+    Timer t;
+    float tieme = 0;
+
+    t.start();
+    do {
+        pErrorP = pErrorC;
+        pErrorC = rotDes - rotC;
+        iError = iError + pErrorC*tieme;
+        dError = (pErrorC - pErrorP)/tieme;
+
+        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
+
+        if(MotorPWM > 0) {
+            *M = 0;
+            *E = MotorPWM;
+        } else {
+            *M = 1;
+            *E = -MotorPWM;
+        }
+
+        rotP = rotC;
+        rotC = Enc->getPulses()/(32*131.25);
+        tieme = t.read();
+        t.reset();
+        //pc.printf("pError: %f dError: %f iError: %f PWM: %f\n\r", pErrorC, dError, iError, MotorPWM);
+    } while (pErrorC > 0.01 || pErrorC < -0.01 ||dError > 0.01 || dError < -0.01);
+    //*E = 0;
+    t.stop();
+}
+