機械工学実験1

Dependencies:   PID QEI mbed

main.cpp

Committer:
neoqased
Date:
2014-10-20
Revision:
0:41033932c9ec
Child:
1:35d14e37db1b

File content as of revision 0:41033932c9ec:

//include header file
#include "mbed.h"
#include "QEI.h"
#include "PID.h"
#include "setting.h"

//define
#define PULSE_PER_REVOLUTION 200
#define PID_RATE 10     //wait **ms

//IO pin setting
DigitalOut led1(LED1);      //mbed LED outputs
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalIn dip1(p5);         //DIP switch inputs
DigitalIn dip2(p6);
DigitalIn dip3(p7);
DigitalIn dip4(p8);
DigitalIn startsw(p9);      //push switch input
DigitalOut buzzer(p18);     //buzzer output
AnalogIn volume(p20);       //potentiometer input
PwmOut pwm(p25);            //pwm output to a motordriver

//other setting
Serial pc(USBTX, USBRX);        //set serial communication with PC
QEI encoder(p30, p29, NC, PULSE_PER_REVOLUTION, QEI::X2_ENCODING);      //QEI setting (See QEI.cpp)
Timer timer;                //mbed timer setting (See mbed description)

//structure of experimentation result
typedef struct{
    float t;
    float rpm;
} result;

//prototype declaration
int Init();
int DipLed();
int GetDipValue();
int SW();
int Buzzer(int buzvar);

//main function
int main() {
    Buzzer(2);  //ring buzzer after pushing a reset button
    LocalFileSystem local("local");     //set mbed root directory as "local"
    Init();     //initialize
    int flag = 0;       //variable of operation mode
    result exp[(int)(15*1000/PID_RATE+1)] = {};     //variable for experimentation result
    
    while(1){       //main loop
        Init();     //initialize
        for(int i = 0; i <= 15*1000/PID_RATE; i++){     //initialize result structure
            exp[i].t = 0;
            exp[i].rpm = 0;
        }
        SW();       //wait for startswitch push
        flag = GetDipValue();       //read DIP switch value
        DipLed();       //make mbed LED shine
        Buzzer(1);      //ring buzzer mode 1
        
        switch (flag){
            /*********************mode 0**********************/
            case 0:         //mode 0: nothing
                break;
            /*********************mode 1**********************/
            case 1:{        //mode 1: manually operated mode
                float vol = 0;      //variable of potentiometer
                float rpm = 0;      //variable of rpm
                float t = 0;        //variable of time
                int i1 = 0;         //variable of loop count
                encoder.reset();
                timer.reset();
                timer.start();
                while(1){
                    vol = 1 - volume;       //min position(max input(H))<-----potentiometer=====>(min input(L))max position
                    pwm = vol;
                    if(i1 >= 20){           //show duty ratio and rpm in PC every sec
                        t = timer.read();
                        rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / t * 2;  //reduction ratio 1/2
                        pc.printf("Duty Ratio = %.3f , %6d RPM\n", vol, (int)rpm);
                        encoder.reset();
                        i1 = 0;
                        timer.reset();
                    }
                    wait_ms(50);
                    i1++;
                }
                break;
            }
            /*********************mode 2**********************/
            case 2:{        //mode 2: step input
                exp[0].t = 0;
                exp[0].rpm = 0;
                encoder.reset();
                timer.reset();
                timer.start();
                pwm = 1;        //duty ratio 1
                for(int i = 0; i < 15*1000/PID_RATE; i++){      //variable assignment in every PID_RATE
                    wait_ms(PID_RATE);
                    exp[i+1].t = timer.read();
                    exp[i+1].rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / (exp[i+1].t - exp[i].t) * 2;
                    encoder.reset();
                }
                for(int i = 100; i >= 0; i--){      //stopping motor slowly
                    pwm = (float)i / 100;
                    wait_ms(20);
                }
                pwm = 0;
                wait(1);
                FILE * fp = fopen("/local/exp2.csv","w");
                if(fp == NULL){
                    Buzzer(-1);
                    break;
                }
                fprintf(fp,"Time , RPM\n");
                for(int i = 0; i <= 15*1000/PID_RATE; i++){     //output results
                    fprintf(fp,"%f , %f\n", exp[i].t, exp[i].rpm);
                }
                fclose(fp);
                Buzzer(4);      //ring finish buzzer
                break;
            }
            /*********************mode 3**********************/
            case 3:{        //mode 3: PID control
                float kp = 0, ti = 0, td = 0, max_rpm = 0, target_rpm = 0, reduction = 0;
                encoder.reset();
                timer.reset();
                
                kp = KP;        //substitute from setting.h
                ti = TI;
                td = TD;
                max_rpm = MAX_RPM;
                target_rpm = TARGET_RPM;
                
                PID pid(kp, ti, td, PID_RATE);      //PID setting (see PID.cpp)
                pid.setInputLimits(0.0, max_rpm);
                pid.setOutputLimits(0.0, 1.0);
                pid.setMode(AUTO_MODE);
                pid.setSetPoint(target_rpm);
                
                exp[0].t = 0;
                exp[0].rpm = 0;
                timer.start();      //count start
                for(int i = 0; i < 15*1000/PID_RATE; i++){      //PID control loop(15 seconds)
                    exp[i+1].t = timer.read();
                    exp[i+1].rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / (exp[i+1].t - exp[i].t) * 2;
                    pid.setProcessValue(exp[i+1].rpm);
                    pwm = pid.compute();
                    encoder.reset();
                    wait_ms(PID_RATE);
                }
                reduction = pwm.read();     //stopping slowly loop
                for(int i = 0; i <= 100; i++){
                    pwm = pwm.read() - reduction / 100;
                    wait_ms(20);
                }
                pwm = 0;
                FILE *fp = fopen("/local/exp3.csv","w");
                if(fp == NULL){
                    Buzzer(-1);
                    break;
                }
                fprintf(fp,", kp = %f  ki = %f  kd = %f\n",kp, ti, td);
                fprintf(fp,"Time , RPM\n");
                for(int i = 0; i <= 15*1000/PID_RATE; i++){     //output results
                    fprintf(fp,"%f , %f\n", exp[i].t, exp[i].rpm);
                }
                fclose(fp);
                Buzzer(5);      //ring finish buzzer
                break;
            }
            /*********************default**********************/
            default:
                break;
        }
    }
}

