jiei suzuki / Mbed 2 deprecated denku_mock_1

電空班モック用プログラム

Dependencies:   EC def_number mbed

main.cpp@4:9d603639d38f, 2017-11-13 (annotated)

Committer:
jack0325suzu
Date:
Mon Nov 13 09:34:55 2017 +0000
Revision:
4:9d603639d38f
Parent:
3:3d240dc22c69 
?

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 3:3d240dc22c69 6 #define CALC_INTERVAL 0.01 //角速度の計算間隔
jack0325suzu 3:3d240dc22c69 7 #define SOLUTION 500 //エンコーダの分解能
jack0325suzu 0:e4cee11fca3b 8
jack0325suzu 3:3d240dc22c69 9 #define TRY_MODE '0' //お試しモード
jack0325suzu 3:3d240dc22c69 10 #define ROTATION_MODE '1' //立ち上がりの記録モード
jack0325suzu 3:3d240dc22c69 11 #define PID_MODE '2' //角度制御モード
jack0325suzu 3:3d240dc22c69 12 #define PID_VELOCITY_MODE '3' //角速度制御モード
jack0325suzu 3:3d240dc22c69 13
jack0325suzu 3:3d240dc22c69 14 SpeedControl em(PA_6,PA_7,NC,SOLUTION,CALC_INTERVAL,PB_5,PB_4); //エンコーダとモータはclass SpeedControllerにより制御する
jack0325suzu 3:3d240dc22c69 15 Serial pc(USBTX,USBRX); //teratermによる入出力
jack0325suzu 3:3d240dc22c69 16 void setup(); //セットアップ
jack0325suzu 3:3d240dc22c69 17 void print_scan(); //teratermによる入出力関数
jack0325suzu 0:e4cee11fca3b 18
jack0325suzu 0:e4cee11fca3b 19 int loop_kai=0;
jack0325suzu 0:e4cee11fca3b 20 char mode='0';
jack0325suzu 0:e4cee11fca3b 21 double duty=0;
jack0325suzu 0:e4cee11fca3b 22 float data[500][2];
jack0325suzu 0:e4cee11fca3b 23 int i=0;
jack0325suzu 0:e4cee11fca3b 24
jack0325suzu 1:548ad0825df5 25 bool print=false;
jack0325suzu 1:548ad0825df5 26
jack0325suzu 0:e4cee11fca3b 27 double target_rotation=0,now_rotation=0,old_rotation=0;
jack0325suzu 0:e4cee11fca3b 28 double diff=0,diff_old=0,integral=0;
jack0325suzu 0:e4cee11fca3b 29 double Kp=0,Kd=0,Ki=0;
jack0325suzu 0:e4cee11fca3b 30
jack0325suzu 1:548ad0825df5 31 double Kp_v=0,Kd_v=0;
jack0325suzu 1:548ad0825df5 32
jack0325suzu 0:e4cee11fca3b 33 Ticker omega_tick;
jack0325suzu 0:e4cee11fca3b 34 Timer timer;
jack0325suzu 0:e4cee11fca3b 35 Num num(USBTX,USBRX);
jack0325suzu 0:e4cee11fca3b 36
jack0325suzu 3:3d240dc22c69 37 //角速度計算関数
jack0325suzu 3:3d240dc22c69 38 //一定時間ごとに割り込む
jack0325suzu 0:e4cee11fca3b 39 void CalOmega()
jack0325suzu 0:e4cee11fca3b 40 {
jack0325suzu 0:e4cee11fca3b 41 em.CalPreOmega();
jack0325suzu 3:3d240dc22c69 42 if(mode==ROTATION_MODE&&i<500){ //プログラムが走り出してから5秒間でログをとる
jack0325suzu 0:e4cee11fca3b 43 data[i][0]=(float)timer.read();
jack0325suzu 0:e4cee11fca3b 44 data[i][1]=(float)em.getOmega()*60/(2*pi);
jack0325suzu 0:e4cee11fca3b 45 i++;
jack0325suzu 3:3d240dc22c69 46 } else if(mode==PID_MODE){ //回転数の計算
jack0325suzu 0:e4cee11fca3b 47 now_rotation=em.getCount()/SOLUTION;
jack0325suzu 0:e4cee11fca3b 48 }
jack0325suzu 0:e4cee11fca3b 49 }
jack0325suzu 0:e4cee11fca3b 50
jack0325suzu 0:e4cee11fca3b 51 int main(void){
jack0325suzu 0:e4cee11fca3b 52 setup();
jack0325suzu 0:e4cee11fca3b 53
jack0325suzu 0:e4cee11fca3b 54 while(1){
jack0325suzu 3:3d240dc22c69 55 if(mode==ROTATION_MODE){ //5秒後にteratermにログを吐き出す
jack0325suzu 0:e4cee11fca3b 56 if(i==500){
jack0325suzu 0:e4cee11fca3b 57 for(int j=0;j<500;j++){
jack0325suzu 0:e4cee11fca3b 58 pc.printf("%f,%f\r\n",data[j][0],data[j][1]);
jack0325suzu 0:e4cee11fca3b 59 }
jack0325suzu 0:e4cee11fca3b 60 i++;
jack0325suzu 0:e4cee11fca3b 61 }
jack0325suzu 3:3d240dc22c69 62 } else if(mode==PID_MODE){ //角度制御のためのduty比のPID制御による算出
jack0325suzu 0:e4cee11fca3b 63 diff=target_rotation-now_rotation;
jack0325suzu 0:e4cee11fca3b 64 integral+=diff;
jack0325suzu 0:e4cee11fca3b 65 duty=Kp*diff+Kd*(diff-diff_old)+Ki*integral;
jack0325suzu 0:e4cee11fca3b 66 diff_old=diff;
jack0325suzu 0:e4cee11fca3b 67 }
jack0325suzu 3:3d240dc22c69 68 //モータの駆動
jack0325suzu 0:e4cee11fca3b 69 if(duty>0){
jack0325suzu 0:e4cee11fca3b 70 em.turnF(duty);
jack0325suzu 0:e4cee11fca3b 71 } else {
jack0325suzu 0:e4cee11fca3b 72 em.turnB(-1*duty);
jack0325suzu 0:e4cee11fca3b 73 }
jack0325suzu 3:3d240dc22c69 74
jack0325suzu 3:3d240dc22c69 75 //ループ10000回ごとにprintおよびscanを行う
jack0325suzu 1:548ad0825df5 76 if(loop_kai%=10000){
jack0325suzu 1:548ad0825df5 77 if(mode!=ROTATION_MODE)print_scan();
jack0325suzu 0:e4cee11fca3b 78 if(loop_kai==300000)loop_kai=0;
jack0325suzu 0:e4cee11fca3b 79 }
jack0325suzu 0:e4cee11fca3b 80 loop_kai++;
jack0325suzu 0:e4cee11fca3b 81 }
jack0325suzu 0:e4cee11fca3b 82 }
jack0325suzu 0:e4cee11fca3b 83
jack0325suzu 0:e4cee11fca3b 84 void setup(){
jack0325suzu 2:2aa947fc7c87 85 pc.printf("\r\nMode 0 : otameshi\r\n");
jack0325suzu 1:548ad0825df5 86 pc.printf("Mode 1 : time-rpm jikkenn\r\n");
jack0325suzu 1:548ad0825df5 87 pc.printf("Mode 2 : Angle PID jikkenn\r\n");
jack0325suzu 1:548ad0825df5 88 pc.printf("Mode 3 : rad/s PID jikkenn\r\n");
jack0325suzu 0:e4cee11fca3b 89 pc.printf("Input mode : ");
jack0325suzu 0:e4cee11fca3b 90 while(1){
jack0325suzu 0:e4cee11fca3b 91 if(pc.readable()) {
jack0325suzu 0:e4cee11fca3b 92 mode=pc.getc();
jack0325suzu 0:e4cee11fca3b 93 pc.printf("%c\r\n",mode);
jack0325suzu 0:e4cee11fca3b 94 break;
jack0325suzu 0:e4cee11fca3b 95 }
jack0325suzu 0:e4cee11fca3b 96 }
jack0325suzu 0:e4cee11fca3b 97 if(mode==ROTATION_MODE){
jack0325suzu 0:e4cee11fca3b 98 pc.printf("duty= ");
jack0325suzu 0:e4cee11fca3b 99 duty=num.get_number();
jack0325suzu 0:e4cee11fca3b 100 } else if(mode==PID_MODE){
jack0325suzu 0:e4cee11fca3b 101 pc.printf("target_rotation= ");
jack0325suzu 0:e4cee11fca3b 102 target_rotation=num.get_number();
jack0325suzu 1:548ad0825df5 103 } else if(mode==PID_VELOCITY_MODE){
jack0325suzu 3:3d240dc22c69 104 /*em.setFBcoefficients(195,-7.26,372,7.06);
jack0325suzu 3:3d240dc22c69 105 em.setPDparam(0,0);*/
jack0325suzu 1:548ad0825df5 106 }
jack0325suzu 1:548ad0825df5 107 pc.printf("\r\n start !\r\n");
jack0325suzu 0:e4cee11fca3b 108 timer.start();
jack0325suzu 3:3d240dc22c69 109 omega_tick.attach(CalOmega,CALC_INTERVAL); //角速度計算関数の繰り返しタイマー割り込みの宣言
jack0325suzu 0:e4cee11fca3b 110 }
jack0325suzu 0:e4cee11fca3b 111
jack0325suzu 0:e4cee11fca3b 112 void print_scan(){
jack0325suzu 2:2aa947fc7c87 113 if(print)pc.printf("time=%f count=%f rotation=%f rpm=%f F=%f B=%f\r\n",timer.read(),em.getPreCount(),em.getPreCount()/SOLUTION,em.getOmega()*60/(2*pi),(double)em.pwm_F_,(double)em.pwm_B_);
jack0325suzu 0:e4cee11fca3b 114 //pc.printf("F=%f B=%f\r\n",(double)em.pwm_F_,(double)em.pwm_B_);
jack0325suzu 0:e4cee11fca3b 115
jack0325suzu 0:e4cee11fca3b 116 if(pc.readable()) {
jack0325suzu 0:e4cee11fca3b 117 char sel=pc.getc();
jack0325suzu 0:e4cee11fca3b 118
jack0325suzu 0:e4cee11fca3b 119 switch(sel) {
jack0325suzu 3:3d240dc22c69 120 case 'i': //duty比を0.01増やす
jack0325suzu 0:e4cee11fca3b 121 duty+=0.1;
jack0325suzu 0:e4cee11fca3b 122 pc.printf("duty=%f \r\n",duty);
jack0325suzu 0:e4cee11fca3b 123 break;
jack0325suzu 3:3d240dc22c69 124 case 'o': //duty比を0.01減らす
jack0325suzu 0:e4cee11fca3b 125 duty-=0.1;
jack0325suzu 0:e4cee11fca3b 126 pc.printf("duty=%f \r\n",duty);
jack0325suzu 0:e4cee11fca3b 127 break;
jack0325suzu 1:548ad0825df5 128 case 'n':
jack0325suzu 0:e4cee11fca3b 129 Kp+=0.01;
jack0325suzu 0:e4cee11fca3b 130 pc.printf("Kp=%f \r\n",Kp);
jack0325suzu 0:e4cee11fca3b 131 break;
jack0325suzu 1:548ad0825df5 132 case 'k':
jack0325suzu 1:548ad0825df5 133 Kp_v+=0.01;
jack0325suzu 1:548ad0825df5 134 em.setPDparam(Kp_v,0);
jack0325suzu 1:548ad0825df5 135 pc.printf("Kp_v=%f \r\n",Kp_v);
jack0325suzu 1:548ad0825df5 136 break;
jack0325suzu 3:3d240dc22c69 137 case 's': //モータの回転を止める
jack0325suzu 2:2aa947fc7c87 138 pc.printf("stop motor\r\n");
jack0325suzu 2:2aa947fc7c87 139 duty=0;
jack0325suzu 2:2aa947fc7c87 140 break;
jack0325suzu 3:3d240dc22c69 141 case 'p': //teraterm上にprintするかしないか
jack0325suzu 1:548ad0825df5 142 pc.printf("change print state\r\n");
jack0325suzu 1:548ad0825df5 143 print=!print;
jack0325suzu 1:548ad0825df5 144 break;
jack0325suzu 0:e4cee11fca3b 145 }
jack0325suzu 0:e4cee11fca3b 146 }
jack0325suzu 0:e4cee11fca3b 147 }