Diff: main.cpp

Revision:

3:e533800b2ef8

Parent:

2:cb7d7e31e30e

--- a/main.cpp	Sun Oct 14 10:59:42 2018 +0000
+++ b/main.cpp	Mon Oct 15 14:48:24 2018 +0000
@@ -1,34 +1,103 @@
 #include "mbed.h"
 #include "QEI.h"
+#include "HIDScope.h"
 
 #define SERIAL_BAUD 115200
 
 Serial pc(USBTX,USBRX);
 
 DigitalOut dirpin(D4);
-DigitalOut dirpin_2(D6);
-
+DigitalOut dirpin_2 (D7);
 PwmOut pwmpin(D5);
-PwmOut pwmpin_2(D7);
-
+PwmOut pwmpin_2 (D6);
 AnalogIn pot_1(A1); //only using one potmeter for both motors, eventually just use a signal created by program or EMG-signals
+AnalogIn pot_2(A2);
+DigitalIn button(D7);
 
 
 QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING);    //channel A=D12, channel B=D13
 
+
+float pos_enc()
+{
+    float radpercount = 6.2830f/8400; //aantal rad per counts
+    float counts = Encoder.getPulses(); //tel aantal counts
+    int revs = counts/8400;
+    float pos_enc = (radpercount * counts)-(revs*6.2830f);
+    return pos_enc;
+    }
+    
+
+float pos_ref(float a)
+{
+    if(!button){
+        float pos_ref = pot_1*6.2830f; //referentie positie vanuit potmeter 1
+        return pos_ref;
+    }
+    else{
+        return a;
+        }
+        }
+float error(float a)
+{
+    float error=pos_ref(a) - pos_enc(); //error bepaling
+    return error;
+    }
+
+float Kp()
+{
+       float Kp=pot_2*10; //Proportionele factor uit potmeter 2
+       return Kp;
+       }
+
+float Ki(float a)
+{
+    if(button){
+        float Ki=pot_1*10; // waar halen we Ki vandaan?
+        return Ki;
+    }
+    else{
+    return a;
+    }
+    }
+float error_int(float a)
+{
+    float error_int = error(a);
+    //float error_int = error_int(a) - error(a)*Ts;
+    return error_int;
+    }   
+    
+float u_k(float a,float b)
+{
+    float u_k=Kp()*error(a)+Ki(b)*error_int(a); //eind waarde
+    return u_k;
+    }
+
+
+    
+    
+
+
 int main()
 {
     //float out_1=1.0f;                         //set potmeter signal as a predetermined digital signal
     pc.baud(115200);                            //also set baudrate to 115200 in teraterm!
     pc.printf("start\r\n");
       
-    pwmpin.period_us(60);                       //???
+    pwmpin.period_us(60);                       //in microseconds for pwmOut 
+    float ki;
+    float Pos_ref;
+    
     
     while(1){
-    float out_1 = (pot_1 * 2.0f) - 1.0f;        //scales potmeter signal from 0 to 1 into -1 to 1
-    dirpin.write(out_1 < 0);                    //sets direction of motor? if negative =true (1), if positive =false (0)
-    pwmpin = fabs (out_1);                      //sets speed of motor?
-    pc.printf("%i\r\n", Encoder.getPulses());   //prints the amount of counts
-    wait(0.1);                                  //repeat loop every 0.01 sec
+        float out_1 = (pot_1 * 2.0f) - 1.0f;        //scales potmeter signal from 0 to 1 into -1 to 1
+        dirpin.write(out_1 < 0);                    //sets direction of motor? if negative =true (1), if positive =false (0)
+        pwmpin = fabs (out_1);                      //sets speed of motor?
+        dirpin_2.write(out_1 < 0);
+        pwmpin_2 = fabs (out_1);
+        ki = Ki(ki);
+        Pos_ref= pos_ref(Pos_ref);
+        pc.printf("Kp=%f Ki=%f error=%f  u_k=%f\r\n", Kp(),ki, error(Pos_ref), u_k(Pos_ref,ki));   //prints the amount of counts
+        wait(0.01);                                  //repeat loop every 0.01 sec
     } 
 }
\ No newline at end of file