![](/media/cache/profiles/1129cb8ba4665189d5a9bd861a2e28b9.jpg.50x50_q85.jpg)
GifuRobo B term
Dependencies: FastPWM cal_PID mbed omuni
Fork of omuni_speed_pid by
Diff: main.cpp
- Revision:
- 7:5cdd805ca8a3
- Parent:
- 6:22ad12682a5d
- Child:
- 8:956a76b98da0
diff -r 22ad12682a5d -r 5cdd805ca8a3 main.cpp --- 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;