Diff: main.cpp

Revision:

0:e4cee11fca3b

Child:

1:548ad0825df5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 13 04:16:15 2017 +0000
@@ -0,0 +1,119 @@
+#include "mbed.h"
+#include "EC.h"
+#include "number.h"
+
+#define pi 3.1415926535
+#define CALC_INTERVAL 0.01
+#define TRY_MODE '0'
+#define ROTATION_MODE '1'
+#define PID_MODE '2'
+#define SOLUTION 500
+
+SpeedControl em(PA_6,PA_7,NC,SOLUTION,CALC_INTERVAL,PB_5,PB_4);
+Serial pc(USBTX,USBRX);
+void setup();
+void print_scan();
+
+int loop_kai=0;
+char mode='0';
+double duty=0;
+float data[500][2];
+int i=0;
+
+double target_rotation=0,now_rotation=0,old_rotation=0;
+double diff=0,diff_old=0,integral=0;
+double Kp=0,Kd=0,Ki=0;
+
+Ticker omega_tick;
+Timer timer;
+Num num(USBTX,USBRX);
+
+void CalOmega()
+{
+    em.CalPreOmega();
+    if(mode==ROTATION_MODE&&i<500){
+        data[i][0]=(float)timer.read();
+        data[i][1]=(float)em.getOmega()*60/(2*pi);
+        i++;
+    } else if(mode==PID_MODE){
+        now_rotation=em.getCount()/SOLUTION;
+    }
+}
+
+int main(void){
+    setup();
+    
+    while(1){
+        if(mode==ROTATION_MODE){
+            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){
+            diff=target_rotation-now_rotation;
+            integral+=diff;
+            duty=Kp*diff+Kd*(diff-diff_old)+Ki*integral;
+            diff_old=diff;
+        }
+        if(duty>0){
+            em.turnF(duty);
+        } else {
+            em.turnB(-1*duty);
+        }
+        if(loop_kai%=100000){
+            if(mode==TRY_MODE||mode==PID_MODE)print_scan();
+            if(loop_kai==300000)loop_kai=0;
+        }
+        loop_kai++;
+    }
+}
+
+void setup(){
+    pc.printf("Mode 0 : otameshi\r\n");
+    pc.printf("Mode 1 : jikkenn1\r\n");
+    pc.printf("Mode 2 : jikkenn2\r\n");
+    pc.printf("Input mode : ");
+    while(1){
+        if(pc.readable()) {
+            mode=pc.getc();
+            pc.printf("%c\r\n",mode);
+            break;
+        }
+    }
+    if(mode==ROTATION_MODE){
+        pc.printf("duty= ");
+        duty=num.get_number();
+    } else if(mode==PID_MODE){
+        pc.printf("target_rotation= ");
+        target_rotation=num.get_number();
+    } 
+    pc.printf("start !\r\n");
+    timer.start();
+    omega_tick.attach(CalOmega,CALC_INTERVAL);
+}
+
+void print_scan(){
+    pc.printf("count=%f rotation=%f omega=%f\r\n",em.getPreCount(),em.getPreCount()/SOLUTION,em.getOmega());
+    //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.1;
+                pc.printf("duty=%f \r\n",duty);
+                break;
+            case 'o':
+                duty-=0.1;
+                pc.printf("duty=%f \r\n",duty);
+                break;
+            case 'p':
+                Kp+=0.01;
+                pc.printf("Kp=%f \r\n",Kp);
+                break;
+        }
+    }
+}
\ No newline at end of file