File content as of revision 7:be97dcdc1b8c:

#include "mbed.h"
#include "math.h"
//#include "HIDScope.h"
// ---- Alles met een 1(of geen getal) gaat over motor 1----
// ---- Alles met een 2 gaat over motor 2---- 
// ------ Hardware Interfaces -------
 
const PinName motor1dir = D7; //direction1 (cw,ccw)
const PinName motor1PWM = D6; //speed1
const PinName motor2PWM = D5; //speed2
const PinName motor2dir = D4; //direction2 (cw,ccw)
 
DigitalOut motor1direction(motor1dir);
PwmOut motor1control(motor1PWM);
PwmOut motor2control(motor2PWM);
DigitalOut motor2direction(motor2dir);
 
const PinName button1name = D10; 
const PinName pot1name = A0;
const PinName button2name = D11;
const PinName pot2name = A1;
InterruptIn button1(button1name);
AnalogIn potMeterIn1(pot1name);
InterruptIn button2(button2name);
AnalogIn potMeterIn2(pot2name);
 
// ------- Objects used -------
 
// Ticker which controls the mother every 1/100 of a second.
Ticker controlTicker;
 
Ticker debugTicker;
Serial pc(USBTX, USBRX);
 
//HIDScope scope(2); //fuck you hidscope we're not using you!
 
 
// ------- Constants
 
const float motorGain = 8.4f;
const float maxVelocity = 8.4f; //radians per second
const bool clockwise = true;
 
 
// ------ variables
 
volatile bool direction1 = clockwise;
volatile bool direction2 = clockwise;
 
// ------ functions
 
float getReferenceVelocity() {
    // Returns reference velocity in rad/s. 
    return maxVelocity * potMeterIn1 ;
    }
float getReferenceVelocity2() {
    // Returns reference velocity in rad/s. 
    return maxVelocity * potMeterIn2;    
}
 
void setMotor1(float motorValue1) {
    // Given motorValue1<=1, writes the velocity to the pwm control.
    // MotorValues outside range are truncated to within range.
    motor1control.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
}

void setMotor2(float motorValue2) {
    // Given motorValue2<=1, writes the velocity to the pwm control.
    // MotorValues outside range are truncated to within range.
    motor2control.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
}
 
float feedForwardControl(float referenceVelocity) {
    // very simple linear feed-forward control
    // returns motorValue
    return referenceVelocity / motorGain;
}

float feedForwardControl2(float referenceVelocity2) {
    // very simple linear feed-forward control
    // returns motorValue
    return referenceVelocity2 / motorGain;
}
 
void measureAndControl(void) {
    // This function measures the potmeter position, extracts a
    // reference velocity from it, and controls the motor with 
    // a simple FeedForward controller. Call this from a Ticker.
    float referenceVelocity = getReferenceVelocity();
    float motorValue1 = feedForwardControl(referenceVelocity);
    setMotor1(motorValue1);

    float referenceVelocity2 = getReferenceVelocity2();
    float motorValue2 = feedForwardControl2(referenceVelocity2);
    setMotor2(motorValue2);
} 

void onButtonPress() {
    // reverses the direction
    motor1direction.write(direction1 = !direction1);
    pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
}

void onButtonPress2() {
    // reverses the direction
    motor2direction.write(direction2 = !direction2);
    pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
} 
void onDebugTick() {
    
    pc.printf("pot input1: %f\r\n", potMeterIn1.read());
    pc.printf("motorValue1: %f\r\n", feedForwardControl(getReferenceVelocity()));
    pc.printf("\n\n\n");
    
    /*
    scope.set(0, potMeterIn.read());
    scope.set(1, feedForwardControl(getReferenceVelocity()));
    scope.send();
    */
    pc.printf("pot input2: %f\r\n", potMeterIn2.read());
    pc.printf("motorValue2: %f\r\n", feedForwardControl2(getReferenceVelocity2()));
    pc.printf("\n\n\n");
    }
    
int main()
{
    pc.baud(115200);
    
    button2.fall(&onButtonPress2);
    controlTicker.attach(&measureAndControl, 1.0f/100.0f);
    debugTicker.attach(&onDebugTick, 1);
    
    button1.fall(&onButtonPress);
    
}