main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 
00004 Serial pc(USBTX, USBRX);
00005 
00006 DigitalIn button1(D8);
00007 AnalogIn potMeterIn(A1);
00008 
00009 DigitalOut motor1DirectionPin(D4);
00010 PwmOut motor1MagnitudePin(D5);
00011 
00012 
00013 float referenceVelocity;  // in rad/s
00014 float motorValue;
00015 
00016 Ticker controlmotor1;
00017 
00018 float GetReferenceVelocity()
00019 {
00020     // Returns reference velocity in rad/s. 
00021     // Positive value means clockwise rotation.
00022     const float maxVelocity=8.4; // in rad/s of course!    
00023     
00024     if (button1)   
00025         {
00026         // Clockwise rotation      
00027         referenceVelocity = potMeterIn * maxVelocity;
00028         } 
00029         else 
00030         {
00031         // Counterclockwise rotation       
00032         referenceVelocity = -1*potMeterIn * maxVelocity;  
00033         }
00034     return referenceVelocity;
00035 }
00036 
00037 
00038 void SetMotor1(float motorValue)
00039 {
00040     // Given -1<=motorValue<=1, this sets the PWM and direction
00041     // bits for motor 1. Positive value makes motor rotating
00042     // clockwise. motorValues outside range are truncated to
00043     // within range
00044     if (motorValue >=0) motor1DirectionPin=1;
00045         else motor1DirectionPin=0;
00046     if (fabs(motorValue)>1) motor1MagnitudePin = 1;
00047         else motor1MagnitudePin = fabs(motorValue);
00048         
00049 }
00050 
00051 float FeedForwardControl(float referenceVelocity)
00052 {
00053     // very simple linear feed-forward control
00054     const float MotorGain=8.4; // unit: (rad/s) / PWM
00055     float motorValue = referenceVelocity / MotorGain;
00056     return motorValue;
00057 }
00058         
00059 void MeasureAndControl(void)
00060 {
00061     // This function measures the potmeter position, extracts a
00062     // reference velocity from it, and controls the motor with 
00063     // a simple FeedForward controller. Call this from a Ticker.
00064     float referenceVelocity = GetReferenceVelocity();
00065     float motorValue = FeedForwardControl(referenceVelocity);
00066     SetMotor1(motorValue);
00067 }
00068 
00069 int main()
00070 {
00071     while (true) 
00072     {
00073     controlmotor1.attach(MeasureAndControl, 0.01);
00074     pc.baud(115200);
00075     wait(0.1f);
00076     return 0;
00077     
00078     }
00079 }