Diff: main.cpp

Revision:

6:16da0de99a8c

Parent:

5:d41998e421ed

Child:

7:59613b7a1631

--- a/main.cpp	Tue Dec 03 16:16:14 2013 +0000
+++ b/main.cpp	Tue Dec 03 16:47:34 2013 +0000
@@ -7,7 +7,8 @@
 #define MOTOR_PPR 300
 #define ENCODER_PPR 1024
 
-#define QUADRATURE_TYPE  2
+#define ENC_QUADRATURE_TYPE  4
+#define MOT_QUADRATURE_TYPE  2
 #define OUR_PI  3.141592653589793
 #define DATA_COLS 7
 #define BUFFER_SIZE 4200
@@ -17,7 +18,7 @@
 
 Serial pc(USBTX, USBRX);
 
-QEI encoder(p29, p30, NC, ENCODER_PPR);
+QEI encoder(p29, p30, NC, ENCODER_PPR, QEI::X4_ENCODING);
 QEI motor(p25, p26, NC, MOTOR_PPR);
 Timer T;
 
@@ -43,17 +44,18 @@
 //float k1 = -0.0316, k2 = 9.7076, k3 = -0.4095, k4 = 1.2340, k5 = 0.0410;
 float k1 = -0.3162, k2 = 18.278, k3 = -0.8964, k4 = 2.4441, k5 = 0.1843;
 
-float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(QUADRATURE_TYPE));
-float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(QUADRATURE_TYPE));
+float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(ENC_QUADRATURE_TYPE));
+float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(MOT_QUADRATURE_TYPE));
 
 float* buffer;
 float lambda1 = 30, lambda2 = 30, lambda3 = 15;
 int index;
 int pulsesPend, pulsesMot;
+bool flag = 0;
 
 void saving(void const *args) {
     index = 0;
-    while (index < BUFFER_SIZE) {
+    while ((index < BUFFER_SIZE)&&(flag == 1))  {
         buffer[index] = theta1;
         buffer[index+1] = theta2;
         buffer[index+2] = dtheta1;
@@ -89,9 +91,13 @@
         
         //set pwm 
         // ADD A SANITY CHECK ON THETA
-        inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent); 
-        if (cos(theta2) < 0.98) 
+        if (cos(theta2) < 0.98) {
+            flag = 0;
             inputVoltage = 0.0;
+        } else {
+            flag = 1;
+            inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
+        }
         setVoltage(inputVoltage);
         
         //read current