File content as of revision 18:54195aa5e534:

#include "mbed.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "ExperimentServer.h"
#include "QEI.h"
#include "MotorShield.h" 
#include "HardwareSetup.h"

#define NUM_INPUTS 9
#define NUM_OUTPUTS 6
float velocity = 0;
float current = 0;
float current_d = 0;
float volt = 0;
float duty = 0;
float cum_error = 0;
float kp = 0;
float ki = 0;
float R =0;
float emf =0;
float th_d= 0;
float period=0;

Serial pc(USBTX, USBRX);    // USB Serial Terminal
ExperimentServer server;    // Object that lets us communicate with MATLAB
Timer t;                    // Timer to measure elapsed time of experiment
Ticker ControlLoop;

QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING);  // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING);  // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING);  // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)

MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 clock ticks or ~20kHZ

void current_control()
{
    float error = 0;
    velocity = encoderA.getVelocity()*(6.2831/1200.0);
    current = -(motorShield.readCurrentA()*(30.0/65536.0)-15.15); //read current for motor A in amps. Note: this is a slightly different current sensor so its a different conversion than last lab.            
    error = current_d - current;
    cum_error = cum_error + error;
    if (cum_error < -3){
       cum_error = -3;
    }
    else if (cum_error > 3){
        cum_error = 3;       
    }
    volt = R*current_d + emf*velocity + kp * error + ki * (cum_error);
    duty  = volt/12.0;
    if (duty > 1){
        duty = 1;
    }
    if (duty <-1){
        duty = -1;   
    }
    if (duty >= 0){
        motorShield.motorAWrite(duty, 0); //run motor A at "v1" duty cycle and in the forward direction 
    }
    else if (duty < 0){
        motorShield.motorAWrite(abs(duty), 1);
    }
}


int main (void)
{
    // Link the terminal with our server and start it up
    server.attachTerminal(pc);
    server.init();
    
    // Continually get input from MATLAB and run experiments
    float input_params[NUM_INPUTS];
    pc.printf("%f",input_params[0]);
    
    while(1) {
        if (server.getParams(input_params,NUM_INPUTS)) {
            //float d1   = input_params[0]; // Duty cycle for first second
            //float d2   = input_params[1]; // Duty cycle for second second
            //float current_d = input_params[0];
            //float volt_in = input_params[0];
            current_d = 0;
            cum_error = 0;
            th_d =input_params[0];
            R = input_params[1];
            emf = input_params[2];
            kp = input_params[3];
            ki = input_params[4];
            float k = input_params[5];
            float b = input_params[6];
            float D = input_params[7];
            period = input_params[8];
            
            ControlLoop.attach(&current_control,period); //start current loop
            
            // Setup experiment
            t.reset();
            t.start();
            float th= 0;
            float error = 0;
            encoderA.reset();
            encoderB.reset();
            encoderC.reset();
            encoderD.reset();

            motorShield.motorAWrite(0, 0); //turn motor A off
            
            //use the motor shield as follows:
            //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
             
            // Run experiment
            while( t.read() < 5 ) {
                // Perform control loop logic
                th = encoderA.getPulses()*(6.2831/1200.0); 
                //velocity = encoderA.getVelocity()*(6.2831/1200.0);
                //current = motorShield.readCurrentA()*(30.0/65536.0)-15.1; //read current for motor A in amps. Note: this is a slightly different current sensor so its a different conversion than last lab.
                
                current_d = k*(th_d-th) - D*velocity + b*velocity;

                float output_data[NUM_OUTPUTS];
                output_data[0] = t.read();
                output_data[1] = th;
                output_data[2] = velocity;
                output_data[3] = current;
                output_data[4] = current_d;
                output_data[5] = volt;
                
                
                // Send data to MATLAB
                server.sendData(output_data,NUM_OUTPUTS);              
                wait(.001); //run control loop at 1kHz
            }
            // Cleanup after experiment
            ControlLoop.detach();
            server.setExperimentComplete();
            motorShield.motorAWrite(0, 0); //turn motor A off
        } // end if
    } // end while
    
} // end main