Nicolas Borla
/
PES2_mbed_os
PES2_mbed_os_6
Revision 3:a292bdaf03f6, committed 2021-03-16
- Comitter:
- boro
- Date:
- Tue Mar 16 17:28:04 2021 +0100
- Parent:
- 2:87ca9b0c9992
- Commit message:
- controller updated
Changed in this revision
--- a/Controller.cpp Sat Mar 13 11:06:23 2021 +0100 +++ b/Controller.cpp Tue Mar 16 17:28:04 2021 +0100 @@ -6,7 +6,7 @@ const float Controller::COUNTS_PER_TURN = 1200.0f; // encoder resolution const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s] const float Controller::KN = 40.0f; // speed constant in [rpm/V] -const float Controller::KP = 0.2f; // speed control parameter +const float Controller::KP = 0.15f; // speed control parameter const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V] const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle @@ -14,7 +14,7 @@ Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), - counterLeft(counterLeft), counterRight(counterRight) { + counterLeft(counterLeft), counterRight(counterRight), thread(osPriorityHigh, STACK_SIZE) { // Initialisieren der PWM Ausgaenge
--- a/Controller.h Sat Mar 13 11:06:23 2021 +0100 +++ b/Controller.h Tue Mar 16 17:28:04 2021 +0100 @@ -22,6 +22,7 @@ private: + static const uint32_t STACK_SIZE = 4096; // stack size of thread, given in [bytes] static const float PERIOD; static const float COUNTS_PER_TURN; static const float LOWPASS_FILTER_FREQUENCY;
--- a/main.cpp Sat Mar 13 11:06:23 2021 +0100 +++ b/main.cpp Tue Mar 16 17:28:04 2021 +0100 @@ -12,10 +12,6 @@ #include "Controller.h" -// Blinking rate in milliseconds -#define BLINKING_RATE_MS 500 - - int main() { @@ -32,7 +28,7 @@ EncoderCounter counter3(PA_0, PA_1); // create controller - Controller controller(pwm_motor1, pwm_motor2, counter1, counter1); + Controller controller(pwm_motor1, pwm_motor2, counter1, counter2); DigitalOut enable(PB_15); @@ -49,12 +45,12 @@ DigitalOut myled(LED1); - + /* // initialise PWM pwm_motor1.period(0.00005f);// 0.05ms 20KHz pwm_motor1.write(0.5f); pwm_motor2.period(0.00005f);// 0.05ms 20KHz - pwm_motor2.write(0.5f); + pwm_motor2.write(0.5f);*/ pwm_motor3.period(0.00005f);// 0.05ms 20KHz pwm_motor3.write(0.5f); @@ -85,7 +81,7 @@ // enable driver DC motors enable = 1; - + while (true) { if(!user_button) { @@ -93,11 +89,12 @@ myled = 0; controller.setDesiredSpeedLeft(50.0f); controller.setDesiredSpeedRight(50.0f); - pwm_motor3.write(0.9f); + pwm_motor3.write(0.7f); + - S0.SetPosition(1200); - S1.SetPosition(1200); - S2.SetPosition(1200); + S0.SetPosition(1000); + S1.SetPosition(1000); + S2.SetPosition(1000); } else { // LED on, reset controller speed, pwm2, position servo @@ -106,14 +103,16 @@ controller.setDesiredSpeedRight(0.0f); pwm_motor3.write(0.5f); - S0.SetPosition(1900); - S1.SetPosition(1900); - S2.SetPosition(1900); + S0.SetPosition(1500); + S1.SetPosition(1500); + S2.SetPosition(1500); } + - printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); + printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight()); + //printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); - thread_sleep_for(BLINKING_RATE_MS); + thread_sleep_for(200); } }