Diff: MotorMove.cpp

Revision:

9:b58e7d72a91c

Parent:

8:f7ad1d7176ba

Child:

10:ef379cbc0004

--- a/MotorMove.cpp	Sat Sep 30 16:43:51 2017 +0000
+++ b/MotorMove.cpp	Mon Oct 02 19:12:02 2017 +0000
@@ -5,32 +5,36 @@
 
 //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};
+//float vol_accel_ini[ACCEL_SIZE] = {0,0.2,0.8,1,1,1};
+float vol_accel_ini[ACCEL_SIZE] = {0.2,0.4,0.6,0.8,1.0,1.0};
 
-void MotorMove::up_motor_set(int time_counter, int speed)
+void MotorMove::up_motor_set(int time_counter, float speed)
     {
-        bflag_up =1;
+        bflag_up   =1;
         bflag_down =0;
         start_time_count=time_counter;
+        accel_count = 0;
+        decel_count = 0;
 
         for (int i=0;i<ACCEL_SIZE;i++)
         {
-             vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
+             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;
+             vol_decel[i] = -vol_decel_ini[i]*speed/SPEED_RATIO;
         }
     };
 
-        
-void MotorMove::down_motor_set(int time_counter, int speed)
+void MotorMove::down_motor_set(int time_counter, float speed)
     {
-        bflag_up =0;
-        bflag_down=1;
+        bflag_up   =0;
+        bflag_down =1;
         start_time_count=time_counter;
-                
+        accel_count = 0;
+        decel_count = 0;
+
         for (int i=0;i<ACCEL_SIZE;i++)
         {
              vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
@@ -43,87 +47,97 @@
     };
     
     
-float MotorMove::ReturnMotorVol(int time_counter, int sw_flag1,int sw_flag2)
+float MotorMove::ReturnMotorVol(int time_counter,int bflag_downed, int bflag_upped)
     {
         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;
+        int time;
         
-        static int up_decel_count=0;
-        static int down_decel_count=0;
+        static int flag_up_stopping=0;
+        static int flag_down_stopping=0;
         
-        //decel timer check
-        if(bflag_up==1)
-           up_decel_count==0;
-        if(bflag_down==1)
-           down_decel_count==0;
-           
-        
+        //Check flag for normal mode or brake mode
+        //accel_count MODE
+
         
-        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];
+        if (bflag_up==1 && bflag_upped ==1)
+        {
+            flag_up_stopping=1;   
+            //bflag_up==0;
+        }
+        else if (bflag_down==1 && bflag_downed ==1)
+        {
+           flag_down_stopping=1;
+           //bflag_down=0;
+        }
+                        
+        if (bflag_up==1)
+          {
+            if  (flag_up_stopping==0)
+            {
+                if (accel_count<(ACCEL_SIZE-1))
+                {
+                    speed = vol_accel[accel_count];
+                    accel_count++;
+                }
+                else
+                {
+                    speed = vol_accel[ACCEL_SIZE-1];    
+                }
+            }
+            else if(flag_up_stopping==1)
+            {
+                if  (decel_count<=(DECEL_SIZE-1))
+                {
+                    speed = vol_decel[decel_count];
+                    decel_count++;
+                }
+                else 
+                {
+                bflag_up=0;
+                speed=0;
+                flag_up_stopping=0;
+                }
             }
          }
-         
-        else if (bflag_down)
+        else if (bflag_down==1)
+          {
+            if  (flag_down_stopping==0)
+            {
+                if (accel_count<(ACCEL_SIZE-1))
+                {
+                    speed = vol_accel[accel_count];
+                    accel_count++;
+                }
+                else
+                {
+                    speed = vol_accel[ACCEL_SIZE-1];    
+                }
+            }
+            else if(flag_down_stopping==1)
+            {
+                if  (decel_count<=(DECEL_SIZE-1))
+                {
+                    speed = vol_decel[decel_count];
+                    decel_count++;
+                }
+                else 
+                {
+                bflag_down=0;
+                speed=0;
+                flag_down_stopping=0;
+                }
+            }
+         }
+         else
          {
-             if (sw_flag1==1 && sw_flag1_pre==0) //State changed 
-             {
-                stop_time=time_counter;
-             }  
+             speed=0;
+        }
+                         
 
-            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];
-             }
-         }
+        //move motor before push stopping switch 
+        return speed;      
+        
          
-        sw_flag1_pre=sw_flag1;
-        sw_flag2_pre=sw_flag2;        
-        return speed;       
     };