shundai miyawaki / Mbed 2 deprecated MotorDriver_ver4-1

モータードライバーver4.1 角度フィードバック（クーロン摩擦補償） 角速度フィードバック

Dependencies:   mbed QEI PID DBG

Diff: MotorDriver_ver4-1.cpp

Revision:
3:141dc1666df2
Parent:
2:50e50ee8e08a
Child:
4:40a764acc4ec
Child:
6:cc51f9e7ef18
--- a/MotorDriver_ver4-1.cpp	Mon Jul 08 15:47:40 2019 +0000
+++ b/MotorDriver_ver4-1.cpp	Mon Jul 08 16:44:35 2019 +0000
@@ -223,8 +223,6 @@
     
     while(1)
     { 
-        
-        
         //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
         
         //pc.printf("motor2.compute : %6.3f motor.compute : %6.3f theta : %6.3f omega[rps] :%6.3f \r\n", motor2.compute(), motor.compute(), Motor.theta, Motor.omega);
@@ -477,40 +475,48 @@
             
             if(target >= 0)
             {
-                if(abs(Motor.omega) < 0.01f) //モータの回転が停止時
+                if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止中
                 {
                     SR = ABLE;
                     PHASE = (float)CW;
-                    PWM1 = 0.4f;
-                    PWM2 = 1.0f;  
-                    //MotorDrive(CURRENT_FEEDBACK, CCW, 1.2f);
                 }
                   
-                else
+                if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上
                 {
                     MotorDrive(OMEGA_FEEDBACK, motor2.compute());
                 }
                 
+                else //誤差の範囲内ならモータのPWMを停止させる
+                {
+                    MotorDrive(OMEGA_FEEDBACK, 0.0); 
+                }
+                
                 //MotorDrive(OMEGA_FEEDBACK, CCW, abs(motor2.compute()));
             }
         
             else
             {
                 
-                if(abs(Motor.omega) < 0.01f) //モータの回転が停止時
+                if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止時
                 {
                     SR = ABLE;
                     PHASE = (float)CCW;
-                    PWM1 = 0.4f;
+                    PWM1 = 0.3f;
                     PWM2 = 1.0f;  
-                    //MotorDrive(CURRENT_FEEDBACK, CW, 1.2f);
                 }
                 
-                else
+                if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上
                 {
                     MotorDrive(OMEGA_FEEDBACK, motor2.compute());
                 }
                 
+                else //誤差の範囲内ならモータのPWMを停止させる
+                {
+                    MotorDrive(OMEGA_FEEDBACK, 0.0); 
+                    PWM1 = 0.0f;
+                    PWM2 = 1.0f; 
+                }
+                
                 //MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
             }