Gerhard Berman
/
prog_pract3
Simple program to control pololu motor with button (direction) and potmeter (velocity)
main.cpp
- Committer:
- GerhardBerman
- Date:
- 2016-10-10
- Revision:
- 0:8f8157690923
File content as of revision 0:8f8157690923:
#include "mbed.h" #include <math.h> #include "MODSERIAL.h" AnalogIn potMeterIn(A0); DigitalIn button1(D7); DigitalOut motor1DirectionPin(D4); PwmOut motor1MagnitudePin(D5); Serial pc(USBTX,USBRX); Ticker MeasureTicker; volatile float referenceVelocity = 0; // in rad/s float GetReferenceVelocity() { // Returns reference velocity in rad/s. // Positive value means clockwise rotation. const float maxVelocity=8.4; // in rad/s of course! 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 volatile 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) motor1DirectionPin=1; else motor1DirectionPin=0; if (fabs(motorValue)>1) motor1MagnitudePin = 1; else motor1MagnitudePin = fabs(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. volatile float referenceVelocity = GetReferenceVelocity(); float motorValue = FeedForwardControl(referenceVelocity); SetMotor1(motorValue); } int main() { //Initialize MeasureTicker.attach(&MeasureAndControl, 1.0/100); pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); pc.baud(115200); while(true) {} }