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@1:548ad0825df5, 2017-11-13 (annotated)

Committer:
jack0325suzu
Date:
Mon Nov 13 05:35:16 2017 +0000
Revision:
1:548ad0825df5
Parent:
0:e4cee11fca3b
Child:
2:2aa947fc7c87
ooo

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 1:548ad0825df5 10 #define PID_VELOCITY_MODE '3'
jack0325suzu 0:e4cee11fca3b 11 #define SOLUTION 500
jack0325suzu 0:e4cee11fca3b 12
jack0325suzu 0:e4cee11fca3b 13 SpeedControl em(PA_6,PA_7,NC,SOLUTION,CALC_INTERVAL,PB_5,PB_4);
jack0325suzu 0:e4cee11fca3b 14 Serial pc(USBTX,USBRX);
jack0325suzu 0:e4cee11fca3b 15 void setup();
jack0325suzu 0:e4cee11fca3b 16 void print_scan();
jack0325suzu 0:e4cee11fca3b 17
jack0325suzu 0:e4cee11fca3b 18 int loop_kai=0;
jack0325suzu 0:e4cee11fca3b 19 char mode='0';
jack0325suzu 0:e4cee11fca3b 20 double duty=0;
jack0325suzu 0:e4cee11fca3b 21 float data[500][2];
jack0325suzu 0:e4cee11fca3b 22 int i=0;
jack0325suzu 0:e4cee11fca3b 23
jack0325suzu 1:548ad0825df5 24 bool print=false;
jack0325suzu 1:548ad0825df5 25
jack0325suzu 0:e4cee11fca3b 26 double target_rotation=0,now_rotation=0,old_rotation=0;
jack0325suzu 0:e4cee11fca3b 27 double diff=0,diff_old=0,integral=0;
jack0325suzu 0:e4cee11fca3b 28 double Kp=0,Kd=0,Ki=0;
jack0325suzu 0:e4cee11fca3b 29
jack0325suzu 1:548ad0825df5 30 double Kp_v=0,Kd_v=0;
jack0325suzu 1:548ad0825df5 31
jack0325suzu 0:e4cee11fca3b 32 Ticker omega_tick;
jack0325suzu 0:e4cee11fca3b 33 Timer timer;
jack0325suzu 0:e4cee11fca3b 34 Num num(USBTX,USBRX);
jack0325suzu 0:e4cee11fca3b 35
jack0325suzu 0:e4cee11fca3b 36 void CalOmega()
jack0325suzu 0:e4cee11fca3b 37 {
jack0325suzu 0:e4cee11fca3b 38 em.CalPreOmega();
jack0325suzu 0:e4cee11fca3b 39 if(mode==ROTATION_MODE&&i<500){
jack0325suzu 0:e4cee11fca3b 40 data[i][0]=(float)timer.read();
jack0325suzu 0:e4cee11fca3b 41 data[i][1]=(float)em.getOmega()*60/(2*pi);
jack0325suzu 0:e4cee11fca3b 42 i++;
jack0325suzu 0:e4cee11fca3b 43 } else if(mode==PID_MODE){
jack0325suzu 0:e4cee11fca3b 44 now_rotation=em.getCount()/SOLUTION;
jack0325suzu 0:e4cee11fca3b 45 }
jack0325suzu 0:e4cee11fca3b 46 }
jack0325suzu 0:e4cee11fca3b 47
jack0325suzu 0:e4cee11fca3b 48 int main(void){
jack0325suzu 0:e4cee11fca3b 49 setup();
jack0325suzu 0:e4cee11fca3b 50
jack0325suzu 0:e4cee11fca3b 51 while(1){
jack0325suzu 0:e4cee11fca3b 52 if(mode==ROTATION_MODE){
jack0325suzu 0:e4cee11fca3b 53 if(i==500){
jack0325suzu 0:e4cee11fca3b 54 for(int j=0;j<500;j++){
jack0325suzu 0:e4cee11fca3b 55 pc.printf("%f,%f\r\n",data[j][0],data[j][1]);
jack0325suzu 0:e4cee11fca3b 56 }
jack0325suzu 0:e4cee11fca3b 57 i++;
jack0325suzu 0:e4cee11fca3b 58 }
jack0325suzu 0:e4cee11fca3b 59 } else if(mode==PID_MODE){
jack0325suzu 0:e4cee11fca3b 60 diff=target_rotation-now_rotation;
jack0325suzu 0:e4cee11fca3b 61 integral+=diff;
jack0325suzu 0:e4cee11fca3b 62 duty=Kp*diff+Kd*(diff-diff_old)+Ki*integral;
jack0325suzu 0:e4cee11fca3b 63 diff_old=diff;
jack0325suzu 0:e4cee11fca3b 64 }
jack0325suzu 0:e4cee11fca3b 65 if(duty>0){
jack0325suzu 0:e4cee11fca3b 66 em.turnF(duty);
jack0325suzu 0:e4cee11fca3b 67 } else {
jack0325suzu 0:e4cee11fca3b 68 em.turnB(-1*duty);
jack0325suzu 0:e4cee11fca3b 69 }
jack0325suzu 1:548ad0825df5 70 if(loop_kai%=10000){
jack0325suzu 1:548ad0825df5 71 if(mode!=ROTATION_MODE)print_scan();
jack0325suzu 0:e4cee11fca3b 72 if(loop_kai==300000)loop_kai=0;
jack0325suzu 0:e4cee11fca3b 73 }
jack0325suzu 0:e4cee11fca3b 74 loop_kai++;
jack0325suzu 0:e4cee11fca3b 75 }
jack0325suzu 0:e4cee11fca3b 76 }
jack0325suzu 0:e4cee11fca3b 77
jack0325suzu 0:e4cee11fca3b 78 void setup(){
jack0325suzu 0:e4cee11fca3b 79 pc.printf("Mode 0 : otameshi\r\n");
jack0325suzu 1:548ad0825df5 80 pc.printf("Mode 1 : time-rpm jikkenn\r\n");
jack0325suzu 1:548ad0825df5 81 pc.printf("Mode 2 : Angle PID jikkenn\r\n");
jack0325suzu 1:548ad0825df5 82 pc.printf("Mode 3 : rad/s PID jikkenn\r\n");
jack0325suzu 0:e4cee11fca3b 83 pc.printf("Input mode : ");
jack0325suzu 0:e4cee11fca3b 84 while(1){
jack0325suzu 0:e4cee11fca3b 85 if(pc.readable()) {
jack0325suzu 0:e4cee11fca3b 86 mode=pc.getc();
jack0325suzu 0:e4cee11fca3b 87 pc.printf("%c\r\n",mode);
jack0325suzu 0:e4cee11fca3b 88 break;
jack0325suzu 0:e4cee11fca3b 89 }
jack0325suzu 0:e4cee11fca3b 90 }
jack0325suzu 0:e4cee11fca3b 91 if(mode==ROTATION_MODE){
jack0325suzu 0:e4cee11fca3b 92 pc.printf("duty= ");
jack0325suzu 0:e4cee11fca3b 93 duty=num.get_number();
jack0325suzu 0:e4cee11fca3b 94 } else if(mode==PID_MODE){
jack0325suzu 0:e4cee11fca3b 95 pc.printf("target_rotation= ");
jack0325suzu 0:e4cee11fca3b 96 target_rotation=num.get_number();
jack0325suzu 1:548ad0825df5 97 } else if(mode==PID_VELOCITY_MODE){
jack0325suzu 1:548ad0825df5 98 em.setFBcoefficients(195,-7.26,372,7.06);
jack0325suzu 1:548ad0825df5 99 em.setPDparam(0,0);
jack0325suzu 1:548ad0825df5 100 }
jack0325suzu 1:548ad0825df5 101 pc.printf("\r\n start !\r\n");
jack0325suzu 0:e4cee11fca3b 102 timer.start();
jack0325suzu 0:e4cee11fca3b 103 omega_tick.attach(CalOmega,CALC_INTERVAL);
jack0325suzu 0:e4cee11fca3b 104 }
jack0325suzu 0:e4cee11fca3b 105
jack0325suzu 0:e4cee11fca3b 106 void print_scan(){
jack0325suzu 1:548ad0825df5 107 if(print)pc.printf("count=%f rotation=%f omega=%f F=%f B=%f\r\n",em.getPreCount(),em.getPreCount()/SOLUTION,em.getOmega(),(double)em.pwm_F_,(double)em.pwm_B_);
jack0325suzu 0:e4cee11fca3b 108 //pc.printf("F=%f B=%f\r\n",(double)em.pwm_F_,(double)em.pwm_B_);
jack0325suzu 0:e4cee11fca3b 109
jack0325suzu 0:e4cee11fca3b 110 if(pc.readable()) {
jack0325suzu 0:e4cee11fca3b 111 char sel=pc.getc();
jack0325suzu 0:e4cee11fca3b 112
jack0325suzu 0:e4cee11fca3b 113 switch(sel) {
jack0325suzu 0:e4cee11fca3b 114 case 'i':
jack0325suzu 0:e4cee11fca3b 115 duty+=0.1;
jack0325suzu 0:e4cee11fca3b 116 pc.printf("duty=%f \r\n",duty);
jack0325suzu 0:e4cee11fca3b 117 break;
jack0325suzu 0:e4cee11fca3b 118 case 'o':
jack0325suzu 0:e4cee11fca3b 119 duty-=0.1;
jack0325suzu 0:e4cee11fca3b 120 pc.printf("duty=%f \r\n",duty);
jack0325suzu 0:e4cee11fca3b 121 break;
jack0325suzu 1:548ad0825df5 122 case 'n':
jack0325suzu 0:e4cee11fca3b 123 Kp+=0.01;
jack0325suzu 0:e4cee11fca3b 124 pc.printf("Kp=%f \r\n",Kp);
jack0325suzu 0:e4cee11fca3b 125 break;
jack0325suzu 1:548ad0825df5 126 case 'k':
jack0325suzu 1:548ad0825df5 127 Kp_v+=0.01;
jack0325suzu 1:548ad0825df5 128 em.setPDparam(Kp_v,0);
jack0325suzu 1:548ad0825df5 129 pc.printf("Kp_v=%f \r\n",Kp_v);
jack0325suzu 1:548ad0825df5 130 break;
jack0325suzu 1:548ad0825df5 131 case 'p':
jack0325suzu 1:548ad0825df5 132 pc.printf("change print state\r\n");
jack0325suzu 1:548ad0825df5 133 print=!print;
jack0325suzu 1:548ad0825df5 134 break;
jack0325suzu 0:e4cee11fca3b 135 }
jack0325suzu 0:e4cee11fca3b 136 }
jack0325suzu 0:e4cee11fca3b 137 }