![](/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:
- 3:6223efea43fe
- Parent:
- 2:c9de02d6d154
- Child:
- 4:5591d3c8a761
diff -r c9de02d6d154 -r 6223efea43fe main.cpp --- a/main.cpp Mon Aug 28 01:39:14 2017 +0000 +++ b/main.cpp Mon Aug 28 07:27:23 2017 +0000 @@ -3,14 +3,25 @@ #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 float omuni_speed[4] = {0, 1.0, 1.7, 3.0}; +const float omega_max = 0.7 * 2 * 3.14159265f; +const float wrap_radius = 1.0f; +const float wrap_speed = 1.7f; const int8_t armDuty[] = {-50, -50}; const signed int spearDuty = 127; -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 int32_t lift_preset_min = 420; +const int32_t lift_preset_max = 1800; +const int32_t lift_preset[4] = { + lift_preset_min, + (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min, + (lift_preset_max - lift_preset_min) * 2.0 / 3.0 + lift_preset_min, + lift_preset_max, + }; // 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 = 30; +const int32_t lift_down_speed = 30; + const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100 const int armAddr[] = {0x16, 0x18}; // 1100 , 0010 @@ -27,12 +38,10 @@ int lift_targ = lift_preset[0]; int lift_stepTarg; -int lift_stepOrigin; +bool lift_stepMoving = false; 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); @@ -45,7 +54,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; @@ -172,12 +181,13 @@ if(rcTemp.byteNum == 3){ // データは揃った -// led = !led; + led = !led; ems = RCData[3].emo != 0; rad = RCData[0].angle * 3.141592 / 8.0; - speed = 0.8 * RCData[0].speed; + speed = omuni_speed[RCData[0].speed]; + if(RCData[0].speed == 0)speed = 0; speed_x = speed * cos(rad) * -1; speed_y = speed * sin(rad) * -1; @@ -211,7 +221,12 @@ int lift_nextTarg; lift_currentTarg = lift_targ; - lift_nextTarg = lift_currentTarg; + if(lift_stepMoving){ + lift_nextTarg = lift_stepTarg; + } + else{ + lift_nextTarg = lift_currentTarg; + } lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray); for(int num = 0; num < 4; num++){ // 最寄りの段階 @@ -220,29 +235,29 @@ lift_nearest = abs(lift_preset[num] - lift_currentTarg); } } - if(lift_diff == 1){ - if(lift_preset[lift_nearest_num] >= lift_currentTarg){ - if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1]; - } - else{ - lift_nextTarg = lift_preset[lift_nearest_num]; - } - } - else if(lift_diff == -1){ - if(lift_preset[lift_nearest_num] <= lift_currentTarg){ - if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1]; + if(lift_diff != 0){ + if(lift_diff == 1){ + if(lift_preset[lift_nearest_num] >= lift_currentTarg){p.printf("$$"); + if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1]; + } + else{ + lift_nextTarg = lift_preset[lift_nearest_num]; + } } - else{ - lift_nextTarg = lift_preset[lift_nearest_num]; + else if(lift_diff == -1){ + if(lift_preset[lift_nearest_num] <= lift_currentTarg){ + if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1]; + } + else{ + lift_nextTarg = lift_preset[lift_nearest_num]; + } } + + lift_stepTarg = lift_nextTarg; + lift_stepMoving = true; + } - 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; @@ -269,14 +284,19 @@ void lift_control(){ int32_t meter = 0; signed int duty = 0; + int32_t error = 0; - if(lift_task != 0){ - if(lift_targ != lift_stepTarg){ - lift_targ = lift_motionCal.calSteps(lift_time) + lift_stepOrigin; - lift_time += 1; + 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_task = 0; + lift_targ = lift_stepTarg; + lift_stepMoving = false; } } else{ @@ -289,7 +309,7 @@ } meter = lift_meter.read_u16() >> 4; - duty = lift_pid.get_pid(meter, lift_targ); + duty = lift_pid.get_pid(meter, lift_targ, -1); drive_motor(liftAddr, duty); } @@ -363,14 +383,14 @@ p.printf("Hello!\n"); pc.attach(pc_rx, Serial::RxIrq); - //i2cMaster.frequency(400000); + i2cMaster.frequency(400000); omuni.set_speed(0.0f, 0.0f); omuni.set_pid(0, 3.0f, 0.07f, 0.05f); omuni.set_pid(1, 3.0f, 0.07f, 0.05f); omuni.set_pid(2, 3.0f, 0.07f, 0.05f); - lift_pid.param(0.2, 0.0, 0.0); + lift_pid.param(0.4, 0.0, 0.0); lift_pid.period(0.001); lift_pid.output(-127, 127);