Nu werkt het wel, opdracht 1 is af
Dependencies: MODSERIAL mbed QEI feed_forward
Fork of feed_forward by
Revision 2:a64e3c37b571, committed 2017-10-11
- Comitter:
- DiondeGreef
- Date:
- Wed Oct 11 07:54:25 2017 +0000
- Parent:
- 1:92a60278860a
- Commit message:
- Servomotoren compleet
Changed in this revision
diff -r 92a60278860a -r a64e3c37b571 QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Wed Oct 11 07:54:25 2017 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
diff -r 92a60278860a -r a64e3c37b571 feed_forward.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/feed_forward.lib Wed Oct 11 07:54:25 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/DiondeGreef/code/feed_forward/#92a60278860a
diff -r 92a60278860a -r a64e3c37b571 main.cpp --- a/main.cpp Tue Oct 03 15:35:26 2017 +0000 +++ b/main.cpp Wed Oct 11 07:54:25 2017 +0000 @@ -2,88 +2,127 @@ #include "MODSERIAL.h" #include "math.h" -DigitalOut gpo(D0); -DigitalOut motorDirection(D4); -PwmOut motorSpeed(D5); -AnalogIn potMeterIn(A1); -InterruptIn button1(D3); -Ticker ticker; - -MODSERIAL pc(USBTX, USBRX); +DigitalOut Ledr(LED_RED); +DigitalOut Ledg(LED_GREEN); +DigitalOut Ledb(LED_BLUE); +PwmOut motorSpeed(D13); +PwmOut motorSpeed2(D12); +InterruptIn Button1(PTC6); +Ticker tick; -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; -} +enum states{Close, Open}; + +states CurrentState = Open; + +int i = 0; +int j = 0; -void setMotor(float motorValue) { - if (motorValue >= 0) - { - //float motor1DirectionPin1 = 1; - motorDirection=1; - } - else - { - //float motor1DirectionPin1 = 0; - motorDirection=0; - } - - if (fabs(motorValue)>1) - { - //float motor1MagnitudePin1 = 1; - motorSpeed = 1; - } - else - { - //float motor1MagnitudePin1 = fabs(motorValue); - motorSpeed = fabs(motorValue); - } -} - -float FeedForwardControl(float referenceVelocity) +void Change() { - // very simple linear feed-forward control - const float MotorGain=8.4; // unit: (rad/s) / PWM - float motorValue = referenceVelocity / MotorGain; - return motorValue; + if(CurrentState == Close){ + CurrentState = Open; + } + else{ + CurrentState = Close; + } } -void MeasureAndControl(void) + +void ProcessStateMachine(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); + switch (CurrentState) + { + case Close: + if(j <= 34){ + motorSpeed.write(0); + motorSpeed2.write(0); + j++; + } + else if (j == 35){ + motorSpeed.write(1); + motorSpeed2.write(0); + j++; + } + else if (j == 36){ + motorSpeed.write(1); + motorSpeed2.write(0); + j++; + } + else if (j == 37){ + motorSpeed.write(1); + motorSpeed2.write(0); + j++; + } + else if (j == 38){ + motorSpeed.write(1); + motorSpeed2.write(0); + j++; + } + else if (j == 39){ + motorSpeed.write(1); + motorSpeed2.write(1); + j = 0; + } + Ledr = 0; + Ledg = 1; + Ledb = 1; + break; + + case Open: + if(j <= 34){ + motorSpeed.write(0); + motorSpeed2.write(0); + j++; + } + else if (j == 35){ + motorSpeed.write(0); + motorSpeed2.write(1); + j++; + } + else if (j == 36){ + motorSpeed.write(0); + motorSpeed2.write(1); + j++; + } + else if (j == 37){ + motorSpeed.write(0); + motorSpeed2.write(1); + j++; + } + else if (j == 38){ + motorSpeed.write(0); + motorSpeed2.write(1); + j++; + } + else if (j == 39){ + motorSpeed.write(1); + motorSpeed2.write(1); + j = 0; + } + Ledr = 1; + Ledg = 1; + Ledb = 0; + break; + + default: + Ledr = 1; + Ledg = 0; + Ledb = 1; + } } + + int main() { - pc.baud(115200); - //ticker.attach(MeasureAndControl, 0.01); - while(true){ - wait(0.1); + tick.attach(ProcessStateMachine, 0.0005); + Ledr = 1; + Ledg = 1; + Ledb = 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 + while (true) + { + Button1.rise(&Change); } -} - \ No newline at end of file +} \ No newline at end of file