Diff: main.cpp

Revision:

1:08d285e33eea

Parent:

0:9cf3d3dc621b

Child:

2:306998d368fc

--- a/main.cpp	Wed May 31 20:30:48 2017 +0000
+++ b/main.cpp	Mon Jun 11 07:47:13 2018 +0000
@@ -2,6 +2,7 @@
 #include "mbed.h"
 #include "MODSERIAL.h" //for more efficient serial comm
 #include "FastPWM.h" //To fix a bug in the standard mbed lib concerning pwm
+#include "QEI.h"
 
 //#efines--------------------------------------------------------------------------------------------------------
 #define PWMCYCLETIMEUS 50 //20 kHz PWM cycle time
@@ -11,7 +12,7 @@
 #define LOOPDURATION ((float)LOOPDURATIONUS*0.000001f) //sec
 #define LEDPERIODS 1000 //How many loops before changing the LED state
 #define CALIBPWM 0.085f //12.5% of max PWM is the calibration PWM
-#define ENCODER_CPR 1024.0f //Counts per revolution for the encode (1x quad, to avoid missing counts)
+#define ENCODER_CPR 2048.0f //Counts per revolution for the encode (1x quad, to avoid missing counts)
 #define TWO_PI 6.283185307179586476925286766559f //rad
 #define WORKSPACEBOUND 7.0f //workspace bound for admittane model
 #define MAXVEL 100.0f //maximal admittance control velocity in rad/s
@@ -27,8 +28,8 @@
 
 //Inputs
 AnalogIn    measuredForce(A5);  //Pin that measures analog force
-InterruptIn EncoderA(D2);       //Encoder interrupt for Channel A
-DigitalIn   EncoderB(D3);       //Encoder Channel B
+
+QEI enc(PTC12,PTC4,NC,1024,QEI::X2_ENCODING);
 
 //Outputs
 DigitalOut  motorDir(D4);       //Motor Direction Pin
@@ -95,15 +96,6 @@
 char buffer[0x80]; //128 byte buffer (necessary?)
 
 //Functions--------------------------------------------------------------------------------------------------------
-//Encoder Interrupt Routine
-void encoderFunctionA()
-{
-    if ((bool)EncoderA == (bool)EncoderB) { //Increment or decrement encoder position during interrupt
-        encoderPos++;
-    } else {
-        encoderPos--;
-    }
-}
 
 //Velocity filtering
 double velocityFilter(float x)
@@ -207,7 +199,8 @@
 void mapIn()
 {
     //Measure relevant signals
-    motorPos = (float)encoderPos / ENCODER_CPR * TWO_PI; //motor position in rad
+    encoderPos = encoderPos;
+    motorPos = (float)enc.getPulses() / ENCODER_CPR * TWO_PI; //motor position in rad
     encoderVel = (float)(encoderPos - encoderPos_prev) / LOOPDURATION; //encoder velocity in counts/s
     encoderPos_prev = encoderPos; //encoder position in counts
     motorVel = encoderVel / ENCODER_CPR * TWO_PI; //motor velocity in rad/s