Control an H-Bridge using a PwmOut (enable) and two DigitalOuts (direction select)
Fork of Motor by
Revision 4:5370aae8ee3b, committed 2015-05-07
- Comitter:
- miczyg
- Date:
- Thu May 07 09:47:58 2015 +0000
- Parent:
- 3:9572ea41a1b4
- Child:
- 5:71d7e9c17980
- Commit message:
- asd
Changed in this revision
Motor.cpp | Show annotated file Show diff for this revision Revisions of this file |
Motor.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/Motor.cpp Thu May 07 09:45:03 2015 +0000 +++ b/Motor.cpp Thu May 07 09:47:58 2015 +0000 @@ -1,31 +1,32 @@ /* mbed simple H-bridge motor controller - * Copyright (c) 2007-2010, sford, http://mbed.org - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ +* Copyright (c) 2007-2010, sford, http://mbed.org +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in +* all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +* THE SOFTWARE. +*/ #include "Motor.h" - #include "mbed.h" -Motor::Motor(PinName pwm, PinName fwd, PinName rev): - _pwm(pwm), _fwd(fwd), _rev(rev) +#define AUTO 1 + +Motor::Motor(PinName pwm, PinName fwd, PinName rev) : +_pwm(pwm), _fwd(fwd), _rev(rev), speedControl(K, Ti, Td, itv), encoder(CH_A, CH_B, NC, pPerRev) { // Set initial condition of PWM @@ -35,20 +36,31 @@ // Initial condition of output enables _fwd = 0; _rev = 0; + + //PID config + speedControl.setInputLimits(0, 1); + speedControl.setOutputLimits(0, 1); + speedControl.setMode(AUTO); + } -void Motor::speed(float speed) -{ - /*if(speed>0) { - _fwd = 1; - _rev = 0; +void Motor::speed(float speed) { + + if (speed != set_speed){ + speedControl.setSetPoint(speed); + set_speed = speed; } - if(speed<0) { - _fwd = 0; - _rev = 1; - }*/ - _fwd = 0; - _rev = 1; + + pulses = encoder.getPulses(); + curr_speed = (pulses-prev_pulses) / max_pulse_rate; + prev_pulses = pulses; + + speedControl.setProcessValue(curr_speed); + speed = speedControl.compute(); + + + _fwd = (speed > 0.0); + _rev = (speed < 0.0); _pwm = abs(speed); }
--- a/Motor.h Thu May 07 09:45:03 2015 +0000 +++ b/Motor.h Thu May 07 09:47:58 2015 +0000 @@ -1,60 +1,77 @@ /* mbed simple H-bridge motor controller - * Copyright (c) 2007-2010, sford, http://mbed.org - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ +* Copyright (c) 2007-2010, sford, http://mbed.org +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in +* all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +* THE SOFTWARE. +*/ #ifndef MBED_MOTOR_H #define MBED_MOTOR_H #include "mbed.h" #include "PID.h" +#include "QEI.h" -/** Interface to control a standard DC motor - * - * with an H-bridge using a PwmOut and 2 DigitalOuts - */ +/*nastawy regulatora PID*/ +const float K = 1; +const float Ti = 0; +const float Td = 0; +const float itv = 0.001; + +/*QEI pins*/ +#define CH_A PTA2 +#define CH_B PTA3 +const int pPerRev = 10; +const int max_pulse_rate; //do policzenia!!!!!!!!!!!!!!!!!!! + +/** Interface to control a standard DC motor +* +* with an H-bridge using a PwmOut and 2 DigitalOuts +*/ class Motor { public: - /** Create a motor control interface - * - * @param pwm A PwmOut pin, driving the H-bridge enable line to control the speed - * @param fwd A DigitalOut, set high when the motor should go forward - * @param rev A DigitalOut, set high when the motor should go backwards - */ + /** Create a motor control interface + * + * @param pwm A PwmOut pin, driving the H-bridge enable line to control the speed + * @param fwd A DigitalOut, set high when the motor should go forward + * @param rev A DigitalOut, set high when the motor should go backwards + */ Motor(PinName pwm, PinName fwd, PinName rev); - + /** Set the speed of the motor - * - * @param speed The speed of the motor as a normalised value between -1.0 and 1.0 - */ + * + * @param speed The speed of the motor as a normalised value between -1.0 and 1.0 + */ void speed(float speed); protected: PwmOut _pwm; DigitalOut _fwd; DigitalOut _rev; +private: + PID speedControl; + QEI encoder; -private: - //PID speedControl; - + float set_speed; + int pulses; + float curr_speed; + float prev_pulses; }; #endif