File content as of revision 17:8a0c720733b8:

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

// Button and potmeter1 control
InterruptIn button1(D11);
InterruptIn button2(D10);
InterruptIn buttonsw2(SW2);
InterruptIn buttonsw3(SW3);
AnalogIn potmeter1(A0);
AnalogIn potmeter2(A1);
AnalogIn potmeter3(A2);
AnalogIn potmeter4(A3);
// Encoder
DigitalIn encA1(D9);
DigitalIn encB1(D8);
DigitalIn encA2(D13);
DigitalIn encB2(D13);
QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING);
QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING);
float Ts = 0.01;
float motor1angle;
float motor2angle;
float omega1;
float omega2;


// Motor
DigitalOut motor2Direction(D4);
FastPWM motor2Power(D5);
DigitalOut motor1Direction(D7);
FastPWM motor1Power(D6);

volatile int motortoggle = 1;

//Motorcontrol
bool motordir1;
bool motordir2;
float motor1ref=0.1745;
float motor2ref=0.0873;
double controlsignal1;
double controlsignal2;
double pi2= 6.283185;
float motor1error; //e = error
float motor2error;
float Kp=0.27;
float Ki=0.35;
float Kd=0.1;
float u_p1;
float u_p2;
float u_i1;
float u_i2;

//Windup control
float ux1;
float ux2;
float up1;
float up2;
float ek1;
float ek2;
float ei1= 0.0;
float ei2=0.0;
float Ka= 1.0;

//RKI 
float Vx;
float Vy;
float q1dot;
float q2dot;
float l1=26.0;  //Afstand base-link [cm]
float l2=62.0;  //Afstand link-endpoint [cm]

//Hidscope
HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.

// PC connection
MODSERIAL pc(USBTX, USBRX);

// Intializing tickers
Ticker motorTicker;
Ticker controlTicker;
Ticker directionTicker;
Ticker encoderTicker;
Ticker scopeTicker;

const float PWM_period = 1e-6;

volatile int counts1; // Encoder counts
volatile int counts2;
volatile int countsPrev1 = 0;
volatile int countsPrev2 = 0;
volatile int deltaCounts1;
volatile int deltaCounts2;

float factorin = 6.23185/64; // Convert encoder counts to angle in rad
float gearratio = 131.25; // Gear ratio of gearbox


float PID_controller1(float motor1error){
    static float error_integral1=0;
    static float e_prev1=motor1error;
    
    //Proportional part:
    u_p1=Kp*motor1error;
    
    //Integral part
    error_integral1=error_integral1+ei1*Ts;
    u_i1=Ki*error_integral1;
    
    //Derivative part
    float error_derivative1=(motor1error-e_prev1)/Ts;
    float u_d1=Kd*error_derivative1;
    e_prev1=motor1error;
    
    // Sum and limit
    up1= u_p1+u_i1+u_d1;
    if (up1>1){
        controlsignal1=1;}
    else if (up1<-1){
        controlsignal1=-1;}
    else {
        controlsignal1=up1;}
    
    // To prevent windup
    ux1= up1-controlsignal1;
    ek1= Ka*ux1;
    ei1= motor1error-ek1;
     //Return
     return controlsignal1; 
}

float PID_controller2(float motor2error){
    static float error_integral2=0;
    static float e_prev2=motor2error;
    
    //Proportional part:
    u_p2=Kp*motor2error;
    
    //Integral part
    error_integral2=error_integral2+ei2*Ts;
    u_i2=Ki*error_integral2;
    
    //Derivative part
    float error_derivative2=(motor2error-e_prev2)/Ts;
    float u_d2=Kd*error_derivative2;
    e_prev2=motor2error;
    
    // Sum and limit
    up2= u_p2+u_i2+u_d2;
    if (up2>1){
        controlsignal2=1;}
    else if (up2<-1){
        controlsignal2=-1;}
    else {
        controlsignal2=up2;}
    
    // To prevent windup
    ux2= up2-controlsignal2;
    ek2= Ka*ux2;
    ei2= motor2error-ek2;
     //Return
     return controlsignal2; 
}


