a

Dependencies:   PID QEI USBDevice mbed

Fork of PID_Control_SU by naoto tanaka

main.cpp

Committer:
NT32
Date:
2014-04-19
Revision:
4:e9aeee8b41e4
Parent:
2:b46f1a4c42fb

File content as of revision 4:e9aeee8b41e4:

#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);

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 initialize();
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};

int main() 
{
    uint8_t i = 0;
    int epls[2] = {0, 0};
    float rps = 0;
    
    //IO,DAconverter and PID parameters configuretion.
    initialize();
    
    //this block is the demonstration of PID control until BOOT switch is pushed.
    while(SW == 1)
    {
        //this block is the outputs DAC data and revolution per second of motor.
        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);        
        }
        
        //led is blinked to display the loop of PID control.
        i++;
        gled = i;

        //calculate rps.
        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();
        
        //send a command to change output of voltage.
        cs = 0;
        spi.write(dac.command);
        cs = 1;
        wait(PIDRATE);
    }
    pidsetup();
    i = 0;
    rps = 0;
    epls[0] = 0;
    timer.reset();
    timer.start();
    vcom.printf("%d,%d,%f\n" , timer.read_ms(), dac.bit.D, rps);
    i = 0;
    while(1)
    {
        if(SW == 0)
        {
            timer.stop();
            pidsetup();
            timer.reset();
            timer.start();
        }

        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;
        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;
    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]);
    controller.setBias(200);

    //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]);
    controller.setInputLimits(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]);
    controller.setOutputLimits(0, 4095);
    
    //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);
}