Kengo Kumagai / Mbed 2 deprecated denku_mock_1_valve

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   EC def_number mbed

Fork of denku_mock_1 by jiei suzuki

main.cpp@0:e4cee11fca3b, 2017-11-13 (annotated)

Committer:
jack0325suzu
Date:
Mon Nov 13 04:16:15 2017 +0000
Revision:
0:e4cee11fca3b
Child:
1:548ad0825df5
??????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jack0325suzu 0:e4cee11fca3b 1 #include "mbed.h"
jack0325suzu 0:e4cee11fca3b 2 #include "EC.h"
jack0325suzu 0:e4cee11fca3b 3 #include "number.h"
jack0325suzu 0:e4cee11fca3b 4
jack0325suzu 0:e4cee11fca3b 5 #define pi 3.1415926535
jack0325suzu 0:e4cee11fca3b 6 #define CALC_INTERVAL 0.01
jack0325suzu 0:e4cee11fca3b 7 #define TRY_MODE '0'
jack0325suzu 0:e4cee11fca3b 8 #define ROTATION_MODE '1'
jack0325suzu 0:e4cee11fca3b 9 #define PID_MODE '2'
jack0325suzu 0:e4cee11fca3b 10 #define SOLUTION 500
jack0325suzu 0:e4cee11fca3b 11
jack0325suzu 0:e4cee11fca3b 12 SpeedControl em(PA_6,PA_7,NC,SOLUTION,CALC_INTERVAL,PB_5,PB_4);
jack0325suzu 0:e4cee11fca3b 13 Serial pc(USBTX,USBRX);
jack0325suzu 0:e4cee11fca3b 14 void setup();
jack0325suzu 0:e4cee11fca3b 15 void print_scan();
jack0325suzu 0:e4cee11fca3b 16
jack0325suzu 0:e4cee11fca3b 17 int loop_kai=0;
jack0325suzu 0:e4cee11fca3b 18 char mode='0';
jack0325suzu 0:e4cee11fca3b 19 double duty=0;
jack0325suzu 0:e4cee11fca3b 20 float data[500][2];
jack0325suzu 0:e4cee11fca3b 21 int i=0;
jack0325suzu 0:e4cee11fca3b 22
jack0325suzu 0:e4cee11fca3b 23 double target_rotation=0,now_rotation=0,old_rotation=0;
jack0325suzu 0:e4cee11fca3b 24 double diff=0,diff_old=0,integral=0;
jack0325suzu 0:e4cee11fca3b 25 double Kp=0,Kd=0,Ki=0;
jack0325suzu 0:e4cee11fca3b 26
jack0325suzu 0:e4cee11fca3b 27 Ticker omega_tick;
jack0325suzu 0:e4cee11fca3b 28 Timer timer;
jack0325suzu 0:e4cee11fca3b 29 Num num(USBTX,USBRX);
jack0325suzu 0:e4cee11fca3b 30
jack0325suzu 0:e4cee11fca3b 31 void CalOmega()
jack0325suzu 0:e4cee11fca3b 32 {
jack0325suzu 0:e4cee11fca3b 33 em.CalPreOmega();
jack0325suzu 0:e4cee11fca3b 34 if(mode==ROTATION_MODE&&i<500){
jack0325suzu 0:e4cee11fca3b 35 data[i][0]=(float)timer.read();
jack0325suzu 0:e4cee11fca3b 36 data[i][1]=(float)em.getOmega()*60/(2*pi);
jack0325suzu 0:e4cee11fca3b 37 i++;
jack0325suzu 0:e4cee11fca3b 38 } else if(mode==PID_MODE){
jack0325suzu 0:e4cee11fca3b 39 now_rotation=em.getCount()/SOLUTION;
jack0325suzu 0:e4cee11fca3b 40 }
jack0325suzu 0:e4cee11fca3b 41 }
jack0325suzu 0:e4cee11fca3b 42
jack0325suzu 0:e4cee11fca3b 43 int main(void){
jack0325suzu 0:e4cee11fca3b 44 setup();
jack0325suzu 0:e4cee11fca3b 45
jack0325suzu 0:e4cee11fca3b 46 while(1){
jack0325suzu 0:e4cee11fca3b 47 if(mode==ROTATION_MODE){
jack0325suzu 0:e4cee11fca3b 48 if(i==500){
jack0325suzu 0:e4cee11fca3b 49 for(int j=0;j<500;j++){
jack0325suzu 0:e4cee11fca3b 50 pc.printf("%f,%f\r\n",data[j][0],data[j][1]);
jack0325suzu 0:e4cee11fca3b 51 }
jack0325suzu 0:e4cee11fca3b 52 i++;
jack0325suzu 0:e4cee11fca3b 53 }
jack0325suzu 0:e4cee11fca3b 54 } else if(mode==PID_MODE){
jack0325suzu 0:e4cee11fca3b 55 diff=target_rotation-now_rotation;
jack0325suzu 0:e4cee11fca3b 56 integral+=diff;
jack0325suzu 0:e4cee11fca3b 57 duty=Kp*diff+Kd*(diff-diff_old)+Ki*integral;
jack0325suzu 0:e4cee11fca3b 58 diff_old=diff;
jack0325suzu 0:e4cee11fca3b 59 }
jack0325suzu 0:e4cee11fca3b 60 if(duty>0){
jack0325suzu 0:e4cee11fca3b 61 em.turnF(duty);
jack0325suzu 0:e4cee11fca3b 62 } else {
jack0325suzu 0:e4cee11fca3b 63 em.turnB(-1*duty);
jack0325suzu 0:e4cee11fca3b 64 }
jack0325suzu 0:e4cee11fca3b 65 if(loop_kai%=100000){
jack0325suzu 0:e4cee11fca3b 66 if(mode==TRY_MODE||mode==PID_MODE)print_scan();
jack0325suzu 0:e4cee11fca3b 67 if(loop_kai==300000)loop_kai=0;
jack0325suzu 0:e4cee11fca3b 68 }
jack0325suzu 0:e4cee11fca3b 69 loop_kai++;
jack0325suzu 0:e4cee11fca3b 70 }
jack0325suzu 0:e4cee11fca3b 71 }
jack0325suzu 0:e4cee11fca3b 72
jack0325suzu 0:e4cee11fca3b 73 void setup(){
jack0325suzu 0:e4cee11fca3b 74 pc.printf("Mode 0 : otameshi\r\n");
jack0325suzu 0:e4cee11fca3b 75 pc.printf("Mode 1 : jikkenn1\r\n");
jack0325suzu 0:e4cee11fca3b 76 pc.printf("Mode 2 : jikkenn2\r\n");
jack0325suzu 0:e4cee11fca3b 77 pc.printf("Input mode : ");
jack0325suzu 0:e4cee11fca3b 78 while(1){
jack0325suzu 0:e4cee11fca3b 79 if(pc.readable()) {
jack0325suzu 0:e4cee11fca3b 80 mode=pc.getc();
jack0325suzu 0:e4cee11fca3b 81 pc.printf("%c\r\n",mode);
jack0325suzu 0:e4cee11fca3b 82 break;
jack0325suzu 0:e4cee11fca3b 83 }
jack0325suzu 0:e4cee11fca3b 84 }
jack0325suzu 0:e4cee11fca3b 85 if(mode==ROTATION_MODE){
jack0325suzu 0:e4cee11fca3b 86 pc.printf("duty= ");
jack0325suzu 0:e4cee11fca3b 87 duty=num.get_number();
jack0325suzu 0:e4cee11fca3b 88 } else if(mode==PID_MODE){
jack0325suzu 0:e4cee11fca3b 89 pc.printf("target_rotation= ");
jack0325suzu 0:e4cee11fca3b 90 target_rotation=num.get_number();
jack0325suzu 0:e4cee11fca3b 91 }
jack0325suzu 0:e4cee11fca3b 92 pc.printf("start !\r\n");
jack0325suzu 0:e4cee11fca3b 93 timer.start();
jack0325suzu 0:e4cee11fca3b 94 omega_tick.attach(CalOmega,CALC_INTERVAL);
jack0325suzu 0:e4cee11fca3b 95 }
jack0325suzu 0:e4cee11fca3b 96
jack0325suzu 0:e4cee11fca3b 97 void print_scan(){
jack0325suzu 0:e4cee11fca3b 98 pc.printf("count=%f rotation=%f omega=%f\r\n",em.getPreCount(),em.getPreCount()/SOLUTION,em.getOmega());
jack0325suzu 0:e4cee11fca3b 99 //pc.printf("F=%f B=%f\r\n",(double)em.pwm_F_,(double)em.pwm_B_);
jack0325suzu 0:e4cee11fca3b 100
jack0325suzu 0:e4cee11fca3b 101 if(pc.readable()) {
jack0325suzu 0:e4cee11fca3b 102 char sel=pc.getc();
jack0325suzu 0:e4cee11fca3b 103
jack0325suzu 0:e4cee11fca3b 104 switch(sel) {
jack0325suzu 0:e4cee11fca3b 105 case 'i':
jack0325suzu 0:e4cee11fca3b 106 duty+=0.1;
jack0325suzu 0:e4cee11fca3b 107 pc.printf("duty=%f \r\n",duty);
jack0325suzu 0:e4cee11fca3b 108 break;
jack0325suzu 0:e4cee11fca3b 109 case 'o':
jack0325suzu 0:e4cee11fca3b 110 duty-=0.1;
jack0325suzu 0:e4cee11fca3b 111 pc.printf("duty=%f \r\n",duty);
jack0325suzu 0:e4cee11fca3b 112 break;
jack0325suzu 0:e4cee11fca3b 113 case 'p':
jack0325suzu 0:e4cee11fca3b 114 Kp+=0.01;
jack0325suzu 0:e4cee11fca3b 115 pc.printf("Kp=%f \r\n",Kp);
jack0325suzu 0:e4cee11fca3b 116 break;
jack0325suzu 0:e4cee11fca3b 117 }
jack0325suzu 0:e4cee11fca3b 118 }
jack0325suzu 0:e4cee11fca3b 119 }