Diff: MotorDriver_ver4-1.cpp

Revision:

2:50e50ee8e08a

Parent:

1:844acc585d3d

Child:

3:141dc1666df2

--- a/MotorDriver_ver4-1.cpp	Mon Jul 08 15:13:43 2019 +0000
+++ b/MotorDriver_ver4-1.cpp	Mon Jul 08 15:47:40 2019 +0000
@@ -22,7 +22,7 @@
 #include "PID.h"
 #include "QEI.h"
 
-#define Kp2 1.5f
+#define Kp2 1.3f
 #define Ki2 0.0f
 #define Kd2 0.002f
 
@@ -45,8 +45,8 @@
 #define ABLE 0
 #define DISABLE 1
 
-#define CW 0
-#define CCW 1
+#define CW 1
+#define CCW 0
 #define MOTOR_FREUENCY 8000 //[Hz]
 
 #define MOTOR_TARGET_OMEGA_THRESHOLD    0.5f   //[deg/s]
@@ -113,7 +113,7 @@
 
 
 
-void MotorDrive(int mode, int direction, double target);
+void MotorDrive(int mode, double target);
 
 //void CurrentFunction();
 
@@ -218,30 +218,21 @@
     Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD*0.001f); //Measure rotation speed every 30ms
     //NVIC_SetPriority(TIMER3_IRQn, 1);
     
-    Motor.target = -250.0f; //[deg]
+    Motor.target = 250.0f; //[deg]
     //Motor.target = 1.0f;  //[rps]
     
     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);
-        
-        MotorDrive(THETA_FEEDBACK, CCW, Motor.target);
+        //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
         
-        /*
-        if(Motor.target >= 0)
-        {
-            MotorDrive(OMEGA_FEEDBACK, CCW, Motor.target);
-        }
+        //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);
+        pc.printf("theta:%6.3f omega:%6.3f\r\n", Motor.theta, Motor.omega);
         
-        else
-        {
-            MotorDrive(OMEGA_FEEDBACK, CW, Motor.target);
-        }
-        */
+        MotorDrive(THETA_FEEDBACK, Motor.target);
+        //MotorDrive(OMEGA_FEEDBACK, Motor.target);
+        
         //pc.printf("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,motor.compute());  
         //CurrentFunction();
     }
@@ -274,6 +265,25 @@
                 pc.printf("get 'a' \r\n");
                 
                 break;
+            
+            case 'p': 
+                
+                //motor.setSetPoint(1.5f);  //PID target value
+                
+                Motor.target += 30.0f;
+                pc.printf("get 'p' \r\n");    
+                
+                break;
+            
+            case 'l':
+        
+                //motor.setSetPoint(0.5f);  //PID target value
+                
+                Motor.target -= 30.0f;
+                pc.printf("get 'l' \r\n");
+                
+                break;
+            
                 
             case 'r':
         
@@ -447,7 +457,7 @@
 }
 */
 
-void MotorDrive(int mode, int direction, double target)
+void MotorDrive(int mode, double target)
 {
     //theta feebback(mode 0)
     //omega feedback(mode 1)
@@ -460,27 +470,25 @@
     {
         case THETA_FEEDBACK:
         
-            pc.printf("THETA_FEEDBACK!! \r\n");
+            //pc.printf("THETA_FEEDBACK!! \r\n");
         
             motor2.setSetPoint(target); 
             motor2.setProcessValue(Motor.theta);
             
-            if(motor2.compute() >= 0)
+            if(target >= 0)
             {
-                
-                if(abs(Motor.omega) < 0.1f) //モータの回転が停止時
+                if(abs(Motor.omega) < 0.01f) //モータの回転が停止時
                 {
                     SR = ABLE;
-                    PHASE = (float)direction;
+                    PHASE = (float)CW;
                     PWM1 = 0.4f;
                     PWM2 = 1.0f;  
                     //MotorDrive(CURRENT_FEEDBACK, CCW, 1.2f);
                 }
-               
-                
+                  
                 else
                 {
-                    MotorDrive(OMEGA_FEEDBACK, direction, abs(motor2.compute()));
+                    MotorDrive(OMEGA_FEEDBACK, motor2.compute());
                 }
                 
                 //MotorDrive(OMEGA_FEEDBACK, CCW, abs(motor2.compute()));
@@ -489,10 +497,10 @@
             else
             {
                 
-                if(abs(Motor.omega) < 0.1f) //モータの回転が停止時
+                if(abs(Motor.omega) < 0.01f) //モータの回転が停止時
                 {
                     SR = ABLE;
-                    PHASE = (float)CW;
+                    PHASE = (float)CCW;
                     PWM1 = 0.4f;
                     PWM2 = 1.0f;  
                     //MotorDrive(CURRENT_FEEDBACK, CW, 1.2f);
@@ -500,7 +508,7 @@
                 
                 else
                 {
-                    MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
+                    MotorDrive(OMEGA_FEEDBACK, motor2.compute());
                 }
                 
                 //MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
@@ -516,9 +524,7 @@
         
         case OMEGA_FEEDBACK:
             
-            
-            pc.printf("OMEGA_FEEDBACK  ");
-        /*
+            /*
             if(direction != old_direction)  //Prevent sudden turn
             {        
                 motor.setProcessValue(0.0f);   
@@ -528,20 +534,35 @@
                     old_direction = direction;
                 }
             }
+            */
             
-            else
-            {
-          */
+            
+            //pc.printf("OMEGA_FEEDBACK  ");
+            
+            if(target >= 0.0f)
+            {  
                 motor.setSetPoint(abs(target)); 
                 motor.setProcessValue(abs(Motor.omega));
                     //ここにtargetの正負によって回転方向を帰るプログラムを書く
                 SR = ABLE;
-                PHASE = (float)direction;
+                PHASE = (float)CW;
                 PWM1 = motor.compute();
                 PWM2 = 1.0f;   
                 
-                //pc.printf("%f \r\n", (float)direction);
-                                      
+                //pc.printf("%f \r\n", (float)direction);   
+            }
+                
+            else
+            {
+                motor.setSetPoint(abs(target)); 
+                motor.setProcessValue(abs(Motor.omega));
+                    //ここにtargetの正負によって回転方向を帰るプログラムを書く
+                SR = ABLE;
+                PHASE = (float)CCW;
+                PWM1 = motor.compute();
+                PWM2 = 1.0f;       
+            }                  
+                    
             break;
         
         case CURRENT_FEEDBACK: