File content as of revision 6:cc3e47c7aac2:

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

#include "motor.h"

const float pid_period = 0.001; // PID sample period in seconds.

const double Kp = 10.0;
const double Ki = 0.1;
const double Kd = 0.5;

AnalogIn potmeter1(A1); // Analoge input van potmeter 1 -> Motor 1
AnalogIn potmeter2(A2); // Analoge input van potmeter 2 -> Motor 2
Serial pc(USBTX, USBRX);

Motor motor1(D6, D7, D13, D12, &pc);
Motor motor2(D5, D4, D10, D11, &pc);


// Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1]
double normalize_pot(double pot_value) {
    // scales value potmeter from 0-1 to -1 - 1.
    return pot_value * 2 - 1;
};


int main()
{
    pc.baud(115200);
    pc.printf("Starting.");
    motor1.set_pid_k_values(Kp, Ki, Kd);
    motor2.set_pid_k_values(Kp, Ki, Kd);
    // Start the motor controller at the desired frequency.
    motor1.start(pid_period);
    motor2.start(pid_period);
    
    while(true){
        // reads out value potmeter 1 between 0-1
        double pot = potmeter1.read();
        double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
        // Update the motor controller with the new angle.
        motor1.set_target_angle(desired_angle);
        
        // reads out value potmeter 1 between 0-1
        pot = potmeter2.read();
        desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
        // Update the motor controller with the new angle.
        motor2.set_target_angle(desired_angle);
        
        wait(0.1);
    } //Lege while loop zodat functie niet afloopt
}