File content as of revision 0:67f1f06bdf17:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"


MODSERIAL pc(USBTX, USBRX);
DigitalOut motor_horizontal_dir(D7);   //Moet nog aangepast worden aan de motoren.
PwmOut motor_horizontal_pwm(D6);
DigitalOut motor_vertical_dir(D10);   //Moet nog aangepast worden aan de motoren.
PwmOut motor_vertical_pwm(D11);

QEI Encoder_hor(D13,D12,NC,8400);     //For the encoder
QEI Encoder_ver(*,*,NC,8400);
int Pulse_state_hor;
int Previous_pulse_state_hor;
double Pulse_destination_hor = 0;
int Pulse_state_ver;
int Previous_pulse_state_ver;
double Pulse_destination_ver = 0;

DigitalIn Button1(SW2);         //Buttons voor testen in plaats van elektrodes.
DigitalIn Button2(SW3);

float Elektrode_1;              //Niet nodig voor de test, input vanuit arm.
float Elektrode_2;
bool Elektrode_3;

float Wait_time = 0.02f;
float Sensitivity = 10;
bool Control_active;
int Arm_state = 1;

float P = 1;                  //Goede waardes vinden.
float I = 0.01;
float D = 0.05;
double Integrated_distance = 0;
double PID_value = 0;

Ticker Motor_control;      //Testen of er niks afgekapt wordt en voor een goede tijd.

void Control_activator{
    Control_active = true;
}

double Pid_controller(int Pulse_state,int Previous_pulse_state,double Pulse_destination) {
    Integrated_distance = Integrated_distance + (Pulse_destination - Pulse_state) * Wait_time;
    PID_value = (P * (Pulse_destination - Pulse_state) + I * Integrated_distance + D * (Previous_pulse_state - Pulse_state)/Wait_time)/500;
    return PID_value;
}

void setmotor1(double val){
    motor_horizontal_dir.write(val>=0?1:0);
    motor_horizontal_pwm.write(fabs(val)>1;1.0:fabs(val));
}

void setmotor2(double val){
    motor_vertical_dir.write(val>=0?1:0);
    motor_vertical_pwm.write(fabs(val)>1;1.0:fabs(val));
}

int main()
{
    pc.baud(115200);
    Motor_control.attach(&Control_activator,Wait_time);
    while (true) {
        Previous_pulse_state_hor = Pulse_state_hor;
        Pulse_state_hor =  Encoder_hor.getPulses();
        Previous_pulse_state_ver = Pulse_state_ver;
        Pulse_state_ver =  Encoder_ver.getPulses();
        if Control_active(){
            switch Arm_state{
                case "1"
                    
                    break
                case "2"
                    
                    break
                case "3"
                    
                    break
                case "4"
                    
                    break
            double PID_value_ver = PID_controller(Pulse_state_ver,Previous_pulse_state_ver,Pulse_destination_ver);
            setmotor2(PID_value_hor);
            double PID_value_hor = PID_controller(Pulse_state_hor,Previous_pulse_state_hor,Pulse_destination_hor);
            setmotor1(PID_value_hor);
            Control_active = false;
            }
        }
        wait(0.01f);
    }
}