Diff: main.cpp

Revision:

10:990287a722d2

Parent:

9:298469a70280

Child:

11:cd7be08f46b0

--- a/main.cpp	Tue Oct 29 18:55:53 2019 +0000
+++ b/main.cpp	Tue Oct 29 20:05:39 2019 +0000
@@ -23,7 +23,7 @@
 QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING);     //Encoding motor 1
 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING);   //Encoding motor 2
 float Ts = 0.01;                                //Sample time
-float motor1angle;                              //Measured angle motor 1
+float motor1angle=-135.0*0.0174532*5.5;                              //Measured angle motor 1
 float motor2angle;                              //Measured angle motor 2
 float motor1offset;                             //Offset bij calibratie
 float motor2offset;
@@ -43,8 +43,8 @@
 //Motorcontrol
 bool motordir1;
 bool motordir2;
-float motor1ref=0.1745;
-float motor2ref=0.0873;
+float motor1ref=-135.0*0.0174532*5.5;
+float motor2ref;
 double controlsignal1;
 double controlsignal2;
 double pi2= 6.283185;
@@ -108,6 +108,10 @@
 
 volatile int counts1; // Encoder counts
 volatile int counts2;
+volatile int counts1af;
+volatile int counts2af;
+int counts1offset;
+int counts2offset;
 volatile int countsPrev1 = 0;
 volatile int countsPrev2 = 0;
 volatile int deltaCounts1;
@@ -129,13 +133,13 @@
 // Ticker Functions
 void readEncoder()
 {
-    counts1 = encoder1.getPulses();        
-    deltaCounts1 = counts1 - countsPrev1;
-    countsPrev1 = counts1;
+    counts1af = encoder1.getPulses();        
+    deltaCounts1 = counts1af - countsPrev1;
+    countsPrev1 = counts1af;
     
-    counts2 = encoder2.getPulses();
-    deltaCounts2 = counts2 - countsPrev2;
-    countsPrev2 = counts2;    
+    counts2af = encoder2.getPulses();
+    deltaCounts2 = counts2af - countsPrev2;
+    countsPrev2 = counts2af;    
 }
 
 void PID_controller(){
@@ -242,6 +246,7 @@
     
 
 void motorControl(){
+    counts1= counts1af-counts1offset;
     motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     motor1error=motor1ref-motor1angle;         
@@ -252,6 +257,7 @@
     motor1Power.write(abs(controlsignal1));
     motor1Direction= motordir1;
     
+    counts2= counts2af - counts2offset;
     motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
     omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     motor2error=motor2ref-motor2angle;         
@@ -290,8 +296,8 @@
     }
     motor1Power.write(0.0f);
     motor2Power.write(0.0f);
-    motor1offset = (counts1 * factorin / gearratio);
-    motor2offset = (counts2 * factorin / gearratio);
+    counts1offset = counts1af ;
+    counts2offset = counts2af;
       
     // State transition guard
 if ( button2_pressed ) {   
@@ -314,7 +320,7 @@
     // State transition guard  
 if ( button2_pressed ) {   
         button2_pressed = false;      
-        motor_curr_state = motor_encoderset;       
+        motor_curr_state = motor_bewegen;       
         motor_state_changed = true;
         // More functions
     }    
@@ -389,9 +395,9 @@
     button2.fall(&button2Press);
     
     while (true) {
-            pc.printf("Omega1: %f Omega 2: %f  controlsignal1: %f \r\n", omega1, omega2, controlsignal1);
-            pc.printf("Currentstate: %i\r\n",motor_curr_state);
-            pc.printf("motor1offset: %f motor2offset: %f\r\n",motor1offset,motor2offset);
+            pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1);
+            pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2);
+            pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle);
         wait(0.5);
     }
 }