first itteration
Dependencies: MODSERIAL QEI mbed
main.cpp
- Committer:
- Arnoud113
- Date:
- 2017-10-04
- Revision:
- 2:abd40da4a5e2
- Parent:
- 1:659489c32e75
- Child:
- 3:cc3766838777
File content as of revision 2:abd40da4a5e2:
#include "mbed.h" #include "QEI.h" #include "MODSERIAL.h" #include "math.h" DigitalOut gpo(D0); DigitalOut led(LED_BLUE); DigitalOut motor1DC(D7); PwmOut motor1PWM(D6); DigitalOut motor2DC(D4); PwmOut motor2PWM(D5); AnalogIn potMeterIn(A0); DigitalIn button1(D11); MODSERIAL pc(USBTX,USBRX); Ticker controller; 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; } 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 SetMotor1(float motorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue >=0) motor1DC= 1; else motor1DC=0; if (fabs(motorValue)>1) motor1PWM = 1; else motor1PWM = fabs(motorValue); } void MeasureAndControl(void) { float referenceVelocity = GetReferenceVelocity(); float motorValue = FeedForwardControl(referenceVelocity); SetMotor1(motorValue); } int main() { pc.baud(115200); QEI Encoder(D12,D13,NC,32); //float v = GetReferenceVelocity(); //float controlValue = FeedForwardControl(v); //SetMotor1(controlValue); controller.attach(&MeasureAndControl, 0.1); while(1) { int counts = Encoder.getPulses(); float rV = GetReferenceVelocity(); float mV = FeedForwardControl(rV); pc.printf("\r reference velocity: %f. Motor Value is: %f number of counts: %i\n",mV,rV,counts); } }