![](/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:
- 6:22ad12682a5d
- Parent:
- 5:12be2ac0f395
- Child:
- 7:5cdd805ca8a3
diff -r 12be2ac0f395 -r 22ad12682a5d main.cpp --- a/main.cpp Mon Sep 11 05:37:59 2017 +0000 +++ b/main.cpp Mon Sep 18 11:51:23 2017 +0000 @@ -5,6 +5,8 @@ #include "FastPWM.h" #include "soundData.h" +#define LIFT_METER_INVERSION 1 + const int comTimeout_ms = 200; const float omuni_speed[4] = {0, 1.0, 2.0, 3.0}; const float omuni_speed_max = 2.0f; @@ -12,10 +14,11 @@ const float omega_f = 0.7 * 2 * 3.14159265f; const float wrap_radius = 1.0f; const float wrap_speed = 1.8f; -const int8_t armDuty[] = {-50, -50}; +const int8_t armDuty[] = {-100, -100}; const signed int spearDuty = 127; -const int32_t lift_preset_min = 135; -const int32_t lift_preset_max = 450; +//const int32_t lift_preset_min = 200; +const int32_t lift_preset_min = 320; // Temporary use +const int32_t lift_preset_max = 480; const int32_t lift_preset[4] = { lift_preset_min, (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min, @@ -24,8 +27,8 @@ }; // 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 = 5; -const int32_t lift_down_speed = 5; +const int32_t lift_up_speed = 2; +const int32_t lift_down_speed = 2; const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100 @@ -36,7 +39,7 @@ int comTimeout_count = 0; - +bool comTimeout_state = false; bool ems = false; @@ -54,7 +57,7 @@ int soundDataCount = 0; bool soundEnd = false; -//DigitalIn button(USER_BUTTON); +DigitalIn button(USER_BUTTON); DigitalOut led(LED1); Serial pc(USBTX, USBRX); @@ -141,21 +144,13 @@ return 0; } -int byteSum(int8_t byte){ - int sum = 0; - for(int count = 0; count < 8; count++){ - if((byte & (0x01 << count)) != 0)sum += 1; - } - return sum; -} - bool checksum_check(comBuf_t *buf){ int sum = 0; for(int count = 0; count < 9; count++){ - sum += byteSum(buf->list[count]); + sum += buf->list[count]; } - return sum == buf->checksum; + return (sum & 0b01111111) == buf->checksum; } @@ -168,7 +163,7 @@ comBuf.list[0] = temp; byteCount = 1; } - else if(byteCount & 0b10000000 != 0){ // 想定外のデータ + else if((temp & 0b10000000) != 0){ // 想定外のデータ byteCount = 0; } else if(byteCount > 0){ @@ -177,6 +172,7 @@ if(byteCount < 9)byteCount += 1; else{ // データは揃った + if(checksum_check(&comBuf)){ led = !led; comTimeout_count = 0; @@ -204,18 +200,21 @@ speed_x = omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f; speed_y = omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f; - if(abs(getJoy7bit(buf->joyRY)) < 10 && abs(getJoy7bit(buf->joyRX)) > 40){ + if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){ f = 1; omega = omega_f; - if(getJoy7bit(buf->joyRX) < 0)omega *= 1; + if(getJoy7bit(buf->joyRX) < 0)omega *= -1; } - - if(buf->L2 >= 5 && buf->R2 < 5){ - omega = omega_max * buf->L2 / 127.0f; + else if(buf->L2 >= 5 && buf->R2 < 5){ + omega = omega_max * buf->L2 * -1.0f / 127.0f; f = 0; } else if(buf->L2 < 5 && buf->R2 >= 5){ - omega = omega_max * buf->L2 * -1.0f / 127.0f; + omega = omega_max * buf->R2 * 1.0f / 127.0f; + f = 0; + } + else{ + omega = 0; f = 0; } @@ -283,7 +282,7 @@ } past_lift_gray = buf->lift_gray; - if(abs(getJoy7bit(buf->joyRY)) > 40 && abs(getJoy7bit(buf->joyRX)) < 10){ + if(abs(getJoy7bit(buf->joyRY)) > 50 && abs(getJoy7bit(buf->joyRX)) < 20){ if(getJoy7bit(buf->joyRY) > 0)lift_inc = 1; else lift_inc = -1; } @@ -319,9 +318,11 @@ arm = 0; spear = 0; lift_inc = 0; + comTimeout_state = true; } else{ comTimeout_count += 1; + comTimeout_state = false; } } @@ -366,7 +367,10 @@ } meter = lift_meter.read_u16() >> 6; - duty = lift_pid.get_pid(meter, lift_targ, -1); +#if LIFT_METER_INVERSION + meter = 1023 - meter; +#endif + duty = lift_pid.get_pid(meter, lift_targ, 1); drive_motor(liftAddr, duty); } @@ -445,6 +449,11 @@ void control(){ + drive_motor(EMO_Addr, 1); + + omuni.set_speed(speed_x, speed_y, omega, f); + omuni.drive(); + arm_control(); lift_control(); spear_control(); @@ -457,7 +466,7 @@ int count = 0; pc.baud(115200); - com.baud(9600); + com.baud(115200); pc.printf("Hello!\n"); com.attach(com_rx, Serial::RxIrq); @@ -491,11 +500,13 @@ while(1) { wait(0.001); - omuni.set_speed(speed_x, speed_y, omega, f); - omuni.drive(); - control(); - drive_motor(EMO_Addr, 1); + if(comTimeout_state == false){ + control(); + } + else{ + drive_motor(EMO_Addr, 0); + } if(ems){ emergencyStop();