File content as of revision 2:6ca30ccec353:

#include "mbed.h"
#include "math.h"
#include "encoder.h"
#include "QEI.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);    //had ook direct gekund maar he
AnalogIn potMeterIn1(pot1name);
InterruptIn button2(button2name);
AnalogIn potMeterIn2(pot2name);

QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING);           //64 = ..., x4 encoding because ...
QEI Encoder2(D9,D8,NC,64,QEI::X4_ENCODING); 
// ------- 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 = 8f;         //just to be sure, niet te veel input geven tegen windup (old motor, suboptimal perfomance)
const float maxVelocity = 8f; //radians per second
const bool clockwise = true;
const float rad_count = 0.0007479;   // 2pi/8400;
const float pi = 3.1415926;
 
// ------ 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");
} 

float Encoding()
{
    // Encoding is necessary for the computations of the joint angles and 
    // velocities of the arm given certain linear velocities. 
 
    double counts1 = Encoder1.getPulses();
    double rad_m1 = (rad_count * (double)counts1) + (1.0f*pi/3.0f);
    return rad_m1;
} 

float Encoding2()
{
    // Encoding is necessary for the computations of the joint angles and 
    // velocities of the arm given certain linear velocities. 
 
    double counts2 = Encoder2.getPulses();
    double rad_m2 = (rad_count * (double)counts2) + (7.0f*pi/4.0f);
    return rad_m2;
} 

   void onDebugTick() {
    
    pc.printf("pot input1: %f\r\n", potMeterIn1.read());
    pc.printf("motorValue1: %f\r\n", feedForwardControl(getReferenceVelocity()));
    pc.printf("radialen motor 1: %f\r\n",Encoding());

    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("radialen motor 2: %f\r\n",Encoding2());
    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);
    
}