void readEncoder()
{
    counts1 = encoder1.getPulses();        
    deltaCounts1 = counts1 - countsPrev1;
    countsPrev1 = counts1;
    
    counts2 = encoder2.getPulses();
    deltaCounts2 = counts2 - countsPrev2;
    countsPrev2 = counts2;    
}

void togglehoek(){
    
    motor1ref= 0.5*pi2+motor1ref;
    motor2ref= 0.5*pi2+motor2ref;
    // static float t = 0;
    // refangle= pi2/3.0f*sin(5.0f*t)*motortoggle;
    //t+=0.01;
    }

void RKI(){
  
    Vy=potmeter1.read()*-10.0*motortoggle;
    //Vy=potmeter2.read()*10*motortoggle;
    //Vx=-1.0*;
    Vx=0.0;
    q1dot=(l2*cos(motor1ref+motor2ref)*Vx+l2*sin(motor1ref+motor2ref)*Vy)/((-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*l2*cos(motor1ref+motor2ref)+l2*sin(motor1ref+motor2ref)*(l1*cos(motor1ref)+l2*cos(motor1ref+motor2ref)));
    q2dot=((-l1*cos(motor1ref)-l2*cos(motor1ref+motor2ref))*Vx+(-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*Vy)/((-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*l2*cos(motor1ref+motor2ref)+l2*sin(motor1ref+motor2ref)*(l1*cos(motor1ref)+l2*cos(motor1ref+motor2ref)));
  
    motor1ref=motor1ref+q1dot*Ts;
    motor2ref=motor2ref+q2dot*Ts;
    }    
    
void motorControl()
{
    button1.fall(&togglehoek);
    RKI();
    motor1angle = counts1 * factorin / gearratio; // Angle of motor shaft in rad
    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
    motor1error=motor1ref-motor1angle;    
    controlsignal1=PID_controller1(motor1error);      
    if (controlsignal1<0){
        motordir1= 0;}
    else {
         motordir1= 1;}
    motor1Power.write(abs(controlsignal1));
    motor1Direction= motordir1;
    
    motor2angle = counts2 * factorin / gearratio; // Angle of motor shaft in rad
    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
    motor2error=motor2ref-motor2angle;    
    controlsignal2=PID_controller2(motor2error);      
    if (controlsignal2<0){
        motordir2= 0;}
    else {
         motordir2= 1;}
    motor2Power.write(abs(controlsignal2));
    motor2Direction= motordir2;    
}

void Plotje(){
    scope.set(0,motor1ref); //gewenste hoek
    scope.set(1,motor2ref);
    scope.set(2,motor1angle); //Gemeten hoek
    scope.set(3,motor1error); //verschil in gewenste en gemeten hoek

    scope.send(); //send what's in scope memory to PC
}

void toggleMotor()
{
    motortoggle = !motortoggle;
}

int main()
{
    pc.baud(115200);
    pc.printf("\r\nStarting...\r\n\r\n");
    
    motor1Power.period(PWM_period);
    motor2Power.period(PWM_period);
    motorTicker.attach(motorControl, 0.01);
    scopeTicker.attach(Plotje, 0.01);
    encoderTicker.attach(readEncoder, Ts);
    
    button2.fall(&toggleMotor);
    
    while (true) {
//        pc.printf("Angle1:  %f   Omega1:       %f\r\n", motor1angle, omega1);
        pc.printf("refangle1: %f   Error1:  %f     \r\n",motor1ref, motor1error);
//        pc.printf("Angle2:  %f   Omega2:       %f\r\n", motor2angle, omega2);
        pc.printf("refangle2: %f   Error2:  %f     \r\n",motor2ref, motor2error);
//        pc.printf("controlsignal2: %d \r\n", controlsignal2);
        pc.printf("Vx: %f Vy: %f \r\n",Vx,Vy);
        pc.printf("q1dot: %f q2dot: %f \r\n",q1dot,q2dot);
        
        wait(0.5);
    }
}