Makoto Kobayashi
機械工学実験1
Diff: main.cpp
- Revision:
- 0:41033932c9ec
- Child:
- 1:35d14e37db1b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Oct 20 11:30:35 2014 +0000
@@ -0,0 +1,346 @@
+//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;
+}
\ No newline at end of file