File content as of revision 20:ac1b4ffa3323:

#include "QEI.h"
#include "mbed.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"

// ENC1A --> D13
// ENC1B --> D12
// POT1 --> A0
// LED1 --> D3
// BUT1 --> D1
// BUT2 --> D0

MODSERIAL   pc(USBTX, USBRX);
Ticker      ControlMotor1;

AnalogIn    theta_ref(A0);
DigitalOut  PWM_motor_1(D6);
DigitalOut  direction_motor_1(D7);

HIDScope scope(2);
QEI encoder(D12,D13,NC,64,QEI::X4_ENCODING);
Ticker RW_scope;

// variables
float duty_cycle_motor_1;
volatile int on_time_ms; // The time the LED should be on, in microseconds
volatile int off_time_ms;

float theta = 0;
volatile double x_0;
volatile double x_prev_0 = 0;
volatile double y_0;
volatile double x_1;
volatile double x_prev_1 = 0;
volatile double y_1; 

float omega;
float torque;
float torque_p;
float torque_i;
float torque_d;
float velocity;
float theta_error;

float Kp = 5;
float Ki = 3;
float Kd = 3;
float Ts = 1;

// functions
float CalculateError(){
    //theta_error = (360*theta_ref.read())-theta;
    theta_error = 180;
    return theta_error;}

float Controller(float theta_error){
    //static float error_integral = 0;
    //static float error_prev = 0;
    //static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    // Proportional part:
    float torque_p = Kp * theta_error;
    
    // Integral part:
    //error_integral = error_integral + theta_error * Ts;
    //float torque_i = Ki * error_integral;
    
    // Derivative part:
    //float error_derivative = (theta_error - error_prev)/Ts;
    //float filtered_error_derivative = LowPassFilter.step(error_derivative);
    //float torque_d = Kd * filtered_error_derivative;
    //float torque_d = Kd*error_derivative;
    //error_prev = theta_error;
    
    // Sum all parts and return it
    //float torque = torque_p + torque_i + torque_d;
    return torque_p;}

void CalculateDirectionMotor1() {
    if (theta_error >= 0) {
        direction_motor_1 = 1;}
    else {
        direction_motor_1 = 0;}}

//float CalculateVelocityMotor1() {
//    return theta_error/Ts;}

void PulseWidthModulation() // Bepaalt de duty cycle gebaseerd op de potentiometer en zet de motor dan op die snelheid mbh PWM
{   on_time_ms = (int) (duty_cycle_motor_1*(1/1e2)*1e3);    // Deze variable maken
    off_time_ms = (int) ((1-duty_cycle_motor_1)*(1/1e2)*1e3);
    PWM_motor_1 = 1;
    wait_ms(on_time_ms);
    PWM_motor_1 = 0;
    wait_ms(off_time_ms);}
    
void MotorDirection(){
    }

void ReadEncoderAndWriteScope(){
    theta = ((360/64)*encoder.getPulses())/131.25; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
    x_0 = theta;
    y_0 = (x_prev_0 + x_0)/2.0;
    scope.set(0,y_0);
    x_prev_0 = x_0;
    y_1 = (x_0 - x_prev_0)/Ts;
    scope.set(1,y_1);
    scope.send();}

//void doeietsnuttigs(){
  //  Controller(CalculateError());}
    
// main
int main()
{
    pc.baud(115200);
    pc.printf("Hello World!\n\r");
    //ControlMotor1.attach(&ReadEncoderAndWriteScope, Ts);
    //ControlMotor1.attach(&doeietsnuttigs, Ts);
    while(1) {
        pc.printf("%f \n\r",Controller(CalculateError()));
        wait(1);
    }
}