Nu werkt het wel, opdracht 1 is af
Dependencies: MODSERIAL mbed QEI feed_forward
Fork of feed_forward by
main.cpp
- Committer:
- RoyvZ
- Date:
- 2017-10-03
- Revision:
- 0:331597250051
- Child:
- 1:92a60278860a
File content as of revision 0:331597250051:
#include "mbed.h" #include "MODSERIAL.h" #include "math.h" DigitalOut gpo(D0); DigitalOut motorDirection(D4); DigitalOut motorSpeed(D5); AnalogIn potMeterIn(A1); InterruptIn button1(D3); Ticker ticker; 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; } 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) { // 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 } }