File content as of revision 11:4b0dbbdc56fb:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#define _USE_MATH_DEFINES
# define M_PI           3.14159265358979323846  /* pi */
//motor 1
double Kp = 5;
double Ki = 1;
double u_k = 1;
double u_i = 1;
double Kswitch;
double kp_scalar = 1;
double ki_scalar = 1;
double m1_scalar;
double motor1_ang=0;
double m1=0;
double input_motor;
//motor 2
double Kp2 = 10;
double Ki2 = 1;
double u_k2 = 1;
double u_i2 = 1;
double Kswitch2;
double kp_scalar2 = 1;
double ki_scalar2 = 1;
double m2_scalar;
double motor2_ang=0;
double m2=0;
double input_motor2;

//rest
bool input_switch = true;
const double Ts = 0.01f;
volatile char input;

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_controller1(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;
            }

double PID_controller2(double error2)         //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_k2 = Kp2*error2;
            
            //Integral part
            //static double error_integral2 = 0;
            //error_integral2 = error_integral2 + error2 * Ts;
            //u_i2 = Ki2 * error_integral2;
            return u_k2; //+ u_i2;
            }
            
void aansturing()
    {       //posities voor de motoren bepalen   
//            if(input=='a')
//               {if(m1>0.1f)
//                {m1 = m1-0.01f;}}
//            else if(input=='s')
//                {if(m1<2.1f)
//                {m1 = m1+0.01f;}}
//            else if(input=='q')
//                {if(m2>0.01f)
//                {m2 = m2-0.01f;}}
//            else if(input=='w')
//                {if(m2<2.1f)
//                {m2 = m2+0.01f;}}
            m1_scalar=pot1.read();
            m2_scalar=pot2.read();
            m1=m1_scalar*1.5f;
            m2=m2_scalar*1.5f;
            //Kswitch = pot1.read();      //kp en ki bepalen voor motor 1
            //Kswitch2 = pot2.read();     //kp en ki bepalen voor motor 2
            
            //kp_scalar=Kswitch;
            //kp_scalar2=Kswitch2;
//            //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;}
//            
//            if(input_switch==true)
//            {kp_scalar2 = Kswitch2;}
//            else
//            {ki_scalar2 = Kswitch2;}
            
            //waarde voor de inputcontrol value instellen aan de hand van potmeter
           // Kp = kp_scalar*20.0f;
           // Ki = ki_scalar*2.0f;
           // Kp2 = kp_scalar2*20.0f;
           // Ki2 = ki_scalar2*2.0f;
            
            
           
            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 -m1;
            
            motor2_ang = countstoangle(motor2_enc.getPulses());      // de hoek van de motor uit bovenstaande formule halen, om de error te kunnen berekenen
            double error2 = motor2_ang -m2;
            
            //input voor de motor met de PID controllers bepalen
            double input_motor = PID_controller1(error);
            double input_motor2 = PID_controller2(error2);
            
            //signaal naar motor 1 sturen
            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 
            
            //signaal naar motor 12 sturen
            if(input_motor2>1.0)                 //pwm signaal kan niet groter zijn dan 1, dus moet gelimiteerd worden
                {input_motor2 = 1.0;}
            else if(input_motor2<-1.0)           //absolute waarde wordt ingevuld, maar de negatieve kant is belangrijk voor de richting van de motor
                {input_motor2 = -1.0;}
            
                    
            if(input_motor2<0.0)                 //bepaal de richting aan de hand van het teken van het pwm signaal.
                {motor2_dir = 0;}
            else                                
                {motor2_dir = 1;}                      
            motor2_pwm.write(fabs(input_motor2));    // 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)
    {       
            //input = pc.getc();
            if(input_switch==true)
            {pc.printf("Kp ingeschakeld\r\n");}
            else
            {pc.printf("Ki ingeschakeld\r\n");}
            
            double errorprint = motor1_ang-m1;
            double errorprint2 = motor2_ang-m2;
            pc.printf("ki1: %f   kp1: %f  m1: %f   u_k1: %f  angle1: %f   error1: %f\r\n",Ki, Kp, m1,u_k,motor1_ang,errorprint);
            pc.printf("ki2: %f   kp2: %f  m2: %f   u_k2: %f  angle2: %f   error2: %f\r\n",Ki2, Kp2, m2,u_k2,motor2_ang,errorprint2);
            pc.printf("toets=%c\r\n",input);
            wait(0.5);  
    }
}