int Init(){     //initialize function
    pwm = 0;
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    buzzer = 0;
    pc.baud(9600);
    encoder.reset();
    timer.reset();
    pwm.period_us(25);
    DipLed();
    return 0;
}

int DipLed(){       //LED flash function
    if(dip1 == 1) led1 = 1; else led1 = 0;
    if(dip2 == 1) led2 = 1; else led2 = 0;
    if(dip3 == 1) led3 = 1; else led3 = 0;
    if(dip4 == 1) led4 = 1; else led4 = 0;
    return 0;
}

int GetDipValue(){      //DIP switch function
    int Dip1 = dip1, Dip2 = dip2, Dip3 = dip3, Dip4 = dip4;
    if(Dip1 == 0){
        if(Dip2 == 0){
            if(Dip3 == 0){
                if(Dip4 == 0) return 0; else return 1;
            }else{
                if(Dip4 == 0) return 2; else return 3;
            }
        }else{
            if(Dip3 == 0){
                if(Dip4 == 0) return 4; else return 5;
            }else{
                if(Dip4 == 0) return 6; else return 7;
            }
        }
    }else{
        if(Dip2 == 0){
            if(Dip3 == 0){
                if(Dip4 == 0) return 8; else return 9;
            }else{
                if(Dip4 == 0) return 10; else return 11;
            }
        }else{
            if(Dip3 == 0){
                if(Dip4 == 0) return 12; else return 13;
            }else{
                if(Dip4 == 0) return 14; else return 15;
            }
        }
    }
}

int SW(){       //startswitch function
    int i = 0, j = 0;
    while(i < 3){
        if(startsw == 1) i++;
        else i = 0;
        DipLed();
        wait_ms(5);
    }
    while(j < 3){
        if(startsw == 0) j++;
        else j = 0;
        DipLed();
        wait_ms(5);
    }
    return 0;
}

int Buzzer(int buzvar){     //ringing buzzer function
    switch (buzvar){
        case -3:        //error * - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        case -2:        //error * - - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        case -1:        //error * - - - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        case 0:         //no sound
            buzzer = 0;
            break;
        case 1:         //*
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 2:         //* *
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.05);
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 3:         //-
            buzzer = 1; wait(0.3); buzzer = 0;
            break;
        case 4:         //- -
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0;
            break;
        case 5:         //---
            buzzer = 1; wait(0.9); buzzer = 0;
            break;
        case 6:         //* * *  * * *  * * *  *
            for(int i = 0; i < 3; i++){
                for(int j = 0; j < 3; j++){
                    buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
                }
                wait(0.2);
            }
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 7:         // **-* ** -* ** *** ****
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 8:         // *-*** -*- --*
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        default:        //no sound
            buzzer = 0;
            break;
    }
    return 0;
}