Diff: MotorMove.cpp

Revision:

8:f7ad1d7176ba

Child:

9:b58e7d72a91c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorMove.cpp	Sat Sep 30 16:43:51 2017 +0000
@@ -0,0 +1,130 @@
+#include    "mbed.h"
+#include    "DRV8830.h"
+#include    "MotorMove.h"
+#define SPEED_RATIO  1.0
+
+//float vol_decel_ini[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0};
+float vol_decel_ini[DECEL_SIZE] = {-0.5,0,0,0,0,0};
+float vol_accel_ini[ACCEL_SIZE] = {0,0.5,0.8,1,1,1};
+
+void MotorMove::up_motor_set(int time_counter, int speed)
+    {
+        bflag_up =1;
+        bflag_down =0;
+        start_time_count=time_counter;
+
+        for (int i=0;i<ACCEL_SIZE;i++)
+        {
+             vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
+        }                
+                
+        for (int i=0;i<DECEL_SIZE;i++)
+        {
+             vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
+        }
+    };
+
+        
+void MotorMove::down_motor_set(int time_counter, int speed)
+    {
+        bflag_up =0;
+        bflag_down=1;
+        start_time_count=time_counter;
+                
+        for (int i=0;i<ACCEL_SIZE;i++)
+        {
+             vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
+        }  
+        
+        for (int i=0;i<DECEL_SIZE;i++)
+        {
+             vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
+        }
+    };
+    
+    
+float MotorMove::ReturnMotorVol(int time_counter, int sw_flag1,int sw_flag2)
+    {
+        float speed;
+        int time = time_counter - start_time_count;
+        
+        
+        static int sw_flag1_pre=0;
+        static int sw_flag2_pre=0;
+        static int stop_time=0;
+        
+        static int up_decel_count=0;
+        static int down_decel_count=0;
+        
+        //decel timer check
+        if(bflag_up==1)
+           up_decel_count==0;
+        if(bflag_down==1)
+           down_decel_count==0;
+           
+        
+        
+        if(bflag_up)
+         {
+             if (sw_flag2==1 && sw_flag2_pre==0) //State changed 
+             {
+                stop_time=time_counter;
+             }  
+
+            if(sw_flag2==0) // move motor
+             {
+                if(time>ACCEL_SIZE)
+                   {
+                     time = ACCEL_SIZE;
+                   }
+                speed = vol_accel[time];
+             }
+             
+            else if  (sw_flag2==1)
+             { 
+              time = time_counter-stop_time;
+              
+                if(time>DECEL_SIZE)
+                   {
+                     time = DECEL_SIZE;
+                     bflag_up =0;
+                   }
+                speed = vol_decel[time];
+            }
+         }
+         
+        else if (bflag_down)
+         {
+             if (sw_flag1==1 && sw_flag1_pre==0) //State changed 
+             {
+                stop_time=time_counter;
+             }  
+
+            if(sw_flag1==0) // move motor
+             {
+                if(time>ACCEL_SIZE)
+                   {
+                     time = ACCEL_SIZE;
+                   }
+                speed = -vol_accel[time];
+             }
+             
+            else if  (sw_flag1==1)
+             { 
+              time = time_counter-stop_time;
+                if(time>DECEL_SIZE)
+                   {
+                     time = DECEL_SIZE;
+                     bflag_down =0;
+                   }
+                speed = -vol_decel[time];
+             }
+         }
+         
+        sw_flag1_pre=sw_flag1;
+        sw_flag2_pre=sw_flag2;        
+        return speed;       
+    };
+    
+  
+