Diff: main.cpp

Revision:

0:85ca550760e9

Child:

1:b66e14435f70

diff -r 000000000000 -r 85ca550760e9 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 09 13:52:37 2017 +0000
@@ -0,0 +1,78 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "math.h"
+
+
+
+DigitalOut gpo(D0);
+DigitalOut ledb(LED_BLUE);
+DigitalOut ledr(LED_RED);
+DigitalOut ledg(LED_GREEN);
+DigitalOut motor1DC(D7);
+DigitalOut motor1PWM(D6);
+DigitalOut motor2DC(D4);
+DigitalOut motor2PWM(D5);
+
+AnalogIn   potMeter1(A0);
+AnalogIn   potMeter2(A1);
+DigitalIn  button1(D11);
+DigitalIn  button2(D12);
+QEI Encoder1(D12,D13,NC,4200);
+
+MODSERIAL pc(USBTX,USBRX);
+
+Ticker controller;
+
+const double pi = 3.1415926535897;
+const float MOTOR1_KP = 5;
+const float RAD_PER_PULSE = (2*pi)/4200;
+const float CONTROLLER_TS = 0.01;     //TIME INTERVAL/ hZ
+
+float P(double error, const float Kp){
+    return Kp * error;
+    }
+
+void motor1_Controller(){
+    double reference = 10*potMeter1.read();
+    double position = RAD_PER_PULSE*Encoder1.getPulses();
+    double motor1 = P(reference-position, MOTOR1_KP);
+    motor1PWM = motor1;
+    
+    if(motor1 > 0.1){
+        motor1DC = 1;
+        
+        ledr = 1;
+        ledg = 1;       //Blau
+        ledb = 0;
+    }
+    else if (motor1<-0.1) {
+        motor1DC = 0; 
+        
+        ledb = 1;
+        ledr = 1;
+        ledg = 0;       //Groen
+        
+    }
+    else{ 
+    motor1PWM = 0;
+        
+        ledb = 1;       //Rood
+        ledr = 0;
+        ledg = 1;
+    }
+    
+    
+}
+
+int main(){
+    pc.baud(115200);
+    controller.attach(&motor1_Controller, CONTROLLER_TS);      
+    
+    while(1){    
+    double reference = 10*potMeter1.read();
+    double position = RAD_PER_PULSE*Encoder1.getPulses();
+    double motor1 = P(reference-position, MOTOR1_KP);
+      pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);         
+    }
+}