Revision 5:0d028b4f599c, committed 2017-11-16

Comitter:

kumapo

Date:

Thu Nov 16 05:24:47 2017 +0000

Parent:

4:9d603639d38f

Commit message:

??????? on 20171116

Changed in this revision

--- a/main.cpp	Mon Nov 13 09:34:55 2017 +0000
+++ b/main.cpp	Thu Nov 16 05:24:47 2017 +0000
@@ -4,7 +4,7 @@
 
 #define pi 3.1415926535
 #define CALC_INTERVAL 0.01      //角速度の計算間隔
-#define SOLUTION 500            //エンコーダの分解能
+#define SOLUTION 500            //エンコーダの分解能 <- resolutionやろ．．．by熊谷
 
 #define TRY_MODE '0'            //お試しモード
 #define ROTATION_MODE '1'       //立ち上がりの記録モード
@@ -15,6 +15,7 @@
 Serial pc(USBTX,USBRX); //teratermによる入出力
 void setup();       //セットアップ
 void print_scan();  //teratermによる入出力関数
+void valvecontrol();
 
 int loop_kai=0;
 char mode='0';
@@ -39,41 +40,42 @@
 void CalOmega()
 {
     em.CalPreOmega();
-    if(mode==ROTATION_MODE&&i<500){ //プログラムが走り出してから5秒間でログをとる
+    if(mode==ROTATION_MODE&&i<500) { //プログラムが走り出してから5秒間でログをとる
         data[i][0]=(float)timer.read();
         data[i][1]=(float)em.getOmega()*60/(2*pi);
         i++;
-    } else if(mode==PID_MODE){      //回転数の計算
+    } else if(mode==PID_MODE) {     //回転数の計算
         now_rotation=em.getCount()/SOLUTION;
     }
 }
 
-int main(void){
+int main(void)
+{
     setup();
     
-    while(1){
-        if(mode==ROTATION_MODE){    //5秒後にteratermにログを吐き出す
-            if(i==500){
-                for(int j=0;j<500;j++){
+    while(1) {
+        if(mode==ROTATION_MODE) {   //5秒後にteratermにログを吐き出す
+            if(i==500) {
+                for(int j=0; j<500; j++) {
                     pc.printf("%f,%f\r\n",data[j][0],data[j][1]);
                 }
                 i++;
             }
-        } else if(mode==PID_MODE){  //角度制御のためのduty比のPID制御による算出
+        } else if(mode==PID_MODE) { //角度制御のためのduty比のPID制御による算出
             diff=target_rotation-now_rotation;
             integral+=diff;
             duty=Kp*diff+Kd*(diff-diff_old)+Ki*integral;
             diff_old=diff;
         }
         //モータの駆動
-        if(duty>0){
+        if(duty>0) {
             em.turnF(duty);
         } else {
             em.turnB(-1*duty);
         }
-        
+
         //ループ10000回ごとにprintおよびscanを行う
-        if(loop_kai%=10000){
+        if(loop_kai%=10000) {
             if(mode!=ROTATION_MODE)print_scan();
             if(loop_kai==300000)loop_kai=0;
         }
@@ -81,41 +83,44 @@
     }
 }
 
-void setup(){
+void setup()
+{
     pc.printf("\r\nMode 0 : otameshi\r\n");
     pc.printf("Mode 1 : time-rpm jikkenn\r\n");
     pc.printf("Mode 2 : Angle PID jikkenn\r\n");
     pc.printf("Mode 3 : rad/s PID jikkenn\r\n");
     pc.printf("Input mode : ");
-    while(1){
+    while(1) {
         if(pc.readable()) {
             mode=pc.getc();
             pc.printf("%c\r\n",mode);
             break;
         }
     }
-    if(mode==ROTATION_MODE){
+    if(mode==ROTATION_MODE) {
         pc.printf("duty= ");
         duty=num.get_number();
-    } else if(mode==PID_MODE){
+    } else if(mode==PID_MODE) {
         pc.printf("target_rotation= ");
         target_rotation=num.get_number();
-    } else if(mode==PID_VELOCITY_MODE){
+    } else if(mode==PID_VELOCITY_MODE) {
         /*em.setFBcoefficients(195,-7.26,372,7.06);
         em.setPDparam(0,0);*/
     }
     pc.printf("\r\n start !\r\n");
     timer.start();
+    
     omega_tick.attach(CalOmega,CALC_INTERVAL);  //角速度計算関数の繰り返しタイマー割り込みの宣言
 }
 
-void print_scan(){
+void print_scan()
+{
     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_);
     //pc.printf("F=%f B=%f\r\n",(double)em.pwm_F_,(double)em.pwm_B_);
-    
+
     if(pc.readable()) {
         char sel=pc.getc();
-        
+
         switch(sel) {
             case 'i':   //duty比を0.01増やす
                 duty+=0.1;
@@ -144,4 +149,9 @@
                 break;
         }
     }
+}
+
+void valvecontrol(){
+    DigitalOut valve (D10);
+    valve=1;    //open
 }
\ No newline at end of file