Diff: main.cpp

Revision:

12:614f1439e679

Parent:

11:e2b2ea8a3be8

--- a/main.cpp	Tue Oct 27 14:46:32 2015 +0000
+++ b/main.cpp	Wed Oct 28 10:02:33 2015 +0000
@@ -1,11 +1,15 @@
 #include "mbed.h"
 #include "QEI.h"
+#include <math.h>
 
 Serial pc(USBTX, USBRX);
 //QEI wheel_one (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
-//QEI wheel_one (PTD3, PTD1, NC, 624);
+QEI wheel_one (PTD3, PTD1, NC, 624);
 QEI wheel_two (PTD0, PTD2, NC, 624);
 
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_red(LED_RED);
+
 // Define pin for motor control
 DigitalOut directionPin_one(D4);
 PwmOut PWM_one(D5);
@@ -107,16 +111,50 @@
 
 }
 
+//control for the keypress
+void motor2_Controller(DigitalOut directionPin, PwmOut PWM, QEI &wheel, double setpoint)
+{
+    double reference = setpoint; // setpoint is in pulses
+    double position = wheel_two.getPulses();
+    double error_pulses = (reference - position); // calculate the error in pulses
+    double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
+        
+    while (fabs(error_pulses) > 40)
+    { 
+        //kijken = wheel_two.getPulses()/pulses_per_revolution;       
+        
+        double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
+    
+        if(error_pulses > 0) {
+            directionPin.write(cw);
+        
+        }
+        else if(error_pulses < 0) {
+            directionPin.write(ccw); 
+        }
+        else{
+            output = 0;
+            }
+        
+        PWM.write(output); // out of the if loop due to abs output
+        
+        position = wheel.getPulses();
+        error_pulses = (reference - position);
+        error_rotation = error_pulses / pulses_per_revolution;
+    }
 
-void counts_showing()
+}
+
+
+void counts_showing(double setpoint, QEI &wheel)
 {
-
+    
+    //double error_pulses = setpoint - wheel_one.getPulses();
     //double kijken = wheel_one.getPulses()/pulses_per_revolution;
-    //pc.printf("ref %.2f rounds%.2f \n",setpoint_one/pulses_per_revolution,kijken);
-    
-    double kijken = wheel_two.getPulses()/pulses_per_revolution;
-    double pulses = wheel_two.getPulses();
-    pc.printf("ref %.2f rounds%.2f pulses %.2f \n",setpoint_two/pulses_per_revolution,kijken, pulses);
+    //pc.printf("ref %.2f rounds%.2f error pulses%.2f \n",setpoint_one/pulses_per_revolution,kijken, error_pulses);
+    double position = wheel.getPulses();
+    double error_pulses = setpoint - position;
+    pc.printf("ref %.2f position%.2f error_pulses%.2f \n",setpoint, position, error_pulses);
 
 }
 
@@ -126,33 +164,47 @@
     Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
     while( 1 ) {
 // to make the motor move
+        led_green = 0;
         if(flag_motor) {
             flag_motor = false;
             //motor1_Controller(directionPin_one, PWM_one, wheel_one, setpoint_one);
-            motor1_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two);
+            //motor1_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two);
         }
 
         if(flag_pcprint) {
             flag_pcprint = false;
-            counts_showing();
+            counts_showing(setpoint_two, wheel_two);
         }
 
+        led_green = 1;
 // Pussing buttons to get input s0ignal
-/*
-`       while(buttoncw.read() == Buttoncw_pressed and buttonccw.read() == Buttonccw_pressed)
+        /*
+        if(buttoncw.read() == Buttoncw_pressed and buttonccw.read() == Buttonccw_pressed)
         {
-            setpoint_two = making_setpoint(
-  */          
+            led_red = 0;
+            
+            
+            setpoint_two = setpoint_two + 1050;
+            motor2_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two);
+            setpoint_two = setpoint_two - 1050;
+            motor2_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two);
+            
+            led_green = 0;
+            wait(1);
+            led_green = 1;
+            
+        }*/
+        //led_red = 1; 
         if(buttoncw.read() == Buttoncw_pressed)
         {
-            //setpoint_one = making_setpoint(cw, setpoint_one);
-            setpoint_two = making_setpoint(ccw, setpoint_two);
+            setpoint_one = making_setpoint(cw, setpoint_one);
+            //setpoint_two = making_setpoint(ccw, setpoint_two);
         }
         
         if(buttonccw.read() == Buttonccw_pressed) 
         {
-            //setpoint_one =  making_setpoint(ccw, setpoint_one);
-            setpoint_two = making_setpoint(cw, setpoint_two);
+            setpoint_one =  making_setpoint(ccw, setpoint_one);
+            //setpoint_two = making_setpoint(cw, setpoint_two);
         }
 
     }