Diff: main.cpp

Revision:

7:2440100fe04c

Parent:

6:ae2ae12328b7

Child:

8:61a3a5fb4c37

--- a/main.cpp	Wed Oct 10 12:09:09 2018 +0000
+++ b/main.cpp	Tue Oct 16 09:13:18 2018 +0000
@@ -10,79 +10,73 @@
 //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 directionM1(D4);
 DigitalOut directionM2(D7);
 //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
 FastPWM motor1_pwm(D5);
-FastPWM motor2_pwm(D6);
 // For Encoder reading
 QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
-QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
 
 // Voor het laten zien van de data op de pc.
-HIDScope scope(2);              // Aantal kanalen wat doorgegeven wordt aan de hidscope
+HIDScope scope(1);              // Aantal kanalen wat doorgegeven wordt aan de hidscope
 Ticker AInTicker;
-Ticker  Encoder;
 
 // Declare variables for motor
-float U1;
-float U2;
+float pos;
 float potwaarde1;
 float potwaarde2;
+const float pi = 3.14159265359;
+int counts1; 
+float theta1;
+
 
 void ReadAnalogIn()
 {
-    scope.set(0,U1);    // Zet de potwaarde in de eerste plot bij de HID scope. Deze wordt automatisch tegen de tijd geplot
-    scope.set(1,U2);
+    scope.set(0,pos);    // Zet de potwaarde in de eerste plot bij de HID scope. Deze wordt automatisch tegen de tijd geplot
     scope.send();               // Zendt de waardes naar de pc 
     }
 
-void ReadEncoder()
+float ReadEncoder()
 {
-    const float pi = 3.14159265359;
-
-    // Declare variables
-    int counts1; 
-    int counts2;
-    float theta1;
-    float theta2;
-    
     // Get counts
     counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
-    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
 
     // Get angles
     theta1 = (float(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
-    theta2 = (float(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
 
     pc.baud(115200);
-    pc.printf("Hoek motor 1 = %0.2f rad, hoek motor 2 = %0.2f rad \n \r ", theta1, theta2);
+    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;  
+}
 
 int main(void)
 {
-    motor1_pwm.period_ms(60);               // Period is 60 ms
+    motor1_pwm.period_us(60);               // Period is 60 microseconde
     AInTicker.attach(&ReadAnalogIn,0.01f);  // Elke 0.01 sec. Lees de analoge waarde
-    Encoder.attach(&ReadEncoder, 1.0f);     // Lees elke 1 sec de encoder uit
 
     while(true){
         potwaarde1 = potmeter1.read();  // Lees de potwaardes uit. Tussen 0 en 1
         potwaarde2 = potmeter2.read();
-                      
-        U1 = potwaarde1*2 -1;   // Scale van -1 tot 1 ipv. 0 tot 1
-        U2 = potwaarde2*2 -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);
         
         // 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)); 
-        motor2_pwm.write(fabs(U2));
         
-        directionM1 = U1 > 0.0f; //either true or false
-        directionM2 = U2 > 0.0f;
+        float directionM1 = U1 > 0.0f; //either true or false
         
         wait(0.002f);   
      }