File content as of revision 0:eb94c702a103:

/*************************************************************************************
Program Name: ES305 Laboratory Experiment 1, part 1 - mbed serial setup and streaming
Description: A basic code to run a DC motor using a Ticker object. The user specifies
sun rimte, and duty cycle. The program runs at 100 Hz and streams data at 50 Hz
Author: Levi DeVries, PhD, USNA
Date: 8/14/2017
**************************************************************************************/

// Include necessary libraries
#include "mbed.h"
#include "mbedWSEsbc.h"
#define PI (3.14159)


// Declare objects (if necessary)
Ticker Controller; // declare Ticker object named "Controller"
DigitalOut myled(LED1); // LED, flash lights for debugging


// variables for data handling and storage
float TotalTime;        // Total run time
float Time;             // Current elapsed time
float Ts = 0.0083;        // Control update period (seconds) (100 Hz equivalent)
float Tstrm = 0.01;     // Data streaming period (seconds) (50 Hz equivalent)
float usrDC;            // uaser-specified duty cycle
float ang,angp,speed;   // variables for approximating speed from encoder measurements
float dc;               // duty cycle applied to motor
long enc1;              // encoder variable
float lowDC;
float dspd;
float spdErr;
float Kp;
float dc_p = 0.0;
float spdErr_p = 0.0;




// Function definition Prototypes (declarations)
void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists
void twoStepCode();
void pCtrlCode();
void PIctrlCode();


// Enter main function
int main ()
{
    // Initializes mbed to access functionality of encoder, A/D, driver, etc. chipsets
    // Input is baud rate for PC communication
    mbedWSEsbcInit(115200); // also initializes timer object t
    mot_en1.period(0.020); // sets PWM period to 0.02 seconds for best DC motor operation

    while(1) {

        // Scan serial port for user input to begin experiment
        //pc.scanf("%f,%f",&TotalTime,&lowDC);
        pc.scanf("%f,%f,%f",&TotalTime,&dspd,&Kp);

        // perform necessary functions to time the experiment
        Time = 0.0; // reset time variable
        t.reset(); // reset timer object

        // Attach the ctrCode function to ticker object specified with period Ts
        //Controller.attach(&twoStepCode,Ts);
        Controller.attach(&PIctrlCode,Ts);

        t.start(); // start measuring elapsed time

        // perform operations while the elapsed time is less than the desired total time
        while(Time <= TotalTime) {



            // read current elapsed time
            Time = t.read();


            // send data over serial port
            pc.printf("%f,%f,%f\n",Time,speed,dc);


            wait(Tstrm); // print data at approximately 50 Hz




        } // end while(Time<=Ttime)

        Controller.detach(); // detach ticker to turn off controller
        // Turn motor off at end of experiment
        mot_control(1,0.0);

    }// end while(1)
}// end main



// Additional function definitions
void ctrCode() // function to attach to ticker
{

    myled = !myled; // toggle LED 2 to indicate control update

    // Read encoder
    enc1 = LS7366_read_counter(1); // input is the encoder channel

    // Convert from counts to radians
    ang = 2.0*PI*enc1/6400.0;

    // Estimate speed
    speed = (ang-angp)/Ts;

    // Age variables
    angp = ang;

    // compute duty cycle for motor (will be changed later!)
    dc = usrDC; // user-specified duty cycle

    // motor control
    mot_control(1,dc); // first input is the motor channel, second is duty cycle

} // end ctrCode()




// Additional function definitions
void twoStepCode() // function to attach to ticker
{
    myled = !myled; // toggle LED 2 to indicate control update

    // Read encoder
    enc1 = LS7366_read_counter(1); // input is the encoder channel

    // Convert from counts to radians
    ang = 2.0*PI*enc1/6400.0;

    // Estimate speed
    speed = (ang-angp)/Ts;

    // Age variables
    angp = ang;

    // compute duty cycle for motor over two step inputs
    if(Time<0.1) {
        dc = 0.0;
    } else if(Time<0.55) {
        dc = lowDC; // low duty cycle
    } else {
        dc = 0.10;
    }
    // motor control
    mot_control(1,dc); // first input is the motor channel, second is duty cycle
} // end twoStepCode()

void pCtrlCode() // function to attach to ticker
{
    myled = !myled; // toggle LED 2 to indicate control update

    // Read encoder
    enc1 = LS7366_read_counter(1); // input is the encoder channel

    // Convert from counts to radians
    ang = 2.0*PI*enc1/6400.0;

    // Estimate speed
    speed = (ang-angp)/Ts;

    // Age variables
    angp = ang;

    // compute error
    spdErr = dspd-speed;

    // compute duty cycle for motor 
    if(Time<0.1) {
        dc = 0.0;
    } else {
        dc = Kp*(spdErr);
    }
    // enforce duty cycle saturation
    if(dc>1.0) {
        dc = 1.0;
    } else if(dc<-1.0) {
        dc = -1.0;
    }
    // motor control
    mot_control(1,dc); // first input is the motor channel, second is duty cycle
} 

void PIctrlCode() // function to attach to ticker
{
    myled = !myled; // toggle LED 2 to indicate control update

    // Read encoder
    enc1 = LS7366_read_counter(1); // input is the encoder channel

    // Convert from counts to radians
    ang = 2.0*PI*enc1/6400.0;

    // Estimate speed
    speed = (ang-angp)/Ts;

    // Age variables
    angp = ang;

    // compute error
    spdErr = dspd-speed;
    
    
    // compute duty cycle for motor 
    if(Time<0.1) {
        dc = 0.0;
    } else {
        dc = dc_p + 0.03554*spdErr - 0.030412*spdErr_p;
        //dc = dc_p+(0.6593/20)*(1.079*spdErr-0.9213*spdErr_p);
    }
    dc_p = dc;
        spdErr_p = spdErr;
    // enforce duty cycle saturation
    if(dc>1.0) {
        dc = 1.0;
    } else if(dc<-1.0) {
        dc = -1.0;
    }
    // motor control
    mot_control(1,dc); // first input is the motor channel, second is duty cycle
}