File content as of revision 8:55ca92c0e39d:

//****************************************************************************/
// Includes
//****************************************************************************/
#include "PID.h"
#include "QEI.h"
#include "HIDScope.h"        // Require the HIDScope library

//****************************************************************************/
// Defines
//****************************************************************************/
#define RATE  0.01 
#define Kc    0.35
#define Ti    0.15
#define Td    0.0

//****************************************************************************/
// Globals
//****************************************************************************/
Serial pc(USBTX, USBRX);
HIDScope    scope(3);        // Instantize a 2-channel HIDScope object
Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
//--------
// Motors 
//--------
//Left motor.
PwmOut leftMotor(D5);
DigitalOut leftBrake(D3);
DigitalOut leftDirection(D4);
QEI leftQei(D12, D13, NC, 624);
PID leftController(Kc, Ti, Td, RATE);
AnalogIn pot1(A0);
AnalogIn pot2(A1);

//--------
// Timers
//--------
Timer endTimer;
//--------------------
// Working variables.
//--------------------
volatile int leftPulses     = 0;
volatile int leftPrevPulses = 0;
volatile float leftPwmDuty  = 1.0;
volatile float leftPwmDutyPrev = 1.0;
volatile float leftPwmDutyChange;
volatile float leftVelocity = 0.0;

float request;
float request_pos;
float request_neg;


int main() {
    //Initialization of motor
    leftMotor.period_us(50);
    leftMotor = 1.0;
    leftBrake = 0.0;
    leftDirection = 1;
    
    //Initialization of PID controller
    leftController.setInputLimits(-1.0, 1.0);
    leftController.setOutputLimits(-1.0, 1.0);
    leftController.setBias(0.0);
    leftController.setMode(AUTO_MODE);


    endTimer.start(); //Run for 100 seconds.
    while (endTimer.read() < 100){
        // get 'emg' signal
        request_pos = pot1.read();
        request_neg = pot2.read();
        
        // determine if forward or backward signal is stronger and set reference
        if (request_pos > request_neg){
            request = request_pos; 
        } 
        else {
            request = -request_neg;
        }
        leftController.setSetPoint(request);
        
        // Velocity calculation
        leftPulses = leftQei.getPulses();
        leftVelocity = ((float)(leftPulses - leftPrevPulses))/ (30000.0* RATE);
        leftPrevPulses = leftPulses;
                
        // PID control
        leftController.setProcessValue(leftVelocity);
        leftPwmDuty = leftController.compute();
        if (leftPwmDuty < 0){
            leftDirection = 0;
            leftMotor = -leftPwmDuty;
        }
        else {
            leftDirection = 1;
            leftMotor = leftPwmDuty;
        }
        
        // User feadback
        scope.set(0, request);
        scope.set(1, leftPwmDuty);
        scope.set(2, leftVelocity);
        scope.send();
        pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);

        wait(RATE);
    }

    //Stop motors.
    leftMotor  = 0;
    
    //Close results file.
    //fclose(fp);

}