Diff: main.cpp

Revision:

16:000f2ebbd16c

Parent:

15:65f295a49a4b

Child:

17:10c18ca3368b

--- a/main.cpp	Thu Nov 02 17:43:05 2017 +0000
+++ b/main.cpp	Mon Nov 06 22:50:16 2017 +0000
@@ -7,30 +7,34 @@
 #include "BiQuad.h"
 
 
-
+//Reference signals
 DigitalOut gpo(D0);
 DigitalOut ledb(LED_BLUE);
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
 
+//Motors
 DigitalOut motor1DC(D7);
 DigitalOut motor2DC(D4);
 FastPWM motor1PWM(D6);
 FastPWM motor2PWM(D5);
 
+//Position control
 AnalogIn    potMeter1(A0);
 AnalogIn    potMeter2(A1);
 AnalogIn    emg1(A2);   
 AnalogIn    emg2(A3);
 AnalogIn    emg3(A4);
 AnalogIn    emg4(A5);
+Encoder     Encoder1(D12,D13);
+Encoder     Encoder2(D8,D9);
 
 DigitalIn   button1(D11);
 DigitalIn   button2(D12);
 DigitalIn   button3(SW2);
 DigitalIn   button4(SW3);
-Encoder     Encoder1(D12,D13);
-Encoder     Encoder2(D8,D9);
+
+
 
 MODSERIAL pc(USBTX,USBRX);
 
@@ -52,16 +56,16 @@
 const double pi = 3.1415926535897;
 float   Pwmperiod = 0.001f;
 int     potmultiplier = 600;    // Multiplier for the pot meter reference which is normally between 0 and 1
-const double   gainM1 = 1/29.17;       // encoder pulses per degree theta
-const double   gainM2 = 1/105.0;       // pulses per mm
+const double   gainM1 = 1/29.17;       // 1 / encoder pulses per degree theta
+const double   gainM2 = 1/105.0;       // 1 / pulses per mm
 
 double motor1;
 double motor2;
 
 double reference_motor1 = 0;     // reference for Theta
 double reference_motor2 = 0;
-double pos_M1 = 0; // start angle theta
-double pos_M2 = 0; // start radius
+double pos_M1 = 0;               // start angle theta
+double pos_M2 = 0;               // start radius
 
 
 //Start constants PID ------------------------------- 
@@ -170,11 +174,11 @@
     double RA = in1+in2;
     
     
-    if (RA < 0.5)
+    if (RA < 0.8)
             {
                 X = X;
             }
-        else if (RA > 1.5)
+        else if (RA > 1.8)
             {
                 X = X-0.4;
             }
@@ -216,7 +220,7 @@
     
     if (potMeter2 < 0.3)
             {
-                Y = Y-0.5;
+                Y = Y-0.8;
             }
         else if (potMeter2 > 0.7)
             {
@@ -235,17 +239,29 @@
     
     double LA = in3+in4;
     
-    if (LA < 0.5)
+    if (LA < 0.8)
             {
                 Y = Y;
+                
+                ledb = 1;
+                ledr = 1;
+                ledg = 0;       //Groen
             }
-        else if (LA > 1.5)
-            {
+        else if (LA > 1.8)
+            { 
                 Y = Y-0.4;
+                
+                ledr = 1;
+                ledg = 1;       //Blau
+                ledb = 0;
             }
         else
             {
                 Y = Y+0.4;
+                
+                ledb = 1;       //Rood
+                ledr = 0;
+                ledg = 1;
             }
    // */        
     if (Y >= Y_maxTreshold){
@@ -345,26 +361,13 @@
     //motor2PWM = motor2;
 
     if(delta1 > 0.5){
-        motor1DC = 0;
-        
-        ledr = 1;
-        ledg = 1;       //Blau
-        ledb = 0;
+        motor1DC = 0; 
     }
     else if (delta1< -0.5) {
         motor1DC = 1; 
-        
-        ledb = 1;
-        ledr = 1;
-        ledg = 0;       //Groen
-        
     }
     else{ 
     motor1PWM = 0;
-        
-        ledb = 1;       //Rood
-        ledr = 0;
-        ledg = 1;
     }
     
     motor1 = abs(delta1)/1000.0;
@@ -376,25 +379,12 @@
 
 if(delta2 > 2.0){
         motor2DC = 0;
-        
-        ledr = 1;
-        ledg = 1;       //Blau
-        ledb = 0;
     }
     else if (delta2<-2.0) {
-        motor2DC = 1; 
-        
-        ledb = 1;
-        ledr = 1;
-        ledg = 0;       //Groen
-        
+        motor2DC = 1;        
     }
     else{ 
     motor2PWM = 0;
-        
-        ledb = 1;       //Rood
-        ledr = 0;
-        ledg = 1;
     }
     
     motor2 = abs(delta2)/1000.0;
@@ -416,6 +406,10 @@
     motor1PWM.period(Pwmperiod);
     motor2PWM.period(Pwmperiod);
     
+    ledr = 1;
+    ledb = 1;
+    ledg = 1;
+    
     bqc.add(&bq1).add(&bq2);
     
     while(1){