Revision 20:ad425c7f84b6, committed 2019-04-20

Comitter:

aschut

Date:

Sat Apr 20 19:55:10 2019 +0000

Parent:

19:24f5e36c6490

Commit message:

Met comments

Changed in this revision

--- a/main.cpp	Wed Apr 17 13:41:00 2019 +0000
+++ b/main.cpp	Sat Apr 20 19:55:10 2019 +0000
@@ -3,16 +3,21 @@
 #include "QEI.h"
 #include "BiQuad.h"
 #include "FastPWM.h"
+// Dit script is voor het testen van motor 2, volg instructies om het om te zetten naar motor 1
+
+
 // Algemeen
 Timer t;
 DigitalIn button2(SW2);
 DigitalIn button3(SW3);
-DigitalIn But2(D12);
+DigitalIn But2(D12);                //Buttons op motorschield
 DigitalIn But1(D13);
 DigitalOut led(LED_GREEN);
 DigitalOut led2(LED_RED);
 DigitalOut led3(LED_BLUE);
 MODSERIAL pc(USBTX, USBRX);
+
+//waardes voor sinus signaal
 float A;
 int i = 0;
 float pi = 3.14159265359;
@@ -107,7 +112,7 @@
 {
 
     if (KPot > 0.45f) {
-        stap2 = KPot*150*Ts;         // 144 graden van de arm in 5 seconden
+        stap2 = KPot*150*Ts;         
         Hoeknieuw2 = ElbowReference + stap2;
         return Hoeknieuw2;
     }
@@ -168,8 +173,8 @@
 {
     static double error1_integral = 0;
     static double error1_prev = error1; // initialization with this value only done once!
-    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
-     /*
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); 
+    /* Zet dit aan als je motor 1 wilt testen
     //PID testing
     Kp1 = 30 * Pot2;
     Ki1 = 10 * Pot1;
@@ -204,8 +209,8 @@
     static double error2_prev = error2; // initialization with this value only done once!
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
     
-    // PID testing
-    Kp2 = 10 * Pot2;
+    // /*PID testing 
+    Kp2 = 50 * Pot2;
     Ki2 = 10 * Pot1;
     
     if (!But2) {
@@ -214,7 +219,7 @@
     if (!But1){
         Kd2 = Kd2 - 0.01;
     }
-    
+    // */Zet dit stukje uit als je motor 1 wilt testen
     
     // Proportional part:
     double u_k2 = Kp2 * error2;
@@ -236,8 +241,8 @@
 void moter1_control(double u1)
 {
     direction1= u1 > 0.0f; //positief = CW
-    if (fabs(u1)> 0.5f) {
-        u1 = 0.5f;
+    if (fabs(u1)> 0.99f) {
+        u1 = 0.99f;
     } else {
         u1= u1;
     }
@@ -257,13 +262,13 @@
 
 void PwmMotor(void)
 {
-    //Reference hoek berekenen, in graden
-    //float Ellebooghoek1 = Kinematics2(pwm2);
-    //float Ellebooghoek4 = Limits2(Ellebooghoek1);
+    //Reference hoek berekenen, in graden 
+    //float Ellebooghoek1 = Kinematics2(pwm2);              //Zet dit aan als je motor 1 wilt testen
+    //float Ellebooghoek4 = Limits2(Ellebooghoek1);         //Zet dit aan als je motor 1 wilt testen
     //ElbowReference = Ellebooghoek4;
     
-    float Polshoek1 = Kinematics1(pwm2);
-    float Polshoek4 = Limits1(Polshoek1);
+    float Polshoek1 = Kinematics1(pwm2);                    //Zet dit uit als je motor 1 wilt testen
+    float Polshoek4 = Limits1(Polshoek1);                   //Zet dit aan als je motor 1 wilt testen
     //PolsReference = Polshoek4;
 
     // Positie motor berekenen, in graden
@@ -305,19 +310,13 @@
 }
 
 
-void Kdcount (void)             // Voor het testen van de PID waardes
+void Kdcount (void)             // Voor het testen van de PID waardes met een sinus signaal
 {
     float ts = t.read();
-    A = 350;
+    A = 350;                                                       //Zet uit voor motor 2
     ElbowReference = 350 + A*sin(2*pi*ts*0.1+(3/4)*pi);
-    //DA = sin(2/180*3.14)*0.5+0.5;
-    /*
-    if( i<360) {
-        DA = sin(i/180*3.14)*0.5+0.5;
-        i++;
-        }
-    */
-    
+    //A = 900;                                                     //Zet aan voor motor 1
+    //PolsReference = A*sin(2*pi*ts*0.1+(3/4)*pi);   
 }
 
 
@@ -351,7 +350,7 @@
         }
         led = 0;
         
-        if(counter==10) {
+        if(counter==10) {                               // Dit zend waardes naar matlab, open door het script 'serialread' te gebruiken
             float tmp = t.read();
             printf("%f,%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,error2,Kp2,Ki2,Kd2);
             counter = 0;