File content as of revision 1:ffa6f4d78c8e:

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"  //make sure hidscope cable is also attached


// in gebruik: D(0,1,4,5,6,7,10,11,12,13) 

DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
FastPWM motor1(D6);     // speed of motor 1
FastPWM motor2(D5);     //speed of motor 2
DigitalOut motor2dir(D4);   //direction of motor 2, attach at m2, set to 0: ccw 

Ticker control; //Ticker for processing encoder input
volatile bool control_go=false;

HIDScope scope(3); // aantal scopes in hidscope opzetten

  
double m1_pwm=0;    //variable for PWM control motor 1
double m2_pwm=0;    //variable for PWM control motor 2

const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 1.0, m1_N = 100; // controller constants motor 1
double m1_v1 = 0, m1_v2 = 0; // Memory variables
const double m1_Ts = 0.01; // Controller sample time

const double m2_Kp = 2.5, m2_Ki = 1.0, m2_Kd = 1.0, m2_N = 100; // controller constants motor 2
double m2_v1 = 0, m2_v2 = 0; // Memory variables
const double m2_Ts = 0.01; // Controller sample time

const double pi=3.14159265359;
const double res = 64/(1/131.25*2*pi);     // resolution on gearbox shaft per pulse
const double V_max=9.0;                    // maximum voltage supplied by trafo
const double minRadius=0.0;                // minimum radius of arm
const double maxRadius=0.3;                // maximum radius of arm
const double pulleyDiameter=0.0398;        // pulley diameter
const double minAngle=-1.25;               // minimum angle for limiting controller

QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder



MODSERIAL pc(USBTX,USBRX);

void activate_controller(){controller_go=true}; //activate go flag

double PID( double err, const double Kp, const double Ki, const double Kd,
const double Ts, const double N, double &v1, double &v2 ) {
    const double a1 =-4/(N*Ts+2),
        a2=-(N*Ts-2)/(N*Ts+2),
        b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
        b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
        b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);

    double v=err-a1*v1-a2*v2;
    double u=b0*v+b1*v1+b2*v2
    v2=v1; v1=v;
    return u;
}

 
 void controller(){ //function for executing controller action              
        //converting radius and theta to gearbox angle    
        ref_angle1=16*theta;    
        ref_angle2=-radius/pi/pulleyDiameter;
        
                
        angle1 = Encoder1.getPulses()/res;   //get number of pulses (counterclockwise is positive)
        angle2 = Encoder2.getPulses()/res;   //get number of pulses
        m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max; 
        //divide by voltage to get pwm duty cycle percentage)
        m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;    
        
        if (m1_pwm >=0.0f && m1_pwm <=1.0f){
            motor1dir=0;
            motor1.write(m1_pwm);
        }
        else if (m1_pwm < 0.0f && m1_pwm >= -1.0f){ 
            motor1dir=1;
            motor1.write(-m1_pwm);
        }
        
        if (m2_pwm >=0.0f && m2_pwm <=1.0f){
            motor1dir=0;
            motor1.write(m2_pwm);
        }
        else if (m2_pwm < 0.0f && m2_pwm >= -1.0f){ 
            motor1dir=1;
            motor1.write(-m2_pwm);
        }
              
        //hidsopce to check what the code does exactly
        scope.set(0,angle1);
        scope.set(1,refangle1-angle1);
        scope.set(2,m1_pwm
        scope.send();
        
        
     }     


        }

int main()
{
    pc.baud(115200);
    motor1.period(0.02f); //period of pwm signal for motor 1
    motor2.period(0.02f); // period of pwm signal for motor 2
    motor1dir=0; // setting direction to ccw 
    motor2dir=0; // setting direction to ccw
    control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
    pc.printf("RESET\n\r");
    while (true) {
        if(controller_go){
            controller_go=false;
            controller();
    }
}