Diff: main.cpp

Revision:

0:259b726d0807

Child:

1:8ff6c7474d95

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Sep 01 19:45:48 2013 +0000
@@ -0,0 +1,55 @@
+#include "mbed.h"
+#include "QEI.h"  //QEI is a quadrature encoder library
+#define pi  3.14159265358979323846
+
+
+//create a new quadrature encoder object
+QEI encoder(p25, p26, NC, 1200, QEI::X4_ENCODING);       //(encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2))
+
+Ticker SampleFrequency;
+
+//Values that will be used for the PID controller
+
+int steps_per_rev = 1200; //Encoder CPR * gear ratio
+float position = 0;
+float error = 0;
+float old_error = 0;
+float goal_position = 0;
+float p_gain = 0;
+float d_gain = 0;
+float sample_period = .0004;
+float command = 0;
+
+//
+
+float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
+{
+    return ((pulses/steps_per_rev)*(2*pi));
+}
+
+
+void updateCommand(float desired_position, float KP, float KD){
+    
+    position = pulsesToRadians(encoder.getPulses());        //use the getPulses functin to read encoder position, convert value to radians
+    old_error = error;                                      
+    error = position - desired_position;
+    command = KP*error + KD*sample_period*(error-old_error);    //modify the command
+    
+    
+}
+
+void update(void){                                  //The function attached to the ticker can't take inputs
+    updateCommand(goal_position, p_gain, d_gain);
+}
+
+
+
+int main() {
+    
+
+    SampleFrequency.attach(&update, sample_period);        //Encoder will be sampled at 2.5 kHz
+    
+    while(1) {
+        
+    }
+}