Makoto Kobayashi
/
Kikaikougaku_Jikken_mecolab
機械工学実験1
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; }