File content as of revision 4:be465e9a12cb:

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

//****************************************************************************/
// Defines
//****************************************************************************/
#define RATE  0.01
#define Kc   -2.6
#define Ti    0.0
#define Td    0.0

//****************************************************************************/
// Globals
//****************************************************************************/
Serial pc(USBTX, USBRX);
HIDScope    scope(2);        // 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);
//-------
// Files
//-------
//LocalFileSystem local("local");
//FILE* fp;
//--------
// Timers
//--------
Timer endTimer;
//--------------------
// Working variables.
//--------------------
volatile int leftPulses     = 0;
volatile int leftPrevPulses = 0;
volatile float leftPwmDuty  = 1.0;
volatile float leftVelocity = 0.0;
//Velocity to reach.
int goal = 3000;
float measure;

//****************************************************************************/
// Prototypes
//****************************************************************************/
//Set motors to go "forward", brake off, not moving.
void initializeMotors(void);
//Set up PID controllers with appropriate limits and biases.
void initializePidControllers(void);

void initializeMotors(void){

    leftMotor.period_us(50);
    leftMotor = 1.0;
    leftBrake = 0.0;
    leftDirection = 1;

}

void initializePidControllers(void){

    leftController.setInputLimits(0.0, 10500.0);
    leftController.setOutputLimits(0.0, 1.0);
    leftController.setBias(1.0);
    leftController.setMode(AUTO_MODE);

}

int main() {
    scopeTimer.attach_us(&scope, &HIDScope::send, 1e4);
    //Initialization.
    initializeMotors();
    initializePidControllers();

    //Open results file.
    //fp = fopen("/local/pidtest.csv", "w");
    
    endTimer.start();

    //Set velocity set point.
    
    
    
    //Run for 3 seconds.
    while (endTimer.read() < 100){
        measure = pot1.read()*10500;
        leftController.setSetPoint(measure);
        leftPulses = leftQei.getPulses();
        leftVelocity = (leftPulses - leftPrevPulses) / RATE;
        leftPrevPulses = leftPulses;
        
        scope.set(0, measure);
        scope.set(1, leftVelocity);
        
        leftController.setProcessValue(-leftVelocity);
        leftPwmDuty = leftController.compute();
        leftMotor = leftPwmDuty;
       
        pc.printf("leftpusles: %d, lefVelocity: %f, leftPwmDuty: %f \n\r",leftPulses,leftVelocity,leftPwmDuty);

        //fprintf(fp, "%f,%f\n", leftVelocity, goal);
        wait(RATE);
    }

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

}