Diff: main.cpp

Revision:

11:d31b03b05f59

Parent:

10:b2742f42de44

Child:

12:e3c5c5acbd09

--- a/main.cpp	Fri Oct 02 11:31:50 2015 +0000
+++ b/main.cpp	Mon Oct 05 10:51:49 2015 +0000
@@ -1,126 +1,79 @@
 #include "mbed.h"
-#include "MODSERIAL.h"
+// #include "MODSERIAL.h"
 #include "encoder.h"
 #include "HIDScope.h"
 
-Serial pc(USBTX,USBRX);
-HIDScope scope(2);
+// Serial pc(USBTX,USBRX);
+HIDScope scope(1);
 
 Ticker          mod;
 
-//MOTOR
-// motor 1 en 2 zijn omgedraait
-// pinnen motor 1 -> pinnen motor 2
-
 //motor 1 gegevens
 PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
-DigitalOut motor1_rich(D7); // digitaal signaal voor richting 
+DigitalOut motor1_rich(D7); // digitaal signaal voor richting
 // einde motor 1
 
-//motor 2 gegevens
-PwmOut motor2_aan(D5);      // PWM signaal motor 1 (uit sheets)
-DigitalOut motor2_rich(D4); // digitaal signaal voor richting 
-// einde motor 1
-//EINDE MOTOR
-
 // ENCODER
 Encoder motor1_enc(D12,D11);
-Encoder motor2_enc(D10,D9);
-
 
 //POTMETERS
-AnalogIn potleft(A1);
 AnalogIn potright(A0);
 
-DigitalIn button(PTA4);
-int button_on = 0;
-
 
-void move_mot1(double left)
+double setpoint;
+const double K = 2 ;
+
+// counts 2 radians
+double get_radians(double counts)
 {
-    if(left < 0.4)
-    {
-        double calc1 = left - 1;
-        double calc2 = abs(calc1);
-        double leftin1 = (calc2-0.6)*2.5 ;
-        motor1_aan.write(leftin1);
-        motor1_rich.write(0);
-    }
-    else if(left > 0.4 && left < 0.6)
-    {
-        motor1_aan.write(0);
-    }
-    else if(left > 0.6)
-    {
-        double leftin2 = (left-0.6)*2.5;
-        motor1_aan.write(leftin2);
-        motor1_rich.write(1);
-    }
+    double pi = 3.14159265359;
+    double radians = (counts/4200)*2*pi;
+    return radians;
 }
 
-void move_mot2(double right)
+double setpoint_f(double input)
 {
-    if(right < 0.4)
-    { 
-        double calc3 = right - 1;
-        double calc4 = abs(calc3);
-        double rightin1 = (calc4-0.6)*2.5 ;
-        motor2_aan.write(rightin1);
-        motor2_rich.write(0);
-    }
-    else if(right > 0.4 && right < 0.6)
-    {
-        motor2_aan.write(0);
-    }
-    else if(right > 0.6)
-    {
-        double rightin2 = (right-0.6)*2.5;
-        motor2_aan.write(rightin2);
-        motor2_rich.write(1);
-    }
-}  
+    double offset = 0.5;
+    double gain = 4;
+    double output = (input-offset)*gain;
+    return output;
+}
+
+double K_control()
+{
+    double setpoint = setpoint_f(potright.read());
+    double rads = get_radians(motor1_enc.getPosition());
+    double error = (setpoint - rads);
+    double output = K*error;
+    return output;
+}
+
 
 void send()
 {
-    int counts1 = motor1_enc.getPosition();
-    int counts2 = motor2_enc.getPosition();
-    pc.printf("motor 1 counts, %d motor 2 counts %d \n", counts1, counts2);
-
-    }
-    
-double get_radians(double counts)
-{
-   double pi = 3.14159265359;
-   double radians = (counts/4200)*2*pi;
-   return radians;   
+    scope.set(0,setpoint);
+    scope.send();
 }
 
+void motor1_control()
+{
+    double output = K_control();
+    if(output > 0) {
+        motor1_rich.write(0);
+        motor1_aan.write(1);
+    } else if(output < 0) {
+        motor1_rich.write(1);
+        motor1_aan.write(1);
+    }
+}
 
 
 int main()
 {
-    pc.baud(115200);
-    mod.attach(&send,1);
-    while(true)
+    double output ;
+    mod.attach(&send,0.01);
+    mod.attach(&motor1_control, 0.1);
+    while(true) 
     {
-        double left = potleft.read();
-        double right = potright.read();        
-        move_mot1(left);
-        move_mot2(right);
-        int counts1 = motor1_enc.getPosition();
-        int counts2 = motor2_enc.getPosition();
-        double radians_1 = get_radians(counts1);
-        double radians_2 = get_radians(counts2); 
-        
-        if( button.read() == button_on)
-        {
-            motor1_enc.setPosition(0);
-            motor2_enc.setPosition(0);
-        }
-        scope.set(0,radians_1);     
-        scope.set(1,radians_2);     
-        scope.send();
-        wait(0.01f);            // is noodzakelijk voor functioneren gradual increase, waarom????
-    
     }
 }