Simple program to control pololu motor with button (direction) and potmeter (velocity)

Dependencies:   MODSERIAL mbed

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)
 {}
}