Diff: main.cpp

Revision:

0:b2973a157cb6

Child:

1:6d5c35b995fb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 15 02:02:38 2014 +0000
@@ -0,0 +1,253 @@
+#include "mbed.h"
+#include "USBSerial.h"
+#include "QEI.h"
+#include "PID.h"
+
+#define     PIDRATE 0.01
+#define     CW      0x01
+#define     CCW     0x02
+#define     STOP    0x03
+#define     FREE    0x00
+
+#define     kc      2.8
+#define     ti      16.0
+#define     td      0.0
+ 
+USBSerial   vcom;
+SPI         spi(P0_21, NC, P1_20);
+DigitalOut  cs(P1_23);
+
+//Ticker      flip;
+
+Timer       timer;
+
+PID         controller(kc, ti, td, PIDRATE);
+
+QEI         wheel (P1_15, P0_19, NC, 1, QEI::X4_ENCODING);
+
+BusOut      gled(P0_8, P0_9);
+BusOut      mdrv(P1_27, P1_26);
+
+DigitalIn   SW(P0_1);
+
+void pidsetup();
+
+union MCP4922
+{
+    uint16_t command;
+    struct
+    {
+        //DAC data bits
+        uint16_t    D   :12;
+        //Output power down control bit
+        uint8_t     SHDN:1;
+        //Outout gain select bit
+        uint8_t     GA  :1;
+        //Vref input buffer Control bit
+        uint8_t     BUF :1;
+        //DACa or DACb select bit
+        uint8_t     AB  :1;
+    }bit;
+};
+
+union MCP4922 dac = {0xF7F};
+
+   
+//void tick()
+//{
+//    static int tcnt, efpls;
+//    int epls;
+//    float etmppls;
+//    tcnt++;
+//    if(SW == 0)
+//    {
+//        wheel.reset();
+//    }
+//    if(tcnt == 40)
+//    {
+//        tcnt = 0;
+//        epls = wheel.getPulses();
+//        etmppls = (float)(epls - efpls);
+//        efpls = epls;
+//        vcom.printf("\033[%d;%dH", 0, 0);
+//        vcom.printf("count  :%9d\n", epls);
+//        vcom.printf("rev/sec:%3.6f\n", (etmppls / 3600));
+//        vcom.printf("DAC.bit:%09d\n", dac.bit.D);
+//    }
+////    cs = 0;
+////    spi.write(dac.command);
+////    cs = 1;
+//}
+
+int main() 
+{
+    uint8_t i = 0;
+    int epls[2] = {0, 0};
+    float rps = 0;
+
+//    flip.attach(tick, (PIDRATE / 2));
+    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);    
+    while(SW == 1)
+    {
+        if(i == 100)
+        {
+            i = 0;
+            vcom.printf("\033[%d;%dH", 0, 0);
+            vcom.printf("DAC.d  :%012d\n", dac.bit.D);
+            vcom.printf("rps    :%12.4f", rps);        
+        }
+        i++;
+        gled = i;
+
+        epls[1] = wheel.getPulses();
+        rps = ((float)(epls[1] - epls[0]) / PIDRATE) / 3600;
+        epls[0] = epls[1];
+        controller.setProcessValue(rps);
+        dac.bit.D = (int)controller.compute();
+        cs = 0;
+        spi.write(dac.command);
+        cs = 1;
+        wait(PIDRATE);
+    }
+    pidsetup();
+    i = 0;
+    rps = 0;
+    epls[0] = 0;
+    timer.reset();
+    timer.start();
+    while(1)
+    {
+        if(SW == 0)
+        {
+            timer.stop();
+            pidsetup();
+            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();
+        rps = ((float)(epls[1] - epls[0]) / PIDRATE) / 3600;
+        epls[0] = epls[1];
+        controller.setProcessValue(rps);
+        dac.bit.D = (int)controller.compute();
+        cs = 0;
+        spi.write(dac.command);
+        cs = 1;
+        wait(PIDRATE);  
+     
+    }
+    
+}
+
+void pidsetup()
+{
+    char str[64] = {'\0'}, *erstr;
+    float tmp[3];
+    dac.bit.D = 0;
+    cs = 0;
+    spi.write(dac.command);
+    cs = 1;
+    controller.reset();
+    vcom.printf("\033[2J");
+    vcom.printf("\033[%d;%dH", 0, 0);
+
+    //Output bias
+    vcom.printf("Input the bias for the controller output\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[0] = strtof(str, &erstr);
+    controller.setBias(tmp[0]);
+    vcom.printf("Output bias : %15f\n", tmp[0]);
+
+    //Input limits
+    vcom.printf("Input the minimum inputlimit\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[0] = strtof(str, &erstr);
+    vcom.printf("Minimum input limit : %15f\n", tmp[0]);
+    vcom.printf("Input the maximum inputlimit\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[1] = strtof(str, &erstr);
+    vcom.printf("Maximum input limit : %15f\n", tmp[1]);
+    controller.setInputLimits(tmp[0], tmp[1]);
+
+    //Output limits
+    vcom.printf("Input the minimum outputlimit\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[0] = strtof(str, &erstr);
+    vcom.printf("Minimum output limit : %15f\n", tmp[0]);
+    vcom.printf("Input the maximum outputlimit\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[1] = strtof(str, &erstr);
+    vcom.printf("Maximum output limit : %15f\n", tmp[1]);
+    controller.setOutputLimits(tmp[0], tmp[1]);
+    
+    //Setpoint
+    vcom.printf("Input the setpoint\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[0] = strtof(str, &erstr);
+    controller.setSetPoint(tmp[0]);
+    vcom.printf("Setpoint : %15f\n", tmp[0]);
+    
+    //tuning parameter
+    vcom.printf("Input the proportional gain\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[0] = strtof(str, &erstr);
+    vcom.printf("proportional gain : %15f\n", tmp[0]);
+    vcom.printf("Input the integral gain\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[1] = strtof(str, &erstr);
+    vcom.printf("integral gain : %15f\n", tmp[1]);
+    vcom.printf("Input the derivative gain\n");
+    vcom.printf("within 15 figures.\n");
+    vcom.scanf("%s", str);
+    tmp[2] = strtof(str, &erstr);
+    vcom.printf("derivative gain : %15f\n", tmp[2]);
+    controller.setTunings(tmp[0], tmp[1], tmp[2]);
+    
+    //interval
+//    vcom.printf("Input the interval seconds\n");
+//    vcom.printf("within 15 figures.\n");
+//    vcom.scanf("%s", str);
+//    tmp[0] = strtof(str, &erstr);
+    controller.setInterval(PIDRATE);
+    vcom.printf("\033[2J");
+    
+    
+        
+    //PID mode
+    controller.setMode(AUTO_MODE);
+}
\ No newline at end of file