Revision 8:61a3a5fb4c37, committed 2018-10-16

Comitter:

arnouddomhof

Date:

Tue Oct 16 11:43:21 2018 +0000

Parent:

7:2440100fe04c

Commit message:

Een poging tot het werkend maken van de p-controller

Changed in this revision

--- a/main.cpp	Tue Oct 16 09:13:18 2018 +0000
+++ b/main.cpp	Tue Oct 16 11:43:21 2018 +0000
@@ -5,12 +5,12 @@
 #include "QEI.h"
 
 AnalogIn  potmeter1(PTC10);
-AnalogIn  potmeter2(PTC11);
+//AnalogIn  potmeter2(PTC11);
 MODSERIAL  pc(USBTX, USBRX);
 //D4 is a digital input for the microcontroller, so should be an digitalOut
 //from the K64F. It will tell the motor shiel to let Motor1 turn clockwise
 //of count clockwise (CW of CCW). D4 for motor 2
-DigitalOut directionM2(D7);
+DigitalOut directionM1(D4);
 //D5 is a PWM input for the motor controller and determines the PWM signal 
 //that the motor controller gives to Motor 1. Higher PWM, higer average voltage.
 // D6 for motor 2
@@ -21,14 +21,18 @@
 // Voor het laten zien van de data op de pc.
 HIDScope scope(1);              // Aantal kanalen wat doorgegeven wordt aan de hidscope
 Ticker AInTicker;
+Ticker Waardesmotor;
 
 // Declare variables for motor
 float pos;
 float potwaarde1;
-float potwaarde2;
+//float potwaarde2;
 const float pi = 3.14159265359;
 int counts1; 
 float theta1;
+float error1;
+float U1;
+
 
 
 void ReadAnalogIn()
@@ -45,38 +49,50 @@
     // Get angles
     theta1 = (float(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
 
-    pc.baud(115200);
-    pc.printf("Hoek motor 1 = %0.2f rad \n \r", theta1);
+    //pc.printf("Hoek motor 1 = %0.2f rad \n \r", theta1);
     return theta1; 
 }
 
+/**
 float P_controller(float error)
 {
     float Kp = 2.0;
     float U1 = Kp*error;
     return U1;  
 }
+*/
+
+void Waardes()
+{
+    pc.printf("Hoek motor 1 = %0.2f rad, hoek potmeter = %0.2f rad, error = %0.2f rad, U1 = %0.2f \n \r", theta1, pos, error1,U1);
+}
+    
 
 int main(void)
 {
+    pc.baud(115200);
     motor1_pwm.period_us(60);               // Period is 60 microseconde
-    AInTicker.attach(&ReadAnalogIn,0.01f);  // Elke 0.01 sec. Lees de analoge waarde
+    AInTicker.attach(&ReadAnalogIn,0.001f);  // Elke 0.001 sec. Lees de analoge waarde
+    Waardesmotor.attach(Waardes,1);     // Elke 1 sec wordt de waarde van motor 1 geplot
 
     while(true){
         potwaarde1 = potmeter1.read();  // Lees de potwaardes uit. Tussen 0 en 1
-        potwaarde2 = potmeter2.read();
+        //potwaarde2 = potmeter2.read();
         
                     
-        pos = (potwaarde1*2 -1) * 2*pi;   // Scale van -1 tot 1 ipv. 0 tot 1
+        pos = (potwaarde1*2 - 1) * 2*pi;   // Scale van -1 tot 1 ipv. 0 tot 1
         theta1 = ReadEncoder();
-        float error = pos - theta1;
-        float U1 = P_controller(error);
+        error1 = pos - theta1;
+        float Kp = 1;
+        U1 = Kp*float(error1)/6.28;
+
+        //float U1 = P_controller(error);
         
         // Gebruik de absolute waarde van de scaled U waardes als input voor de motor. 
         // Negatieve waardes kunnen niet naar de motor gestuurd worden.
         motor1_pwm.write(fabs(U1)); 
         
-        float directionM1 = U1 > 0.0f; //either true or false
+        directionM1 = U1 > 0.0f; //either true or false
         
         wait(0.002f);   
      }