adsfadsfa
Dependencies: Encoder MODSERIAL feed_forward mbed
Fork of feed_forward by
Revision 2:68de33d10c67, committed 2017-10-11
- Comitter:
- DiondeGreef
- Date:
- Wed Oct 11 09:49:09 2017 +0000
- Parent:
- 1:92a60278860a
- Commit message:
- alkdfjakljasdfladksfj;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Wed Oct 11 09:49:09 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/feed_forward.lib Wed Oct 11 09:49:09 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/DiondeGreef/code/feed_forward/#92a60278860a
--- a/main.cpp Tue Oct 03 15:35:26 2017 +0000 +++ b/main.cpp Wed Oct 11 09:49:09 2017 +0000 @@ -1,89 +1,56 @@ #include "mbed.h" #include "MODSERIAL.h" #include "math.h" +#include "encoder.h" DigitalOut gpo(D0); DigitalOut motorDirection(D4); PwmOut motorSpeed(D5); -AnalogIn potMeterIn(A1); -InterruptIn button1(D3); -Ticker ticker; +AnalogIn potMeterIn1(A1); +AnalogIn potMeterIn2(A2); +//InterruptIn button1(D3); +Ticker m1_Ticker; +Encoder encoder1(D13,D12); +const double M1_KP = 2.5, M1_KI = 1.0; +const double M1_TS = 0.01; +const double RAD_PER_PULSE = 0.002991; +double m1_err_int = 0; +int motorD = 0; +double motor1 = 0; MODSERIAL pc(USBTX, USBRX); -float GetReferenceVelocity() -{ - // Returns reference velocity in rad/s. - // Positive value means clockwise rotation. - const float maxVelocity=8.4; // in rad/s of course! - float referenceVelocity; // in rad/s - if (button1) { - // Clockwise rotation - referenceVelocity = potMeterIn * maxVelocity; - } - else { - // Counterclockwise rotation - referenceVelocity = -1*potMeterIn * maxVelocity; - } - return referenceVelocity; + // Reusable PI controller +double PI( double e, const double Kp, const double Ki, double Ts, double &e_int ){ + e_int += Ts ∗ e; // e_int is changed globally because it’s ’by reference’ (&) + return Kp ∗ e + Ki ∗ e_int; } - -void setMotor(float motorValue) { - if (motorValue >= 0) + // Next task, measure the error and apply the output to the plant +void m1_Controller() { + double reference = potMeterIn1; + + + double position = RAD_PER_PULSE∗encoder1; // Don’t use magic numbers! + motor1 = PI( reference − position, M1_KP, M1_KI, M1_TS, m1_err_int ); + if ( reference − position >= 0) { //float motor1DirectionPin1 = 1; - motorDirection=1; + motorD=1; } else { //float motor1DirectionPin1 = 0; - motorDirection=0; + motorD=0; } - - if (fabs(motorValue)>1) - { - //float motor1MagnitudePin1 = 1; - motorSpeed = 1; - } - else - { - //float motor1MagnitudePin1 = fabs(motorValue); - motorSpeed = fabs(motorValue); - } -} - -float FeedForwardControl(float referenceVelocity) -{ - // very simple linear feed-forward control - const float MotorGain=8.4; // unit: (rad/s) / PWM - float motorValue = referenceVelocity / MotorGain; - return motorValue; -} - - -void MeasureAndControl(void) -{ - // This function measures the potmeter position, extracts a - // reference velocity from it, and controls the motor with - // a simple FeedForward controller. Call this from a Ticker. - float referenceVelocity = GetReferenceVelocity(); - float motorValue = FeedForwardControl(referenceVelocity); - setMotor(motorValue); } int main() { - pc.baud(115200); - //ticker.attach(MeasureAndControl, 0.01); - while(true){ - wait(0.1); - - //pc.printf("%f\r\n",GetReferenceVelocity()); - float v_ref = GetReferenceVelocity(); - setMotor(v_ref); - pc.printf("%f \r\n", FeedForwardControl(v_ref)); - motorDirection.write(motorDirection); - motorSpeed.write(motorSpeed); //PWM Speed Control + m1_Ticker.attach( &m1_Controller, M1_TS ); // 100 Hz + while( 1 ) { + motorDirection.write(motorD); + motorSpeed.write(motor1); } } +} \ No newline at end of file