![](/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:
- 2:c9de02d6d154
- Parent:
- 1:baab5b88a142
- Child:
- 3:6223efea43fe
--- a/main.cpp Wed Aug 23 07:50:14 2017 +0000 +++ b/main.cpp Mon Aug 28 01:39:14 2017 +0000 @@ -1,15 +1,16 @@ #include "mbed.h" #include "omuni.hpp" #include "cal_PID.hpp" +#include "timeBaseTrapezoidalMotionCal.h" const float omega_max = 1.0 * 2 * 3.14159265f; const float wrap_radius = 1.5f; const float wrap_speed = 2.0f; const int8_t armDuty[] = {-50, -50}; const signed int spearDuty = 127; -const int32_t lift_preset[4] = {620, 990, 1360, 1730}; // 12bit 0 ~ 4095 -const int32_t lift_max = 1830; -const int32_t lift_min = 500; +const int32_t lift_preset[4] = {550, 1033, 1516, 2000}; // 12bit 0 ~ 4095 +const int32_t lift_max = 1980; +const int32_t lift_min = 550; const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100 const int armAddr[] = {0x16, 0x18}; // 1100 , 0010 @@ -23,10 +24,15 @@ bool arm = false; bool spear = false; + int lift_targ = lift_preset[0]; +int lift_stepTarg; +int lift_stepOrigin; bool first_receive = true; int past_lift_gray = 0b00; int lift_inc = 0; +int lift_task = 0; +int lift_time = 0; //DigitalIn button(USER_BUTTON); DigitalOut led(LED1); @@ -39,6 +45,7 @@ DigitalIn spear_sensor(PC_3); Timer mesPeriod; cal_pid lift_pid; +timeBaseTrapezoidalMotionCal lift_motionCal(500, 500, 5000, 70000, -70000); float speed_x, speed_y, omega; bool f; @@ -165,7 +172,7 @@ if(rcTemp.byteNum == 3){ // データは揃った - led = !led; +// led = !led; ems = RCData[3].emo != 0; @@ -198,45 +205,51 @@ first_receive = false; } if(RCData[2].lift_gray != past_lift_gray){ // 段階移動 + int lift_currentTarg; int lift_diff = 0; int lift_nearest = 10000; + int lift_nextTarg; + + lift_currentTarg = lift_targ; + lift_nextTarg = lift_currentTarg; + lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray); - //p.printf("%d --> %d\n", past_lift_gray, RCData[2].lift_gray); - //p.printf("gray = %d\n", lift_diff); for(int num = 0; num < 4; num++){ // 最寄りの段階 - //p.printf("nearest = %d, num = %d\n", lift_nearest, lift_nearest_num); - if(lift_nearest > abs(lift_preset[num] - lift_targ)){ + if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){ lift_nearest_num = num; - lift_nearest = abs(lift_preset[num] - lift_targ); + lift_nearest = abs(lift_preset[num] - lift_currentTarg); } } - //p.printf("lift_targ = %d num = %d\n", lift_targ, lift_nearest_num); if(lift_diff == 1){ - if(lift_preset[lift_nearest_num] >= lift_targ){ - if(lift_nearest_num < 3)lift_targ = lift_preset[lift_nearest_num + 1]; + if(lift_preset[lift_nearest_num] >= lift_currentTarg){ + if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1]; } else{ - lift_targ = lift_preset[lift_nearest_num]; + lift_nextTarg = lift_preset[lift_nearest_num]; } } else if(lift_diff == -1){ - if(lift_preset[lift_nearest_num] <= lift_targ){ - if(lift_nearest_num > 0)lift_targ = lift_preset[lift_nearest_num - 1]; + if(lift_preset[lift_nearest_num] <= lift_currentTarg){ + if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1]; } else{ - lift_targ = lift_preset[lift_nearest_num]; + lift_nextTarg = lift_preset[lift_nearest_num]; } } + if(lift_task == 0){ + lift_stepOrigin = lift_currentTarg; + lift_stepTarg = lift_nextTarg; + lift_motionCal.setTarg(lift_stepTarg - lift_stepOrigin); + lift_time = 0; + lift_task += 1; + } } past_lift_gray = RCData[2].lift_gray; - if(RCData[2].lift_down != 0)lift_inc = -5; - else if(RCData[2].lift_up != 0)lift_inc = 5; + if(RCData[2].lift_down != 0)lift_inc = -2; + else if(RCData[2].lift_up != 0)lift_inc = 2; else lift_inc = 0; - //p.printf("inc = %d\n",lift_inc); - //p.printf("lift_targ = %d\n", lift_targ); - } // すっきりした // してない } @@ -257,12 +270,24 @@ int32_t meter = 0; signed int duty = 0; - if(lift_inc < 0){ - if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc; + if(lift_task != 0){ + if(lift_targ != lift_stepTarg){ + lift_targ = lift_motionCal.calSteps(lift_time) + lift_stepOrigin; + lift_time += 1; + } + else{ + lift_task = 0; + } } - 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; + } } + meter = lift_meter.read_u16() >> 4; duty = lift_pid.get_pid(meter, lift_targ); drive_motor(liftAddr, duty); @@ -345,7 +370,7 @@ omuni.set_pid(1, 3.0f, 0.07f, 0.05f); omuni.set_pid(2, 3.0f, 0.07f, 0.05f); - lift_pid.param(0.3, 0.0, 0.0); + lift_pid.param(0.2, 0.0, 0.0); lift_pid.period(0.001); lift_pid.output(-127, 127); @@ -357,7 +382,7 @@ while(1) { - wait(0.0001); + wait(0.001); omuni.set_speed(speed_x, speed_y, omega, f); omuni.drive(); control(); @@ -365,19 +390,6 @@ if(ems){ emergencyStop(); } - /* - if(count < 100){ - count += 1; - } - else{ - period_us = mesPeriod.read_us(); - mesPeriod.reset(); - - p.printf("%dHz\n", 1000000 * 100 / period_us); - - count = 0; - } - */ } }