Diff: main.cpp

Revision:

7:5cdd805ca8a3

Parent:

6:22ad12682a5d

Child:

8:956a76b98da0

--- a/main.cpp	Mon Sep 18 11:51:23 2017 +0000
+++ b/main.cpp	Tue Sep 19 02:39:07 2017 +0000
@@ -9,7 +9,8 @@
 
 const int comTimeout_ms = 200;
 const float omuni_speed[4] = {0, 1.0, 2.0, 3.0};
-const float omuni_speed_max = 2.0f; 
+const float omuni_speed_max = 2.0f;
+const float omuni_burst_coeff = 1.5f;
 const float omega_max = 0.7 * 2 * 3.14159265f;
 const float omega_f = 0.7 * 2 * 3.14159265f;
 const float wrap_radius = 1.0f;
@@ -27,8 +28,9 @@
     }; // 12bit 0 ~ 4095
 const int32_t lift_max = lift_preset_max;
 const int32_t lift_min = lift_preset_min;
-const int32_t lift_up_speed = 2;
-const int32_t lift_down_speed = 2;
+const int32_t lift_up_speed = 10;
+const int32_t lift_down_speed = 10;
+const int32_t lift_prescaler = 10;
 
 
 const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
@@ -39,7 +41,7 @@
 
 int comTimeout_count = 0;
 
-bool comTimeout_state = false;
+bool comTimeout_state = true;
 
 bool ems = false;
 
@@ -176,6 +178,7 @@
             if(checksum_check(&comBuf)){
                 led = !led;
                 comTimeout_count = 0;
+                comTimeout_state = false;
                 received_processing(&comBuf);
             }
             else{
@@ -200,11 +203,23 @@
     speed_x = omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
     speed_y = omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
     
+    if(buf->pushL == 1){
+        speed_x *= omuni_burst_coeff;
+        speed_y *= omuni_burst_coeff;
+    }
+    
     if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){
         f = 1;
         omega = omega_f;
         if(getJoy7bit(buf->joyRX) < 0)omega *= -1;
     }
+    else{
+        int diff = (int)buf->R2 - (int)buf->L2;
+        omega = omega_max * diff / 127.0f;
+        f = 0; 
+    }
+    
+    /*
     else if(buf->L2 >= 5 && buf->R2 <  5){
         omega = omega_max * buf->L2 * -1.0f / 127.0f;
         f = 0;
@@ -217,6 +232,7 @@
         omega = 0;
         f = 0;
     }
+    */
     
     if(buf->R1 || buf->L1){
         speed_x = wrap_speed;
@@ -258,7 +274,7 @@
         }
         if(lift_diff != 0){
             if(lift_diff == 1){
-                if(lift_preset[lift_nearest_num] >= lift_currentTarg){
+                if(lift_preset[lift_nearest_num] <= lift_currentTarg){
                     if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1];
                 }
                 else{
@@ -266,7 +282,7 @@
                 }
             }
             else if(lift_diff == -1){
-                if(lift_preset[lift_nearest_num] <= lift_currentTarg){
+                if(lift_preset[lift_nearest_num] >= lift_currentTarg){
                     if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1];
                 }
                 else{
@@ -282,9 +298,10 @@
     }
     past_lift_gray = buf->lift_gray;
     
+    
     if(abs(getJoy7bit(buf->joyRY)) > 50 && abs(getJoy7bit(buf->joyRX)) < 20){
-        if(getJoy7bit(buf->joyRY) > 0)lift_inc = 1;
-        else lift_inc = -1;
+        if(getJoy7bit(buf->joyRY) > 0)lift_inc = -5;
+        else lift_inc = 5;
     }
     else{
         lift_inc = 0;
@@ -322,7 +339,6 @@
     }
     else{
         comTimeout_count += 1;
-        comTimeout_state = false;
     }
     
 }
@@ -340,30 +356,38 @@
 }
 
 void lift_control(){
+    static int prescaler_count = 0;
     int32_t meter = 0;
     signed int duty = 0;
     int32_t error = 0;
     
-    error = lift_stepTarg - lift_targ;
-    if(lift_stepMoving){
-        if(error > lift_up_speed){
-            lift_targ += lift_up_speed; 
-        }
-        else if(error < lift_down_speed * -1){
-            lift_targ -= lift_down_speed;
+    if(prescaler_count >= lift_prescaler){
+        prescaler_count = 0;
+        
+        error = lift_stepTarg - lift_targ;
+        if(lift_stepMoving){
+            if(error > lift_up_speed){
+                lift_targ += lift_up_speed; 
+            }
+            else if(error < lift_down_speed * -1){
+                lift_targ -= lift_down_speed;
+            }
+            else{
+                lift_targ = lift_stepTarg;
+                lift_stepMoving = false;
+            }
         }
         else{
-            lift_targ = lift_stepTarg;
-            lift_stepMoving = false;
+            if(lift_inc < 0){
+                if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc;
+            }
+            else if(lift_inc > 0){
+                if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc;
+            }
         }
     }
     else{
-        if(lift_inc < 0){
-            if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc;
-        }
-        else if(lift_inc > 0){
-            if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc;
-        }
+        prescaler_count += 1;
     }
     
     meter = lift_meter.read_u16() >> 6;