nucleo log motor script

gistfile1.txt

//For 2.74 class project
#include "mbed.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "ExperimentServer.h"
#include "QEI.h"

#define NUM_INPUTS 1
#define NUM_OUTPUTS 4

Serial pc(USBTX, USBRX);    // USB Serial Terminal
ExperimentServer server;    // Set up communications with MATLAB
PwmOut motorPWM(D5);        // Motor PWM output
DigitalOut motorFwd(D6);    // Motor forward enable
DigitalOut motorRev(D7);    // Motor backward enable
AnalogIn currentsense(A0);
Timer t;                    // Experiment timer
float r = 2.26; //winding resistance
float emf_c = 0.154; //Kb
float Kp = .1;
float i_meas; float i_desired; float vel; float emf; float duty_V; float pos;

QEI encoder(D3,D4, NC, 1200 , QEI::X4_ENCODING); // Pins D3, D4, no index, 1200 counts/rev, Quadrature encoding    

Ticker currentLoop; //create a currentloop interrupt ticker

void CurrentLoop() {
    i_meas = currentsense.read()*3.3/.14;
    vel = encoder.getVelocity()*6.28/1200.0;
    pos = encoder.getPulses()*6.28/1200.0;
    if (i_desired < 0) {
        motorFwd = 0; motorRev = 1;
        emf = vel*emf_c*(-1);
    }else if (i_desired > 0) {
        motorFwd = 1; motorRev = 0;
        emf = vel*emf_c;
    }
    duty_V = (r*abs(i_desired) + emf + Kp*(abs(i_desired - pos/emf_c) - i_meas))/12.0;
    //duty_V = (r*abs(i_desired)+emf_c*vel)/12.0;
    //duty_V = (Kp*(abs(i_desired) - i_meas))/12.0;
            
    if (duty_V < 0) {
        motorFwd = motorFwd*(-1);
        motorRev = motorRev*(-1);
    }
    if (abs(duty_V) > 1) {
        duty_V = 1;
    }
}

int main (void) {
    // Link terminal to server
    server.attachTerminal(pc);
    server.init();

    // PWM period is nominally a multiple of the control loop
    motorPWM.period_us(500);
    
    // Continually get input from MATLAB and run experiments
    float input_params[NUM_INPUTS];
    
    while(1) {
        if (server.getParams(input_params,NUM_INPUTS)) {
            
             //Current input loop
            i_desired = input_params[0];
            
            // Setup experiment
            t.reset();
            t.start();
            encoder.reset();
        while ( t.read() < 2) {
            // Run experiment
            currentLoop.attach(&CurrentLoop, 0.00005);
            // Perform control loop logic
            motorPWM.write(abs(duty_V));
                
            // Form output to send to MATLAB    
            float output_data[NUM_OUTPUTS];
            output_data[0] = t.read();
            output_data[1] = i_meas;
            output_data[3] = pos;
            output_data[2] = vel;
            //output_data[4] = pos;
            //output_data[3] = duty_V;
            //output_data[4] = i_meas*emf_c/vel;
             
            // Send data to MATLAB
            server.sendData(output_data,NUM_OUTPUTS);
            //totalerr += err; 
            wait(.001); 
        }     
            // Cleanup after experiment
            server.setExperimentComplete();
            currentLoop.detach();
            motorPWM.write(0);
        } // end if
    } // end while
} // end main