Diff: main.cpp

Revision:

0:679aca73f9c0

Child:

1:64229388d55c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 08 06:31:54 2021 +0000
@@ -0,0 +1,231 @@
+#include "mbed.h"
+#include "EC.h"
+#define RESOLUTION 500
+#include "CalPID.h"
+#include "MotorController.h"
+#define DELTA_T 0.001
+
+#define TIME_TURNING 0.8
+#define DUTY_MAX 0.8
+#define OMEGA_MAX 5
+
+////////////////////////////////////////
+#define THROW_SPEED 10.0
+
+///////////////////////////////////////
+#define THROW_FIN 50
+//#define THROW_FIN 35
+#define BOTTOM_ANGLE 0.5
+#define STOP_ANGLE 0.1
+
+
+DigitalIn toggle(p6,PullUp);
+
+Timer timer;
+Timer timer_loop;
+Ticker ticker;
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+//CalPID speed_pid(0.9281,0,0.0002486,DELTA_T,DUTY_MAX);
+//CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);
+//CalPID angle_omega_pid(6.726,0,0.0007063,DELTA_T,OMEGA_MAX);
+
+CalPID speed_pid(0.9281,0,0.0003086,DELTA_T,DUTY_MAX);
+CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);
+CalPID angle_omega_pid(6.726,0,0.0007063,DELTA_T,OMEGA_MAX);
+
+
+Ec1multi ec(p17,p18,RESOLUTION);  //2逓倍用class
+MotorController motor(p22,p21,DELTA_T,ec,speed_pid,angle_duty_pid,angle_omega_pid);
+
+
+double convertRad(double degree)
+{
+    return degree*M_PI/180.0;
+}
+
+float data_saved[2500]= {};
+int data_count=0;
+void saveData()
+{
+    if(data_count<150) {
+        data_saved[data_count]=ec.getDeg();
+        data_count++;
+    }
+}
+float target_rad=0;
+float target_speed=0;
+float omega_saved[2500]= {};
+int omega_count=0;
+void saveOmega()
+{
+    if(omega_count<150) {
+        omega_saved[omega_count]=ec.getOmega();
+        omega_count++;
+    }
+}
+void speedControll()
+{
+    motor.Sc(target_speed);
+//    saveData();
+//    saveOmega();
+}
+void angleControll()
+{
+    motor.AcOmega(target_rad);
+//    saveData();
+//    saveOmega();
+}
+
+void displayData()
+{
+    for(int i=0; i<omega_count; i++) {
+        printf("%f\t%f\t%f\r\n",i*DELTA_T,data_saved[i],omega_saved[i]);
+        wait(0.01);
+    }
+    omega_count=0;
+    data_count=0;
+}
+
+double initial_duty=0;
+void inputDuty()
+{
+    motor.turn(initial_duty);
+    ec.calOmega();
+    saveData();
+    saveOmega();
+}
+void initialThrow() //バックラッシュ対策
+{
+    double dead_time=0;
+//    if(target_speed<9) {
+//        initial_duty=0.4;
+//        motor.turn(initial_duty);
+//        dead_time=0.020;
+//    } else if(target_speed<13) {
+//        initial_duty=0.30;
+//        motor.turn(initial_duty);
+//        dead_time=0.0200;
+//    } else if(target_speed<18) {
+//        initial_duty=0.30;
+//        motor.turn(initial_duty);
+////        dead_time=0.024;
+//        dead_time=0.020;
+//    }  else if(target_speed<20) {
+//        initial_duty=0.63;
+//        motor.turn(initial_duty);
+//        dead_time=0.020;
+//    } else if(target_speed<21) {
+//        initial_duty=0.63;
+//        motor.turn(initial_duty);
+//        dead_time=0.031;
+//    } else if(target_speed<23) {
+//        initial_duty=0.64;
+//        motor.turn(initial_duty);
+//        dead_time=0.030;
+//    } else if(target_speed<25) {
+//        initial_duty=0.70;
+//        motor.turn(initial_duty);
+//        dead_time=0.030;
+//    } else if(target_speed<27) {
+//        initial_duty=0.75;
+//        motor.turn(initial_duty);
+//        dead_time=0.030;
+//    } else if(target_speed<29) {
+//        initial_duty=0.87;
+//        motor.turn(initial_duty);
+//        dead_time=0.030;
+//    }
+
+    initial_duty=0.05;
+    dead_time=0.30;
+    ticker.attach(&inputDuty,DELTA_T);
+    wait(dead_time);
+    ticker.detach();
+
+}
+int main ()
+{
+
+    NVIC_SetPriority(TIMER3_IRQn, 5);
+/////////////////////////////////////////////////////////////////////////////
+
+    motor.setEquation(0.0226,0.0039,-0.0226,0.0044);//MAXON_17.2
+
+//    while(1) {
+//        motor.turn(0.1);
+//        printf("%d\r\n",ec.getCount());
+//        wait(0.5);
+//    }
+//////////////////////////////////////////////////////////////////////////////
+
+    int state=0;
+
+    //printf("READY!!\r\n");
+    led2=1;
+
+    while(1) {
+        if(state==0) {
+            if(toggle) {
+                state++;
+                motor.reset();
+                target_speed=THROW_SPEED;
+
+                initialThrow();
+                ticker.attach(&speedControll,DELTA_T);
+            } else {
+                led1=!led1;
+                wait(0.5);
+            }
+        } else if(state==1) {
+            if(ec.getDeg()>THROW_FIN) {
+                ticker.detach();
+                target_rad=convertRad(THROW_FIN);
+                ticker.attach(&angleControll,DELTA_T);
+                state++;
+                timer.reset();
+                timer.start();
+            }
+        }  else if(state==2) {
+            if(fabs(ec.getDeg()-THROW_FIN)>2.5) {
+                timer.reset();
+            } else if(timer.read()>0.15) {
+                ticker.detach();
+                target_speed=0;
+                ticker.attach(&speedControll,DELTA_T);
+                state++;
+            }
+        } else if(state==3) {
+            if(fabs(ec.getOmega())>1) {
+                timer.reset();
+            } else if(timer.read()>0.05) {
+                ticker.detach();
+                target_rad=convertRad(BOTTOM_ANGLE);
+                ticker.attach(&angleControll,DELTA_T);
+                state++;
+            }
+        } else if(state==4) {
+            if(fabs(ec.getDeg()-BOTTOM_ANGLE)>1) {
+                timer.reset();
+            } else if(timer.read()>0.05) {
+                ticker.detach();
+                state++;
+
+            }
+        } else if(state==5) {
+            state++;
+            motor.stop();
+
+        } else if(state==6) {
+            displayData();
+            state++;
+        } else if(state==7) {
+//            state=0;
+            state++;
+            wait(2);
+        }
+        wait_us(10);
+
+    }
+}