File content as of revision 10:61a7cb3b3aa3:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#define _USE_MATH_DEFINES
# define M_PI           3.14159265358979323846  /* pi */

double Kp = 1;
double Ki = 1;
double u_k = 1;
double u_i = 1;
double Kswitch;
double kp_scalar = 1;
double ki_scalar = 1;
double y_ref=0;
double motor1_ang=0;
double y_ref_scalar = 0;
double input_motor;
bool input_switch = true;
const double Ts = 0.01f;

Ticker Control;

PwmOut  motor1_pwm(D5);
PwmOut  motor2_pwm(D6);
DigitalOut motor1_dir(D4);
DigitalOut motor2_dir(D7);
AnalogIn pot1(A0);
AnalogIn pot2(A1);

QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING);
QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING);  

InterruptIn  sw2(SW2);
InterruptIn  sw3(SW3);

MODSERIAL pc(USBTX, USBRX);

//hoek uitrekenen van de motor
double countstoangle(int counts){
    double angle;
    int counts_abs =(counts);
    angle = (2 * M_PI * counts_abs)/8400;
    return angle;
    
}
void buttonpress()
{
    input_switch=!input_switch;             //verander de potmeter input tussen kp en ki
}

double PID_controller(double error)         //bereken de input die naar de motor wordt gestuurd aan de hand van kp en ki en de error op elk moment
            {
            //Proportional part
            u_k = Kp*error;
            
            //Integral part
            static double error_integral = 0;
            error_integral = error_integral + error * Ts;
            u_i = Ki * error_integral;
            return u_k + u_i;
            }


void aansturing()
    {       
            y_ref_scalar = pot2.read();     //altijd de potmeters uitlezen, buiten de ticker
            Kswitch = pot1.read();
            
            //kijken of er gekozen is voor het uitlezen van de kp of ki van de potmeter
            if(input_switch==true)
            {kp_scalar = Kswitch;}
            else
            {ki_scalar = Kswitch;}
            
            
            //waarde voor de inputcontrol value instellen aan de hand van potmeter
            Kp = kp_scalar*20.0f;
            Ki = ki_scalar*2.0f;
            
            y_ref = y_ref_scalar*2.0f*M_PI;                          // de postitie waar je naartoe wilt instellen tussen 0 en 2pi aan de hand van de potmeter waarde
            motor1_ang = countstoangle(motor1_enc.getPulses());      // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen
            double error = motor1_ang -y_ref;
            
            
            double input_motor = PID_controller(error);
            
            if(input_motor>1.0)                 //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden
                {input_motor = 1.0;}
            else if(input_motor<-1.0)           //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor
                {input_motor = -1.0;}
            
                    
            if(input_motor<0.0)                 //bepaal de richting aan de hand van het teken van het pwm signaal.
                {motor1_dir = 0;}
            else                                
                {motor1_dir = 1;}                      
            motor1_pwm.write(fabs(input_motor));    // de uiteindelijke input aan de motor geven 
 //   pc.printf("ki: %f  kp: %f  yref: %f     error: %f   u_k: %f  angle: %f\r\n",Ki, Kp, y_ref,error,u_k,motor1_ang);
    }

int main()
{
    
    sw2.fall(buttonpress);
    pc.baud(115200);
    pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n");
    
    motor1_pwm.period_us(60);
    motor1_pwm = 0;  
    motor2_pwm.period_us(60);
    motor2_pwm = 0;  
    
    Control.attach(aansturing, Ts);         //de aansturing regelen met een sampletime
    
    while (true)
    {       
            if(input_switch==true)
            {pc.printf("Kp ingeschakeld\r\n");}
            else
            {pc.printf("Ki ingeschakeld\r\n");}
            double errorprint = motor1_ang-y_ref;
                pc.printf("ki: %f   kp: %f  yref: %f    u_k: %f angle: %f   error: %f\r\n",Ki, Kp, y_ref,u_k,motor1_ang,errorprint);

            wait(0.5);  
    }
}