Diff: main.cpp

Revision:

12:505e5ea9f639

Parent:

11:4b0dbbdc56fb

--- a/main.cpp	Mon Oct 22 10:01:47 2018 +0000
+++ b/main.cpp	Thu Oct 25 13:01:31 2018 +0000
@@ -5,7 +5,7 @@
 #define _USE_MATH_DEFINES
 # define M_PI           3.14159265358979323846  /* pi */
 //motor 1
-double Kp = 5;
+double Kp = 10;
 double Ki = 1;
 double u_k = 1;
 double u_i = 1;
@@ -17,7 +17,7 @@
 double m1=0;
 double input_motor;
 //motor 2
-double Kp2 = 10;
+double Kp2 = 12;
 double Ki2 = 1;
 double u_k2 = 1;
 double u_i2 = 1;
@@ -35,6 +35,8 @@
 volatile char input;
 
 Ticker Control;
+Ticker serialEvent;
+
 
 PwmOut  motor1_pwm(D5);
 PwmOut  motor2_pwm(D6);
@@ -90,43 +92,46 @@
             
 void aansturing()
     {       //posities voor de motoren bepalen   
-//            if(input=='a')
-//               {if(m1>0.1f)
-//                {m1 = m1-0.01f;}}
-//            else if(input=='s')
-//                {if(m1<2.1f)
-//                {m1 = m1+0.01f;}}
-//            else if(input=='q')
-//                {if(m2>0.01f)
-//                {m2 = m2-0.01f;}}
-//            else if(input=='w')
-//                {if(m2<2.1f)
-//                {m2 = m2+0.01f;}}
-            m1_scalar=pot1.read();
-            m2_scalar=pot2.read();
-            m1=m1_scalar*1.5f;
-            m2=m2_scalar*1.5f;
-            //Kswitch = pot1.read();      //kp en ki bepalen voor motor 1
-            //Kswitch2 = pot2.read();     //kp en ki bepalen voor motor 2
+        
+            if(input=='a')
+               {if(m1>0)
+                {m1 = m1-0.01f;}}
+            else if(input=='s')
+                {if(m1<1.5f)
+                {m1 = m1+0.01f;}}
+            else if(input=='q')
+                {if(m2>0)
+                {m2 = m2-0.01f;}}
+            else if(input=='w')
+                {if(m2<4.0f)
+                {m2 = m2+0.01f;}}
+                
+            input= NULL;
+            //m1_scalar=pot1.read();
+            //m2_scalar=pot2.read();
+            //m1=m1_scalar*1.5f;
+            //m2=m2_scalar*1.5f;
+            Kswitch = pot1.read();      //kp en ki bepalen voor motor 1
+            Kswitch2 = pot2.read();     //kp en ki bepalen voor motor 2
             
-            //kp_scalar=Kswitch;
-            //kp_scalar2=Kswitch2;
+            kp_scalar=Kswitch;
+            kp_scalar2=Kswitch2;
 //            //kijken of er gekozen is voor het uitlezen van de kp of ki van de potmeter
-//            if(input_switch==true)
-//            {kp_scalar = Kswitch;}
-//            else
-//            {ki_scalar = Kswitch;}
-//            
-//            if(input_switch==true)
-//            {kp_scalar2 = Kswitch2;}
-//            else
-//            {ki_scalar2 = Kswitch2;}
+            if(input_switch==true)
+            {kp_scalar = Kswitch;}
+            else
+            {ki_scalar = Kswitch;}
+           
+           if(input_switch==true)
+            {kp_scalar2 = Kswitch2;}
+            else
+            {ki_scalar2 = Kswitch2;}
             
             //waarde voor de inputcontrol value instellen aan de hand van potmeter
-           // Kp = kp_scalar*20.0f;
-           // Ki = ki_scalar*2.0f;
-           // Kp2 = kp_scalar2*20.0f;
-           // Ki2 = ki_scalar2*2.0f;
+            Kp = kp_scalar*5.0f;
+            Ki = ki_scalar*5.0f;
+            Kp2 = kp_scalar2*5.0f;
+            Ki2 = ki_scalar2*5.0f;
             
             
            
@@ -168,12 +173,22 @@
  //   pc.printf("ki: %f  kp: %f  yref: %f     error: %f   u_k: %f  angle: %f\r\n",Ki, Kp, y_ref,error,u_k,motor1_ang);
     }
 
+void serialSampler(){
+    if(pc.readable()){
+        input = pc.getc();
+        pc.printf("Input = %c\r\n",input);
+    }else if(input_switch==true){
+        pc.printf("Kp ingeschakeld\r\n");
+    }else{
+        pc.printf("Ki ingeschakeld\r\n");
+    }
+}
+
 int main()
 {
     
     sw2.fall(buttonpress);
     pc.baud(115200);
-    pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n");
     
     motor1_pwm.period_us(60);
     motor1_pwm = 0;  
@@ -181,19 +196,13 @@
     motor2_pwm = 0;  
     
     Control.attach(aansturing, Ts);         //de aansturing regelen met een sampletime
-    
+    serialEvent.attach(&serialSampler, 0.05);
     while (true)
-    {       
-            //input = pc.getc();
-            if(input_switch==true)
-            {pc.printf("Kp ingeschakeld\r\n");}
-            else
-            {pc.printf("Ki ingeschakeld\r\n");}
-            
+    {                  
             double errorprint = motor1_ang-m1;
             double errorprint2 = motor2_ang-m2;
-            pc.printf("ki1: %f   kp1: %f  m1: %f   u_k1: %f  angle1: %f   error1: %f\r\n",Ki, Kp, m1,u_k,motor1_ang,errorprint);
-            pc.printf("ki2: %f   kp2: %f  m2: %f   u_k2: %f  angle2: %f   error2: %f\r\n",Ki2, Kp2, m2,u_k2,motor2_ang,errorprint2);
+            pc.printf("ki1: %.2f   kp1: %.2f  m1: %.2f   u_k1: %.2f  angle1: %.2f   error1: %.2f\r\n",Ki, Kp, m1,u_k,motor1_ang,errorprint);
+            pc.printf("ki2: %.2f   kp2: %.2f  m2: %.2f   u_k2: %.2f  angle2: %.2f   error2: %.2f\r\n\n",Ki2, Kp2, m2,u_k2,motor2_ang,errorprint2);
             pc.printf("toets=%c\r\n",input);
             wait(0.5);  
     }