Diff: main.cpp

Revision:

2:b46f1a4c42fb

Parent:

1:6d5c35b995fb

Child:

3:e9aeee8b41e4

--- a/main.cpp	Tue Apr 15 02:10:03 2014 +0000
+++ b/main.cpp	Thu Apr 17 07:14:37 2014 +0000
@@ -28,6 +28,7 @@
 
 DigitalIn   SW(P0_1);
 
+void initialize();
 void pidsetup();
 
 union MCP4922
@@ -55,29 +56,7 @@
     uint8_t i = 0;
     int epls[2] = {0, 0};
     float rps = 0;
-
-    SW.mode(PullUp);
-    mdrv = CW;
-    cs = 1;
-    dac.bit.AB = 0;
-    dac.bit.BUF = 1;
-    dac.bit.GA = 1;
-    dac.bit.SHDN = 1;
-    dac.bit.D = 0;
-    spi.format(16,0);
-    spi.frequency(20000000);
-    cs = 0;
-    spi.write(dac.command);
-    cs = 1;
-    //Revolution per second input from 0.0 to 50.0rev/sec
-    controller.setInputLimits(0.0, 30.0);
-    //DAC output from 0.0 to 4096.0
-    controller.setOutputLimits(0.0, 4095.0);
-    //If there's a bias.
-    controller.setBias(1000.0);
-    controller.setMode(AUTO_MODE);
-    //We want the process variable to be 12rev/sec
-    controller.setSetPoint(12.0);    
+    initialize();
     while(SW == 1)
     {
         if(i == 100)
@@ -115,11 +94,7 @@
             timer.reset();
             timer.start();
         }
-        if(i == 10)
-        {
-            vcom.printf("%d,%d\n" , timer.read_ms(), dac.bit.D);
-            i = 0;
-        }
+
         i++;
         gled = i;
         epls[1] = wheel.getPulses();
@@ -130,12 +105,43 @@
         cs = 0;
         spi.write(dac.command);
         cs = 1;
+        if(i == 10)
+        {
+            vcom.printf("%d,%d,%f\n" , timer.read_ms(), dac.bit.D, rps);
+            i = 0;
+        }
         wait(PIDRATE);  
      
     }
     
 }
 
+void initialize()
+{
+    SW.mode(PullUp);
+    mdrv = CW;
+    cs = 1;
+    dac.bit.AB = 0;
+    dac.bit.BUF = 1;
+    dac.bit.GA = 1;
+    dac.bit.SHDN = 1;
+    dac.bit.D = 0;
+    spi.format(16,0);
+    spi.frequency(20000000);
+    cs = 0;
+    spi.write(dac.command);
+    cs = 1;
+    //Revolution per second input from 0.0 to 50.0rev/sec
+    controller.setInputLimits(0.0, 30.0);
+    //DAC output from 0.0 to 4096.0
+    controller.setOutputLimits(0.0, 4095.0);
+    //If there's a bias.
+    controller.setBias(1000.0);
+    controller.setMode(AUTO_MODE);
+    //We want the process variable to be 12rev/sec
+    controller.setSetPoint(12.0);      
+}
+
 void pidsetup()
 {
     char str[64] = {'\0'}, *erstr;