File content as of revision 31:cc08254ab7b5:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "FastPWM.h"
#include "refGen.h"
#include "controller.h"
#include "motorConfig.h"

// Defining relevant constant parameters
const float gearRatio = 131;

// Controller parameters
const float k_p = 1;
const float k_i = 0; // Still needs a reasonable value
const float k_d = 0; // Again, still need to pick a reasonable value

QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(5);

// Defining inputs
InterruptIn sw2(SW2);
InterruptIn sw3(SW3);
InterruptIn button1(D2);
InterruptIn button2(D3);
AnalogIn pot2(A1);

// PWM settings
float pwmPeriod = 1.0/5000.0;
int frequency_pwm = 10000; //10kHz PWM


// Setting up HIDscope
volatile float x;
volatile float y;  
volatile float z; 
volatile float q;
volatile float k;

void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
        // Capture data
        x = data1;
        y = data2;
        z = data3;
        q = data4;
        k = data5;
        scope.set(0, x);   
        scope.set(1, y); 
        scope.set(2, z);
        scope.set(3, q);
        scope.set(4, z);
        scope.send(); // send what's in scope memory to PC
}

// Initializing encoder
Ticker encoderTicker;
Ticker controllerTicker;
volatile int counts = 0;
volatile float revs = 0.00;

// MOTOR CONTROL PART    
bool r_direction = false;
int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
const float maxAngle = 2*3.14*posRevRange; // max angle in radians
const float Ts = 0.1;

// Function getReferencePosition returns reference angle based on potmeter 1
refGen ref(A0);

//ref.getReferencePosition(maxAngle);

// Initializing position and integral errors to zero
float e_pos = 0;
float e_int = 0;
float e_der = 0;
float e_prev = 0;

// readEncoder reads counts and revs and logs results to serial window
void getError(float &e_pos, float &e_int, float &e_der){
    // Getting encoder counts and calculating motor position
    counts = Encoder.getPulses();
    double motor1Position = 2*3.14*(counts/(gearRatio*64.0f));

    // Computing position error
    e_pos = ref.getReferencePosition(maxAngle, r_direction) - motor1Position;
    
    // Limiting the integral error to prevent integrator saturation
    if(fabs(e_int) <= 5){
        e_int = e_int + Ts*e_pos;    
        }
    
    // Derivative error   
    e_der = (e_pos - e_prev)/Ts;
    e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error
    }

// Generate a PID controller with the specified values of k_p, k_d and k_i
controller motorController1(k_p, k_d, k_i);
    
// setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
    
void measureAndControl(){
    getError(e_pos, e_int, e_der);
    float motorValue = motorController1.control(e_pos, e_int, e_der);
    float r = ref.getReferencePosition(maxAngle, r_direction);
    sendDataToPc(r, e_pos, e_int, e_der, motorValue);
    pc.printf("Motorvalue is %.2f \r\n", motorValue);
    pc.printf("Error is %.2f \r\n", e_pos);
    pc.printf("Reference is %.2f \r\n", r);
    //pc.printf("motor1 direction is %d \r\n", motor1_direction.read());
    motor1.setMotor(motorValue);
    }

void rSwitchDirection(){
    r_direction = !r_direction;
    pc.printf("Switched reference direction! \r\n");
    }


int main()
    {
    pc.printf("Main function");
    sw2.fall(&motor1,&motorConfig::killSwitch);
    sw3.fall(&motor1, &motorConfig::turnMotorOn);
    button2.rise(&rSwitchDirection);
    pc.baud(115200);
    controllerTicker.attach(measureAndControl, Ts);
      
    pc.printf("Encoder ticker attached and baudrate set");    
    }