Diff: main.cpp

Revision:

0:155294201f36

Child:

1:0db79ea80741

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 01 15:34:03 2013 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+
+// Maken van een looptimer.
+volatile bool looptimerflag;
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+int main()
+{
+
+// Communicatie voor pc
+    MODSERIAL pc(USBTX,USBRX);
+    pc.baud(115200);
+
+// Voor aansturen motoren
+    double pi;
+    pi=3.14159265359;
+
+    double x,y;
+
+
+
+// Variabelen benoemen voor regelaar motor.
+    double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm;
+    double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm;
+    double motor1_maxu, motor2_maxu;
+    
+    double Ts;
+    
+// Sample tijd
+    Ts = 0.001;
+
+// Pinnen voor potmeter
+    AnalogIn potmeter1(PTB2);
+    AnalogIn potmeter2(PTB3);
+
+// Pinnen voor encoder
+    /* First pin should be PTDx or PTAx because those pins can be used as interruptIn */
+    Encoder motor1(PTD0,PTC9);
+    Encoder motor2(PTD2,PTC8);
+
+    /* PWM naar motor */
+    PwmOut pwm_motor1(PTA12);
+    PwmOut pwm_motor2(PTA5);
+
+    /* Pin voor richting */
+    DigitalOut motor1dir(PTD3);
+    DigitalOut motor2dir(PTD1);
+
+    //Tijd looptimer instellen.
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,Ts);
+
+
+// Oneidige loop...
+    while(true) {
+        while(looptimerflag != true);
+        looptimerflag = false;
+
+        x = (potmeter1.read()*297.0+69.8);
+        y = (potmeter2.read()*210.0+69.8);
+
+
+        theta   = atan(y/x)       ;// *   (400.0/(.5*pi));
+        r       = (sqrt(x*x+y*y)) ;// *   (2577/461.335);
+        
+
+        theta_pen   = motor1.getPosition()  *   ((.5*pi)/400);
+        r_pen       = motor2.getPosition()  *   (363/2577);
+        
+        
+        dtheta  = (theta - theta_pen);
+        dr      = (r - r_pen);
+
+        dtheta  =  
+
+        theta_pwm   = (dtheta)*0.001;   
+        r_pwm       = (dr)*0.001;
+
+        //NAAR MOTOR
+        
+        //Zorgen dat pwm tussen -1 en 1 blijft.
+        if(theta_pwm > 1) {
+            theta_pwm=1;
+        }
+        if(theta_pwm < -1) {
+            theta_pwm=-1;
+        }
+        if(r_pwm > 1) {
+            r_pwm=1;
+        }
+        if(r_pwm < -1) {
+            r_pwm=-1;
+        }
+
+        // Bepaal richting waarin motoren moeten draaien
+        if(theta_pwm > 0)
+            motor1dir.write(1);
+        else
+            motor1dir.write(0);
+        if(r_pwm > 0)
+            motor2dir.write(1);
+        else
+            motor2dir.write(0);
+        
+        // print naar pc
+        pc.printf("t=%.3f   dt=%.3f  tpwm=%.3f\n", theta, dtheta, theta_pwm);
+
+        //schrijf PWM naar motor
+        pwm_motor1.write(abs(theta_pwm));
+        pwm_motor2.write(abs(r_pwm));
+
+
+
+    }
+}