File content as of revision 2:725188195139:

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

MODSERIAL pc(USBTX, USBRX);

//Pinnen voor spieren
AnalogIn Spier1 (A0);
AnalogIn Spier2 (A1);
InterruptIn Spier3 (D2);

//Pinnen voor motor
DigitalOut motor1direct (D4);
DigitalOut motor2direct (D7);
PwmOut motor1pwm (D5);
PwmOut motor2pwm (D6);
//QEI Encoder1(D10, D11, NC, 32); // met encoder onthouden waar je bent
//QEI Encoder2(D12, D13, NC, 32); // met encoder onthouden waar je bent

//Pin voor de servo
PwmOut servo(D9);

//Define variables
volatile int indrukken = 0;
volatile int i = 0;
volatile float MV = 0;
//const float maxVelocity = 8.4;
//const float MotorGain=8.4;


//Als straks de kalibratie af is, dan moet Spier1 > 0.4 en Spier2 >0.4 nog aangepast worden met boven threshold of niet

//Motorvalue is een waarde tussen -1 en 1 waarmee de motor een richting en een snelheid krijgt
float GetMotorValue() //We nemen aan dat je maar één spier tegelijkertijd kan aanspannen
{
    pc.printf("\n\n\n");
    if(Spier1 > 0.4f && Spier2 < 0.4f) //Spier komt boven de threshold uit en spier2 niet
    {
        MV = 0.5;
        pc.printf("Spier 1 is aangespannen\r\n");
    }
    else if(Spier1 < 0.4f && Spier2 > 0.4f)
    {
        MV = -0.5;
        pc.printf("Spier 2 is aangespannen\r\n");
    }
    else if(Spier1 <0.4f && Spier2 <0.4f)
    {
        MV = 0;
        pc.printf("Geen spier is aangespannen\r\n");
    }
    else
    {
        MV = 0;
        pc.printf("Beide spieren zijn aangespannen\r\n");
    }
    pc.printf("de motorvalue is %f\n\n\r",MV);
    return MV;
}

//Aan de hand van de motorvalue wordt de motor aangezet
void SetMotor1(float MV)
{
    //Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    if (MV >=0)
        {
            motor1direct = 1;
            pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in
        }
    else
        {
            motor1direct = 0;
            pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee
        }
    if (fabs(MV)>1) 
    {
        motor1pwm = 1;
        pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1
    }
    else
    {
        motor1pwm = fabs(MV);
        pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue
    }
}

void SetMotor2(float MV)
{
    //Given -1<=motorValue<=1, this sets the PWM and direction
    // bits for motor 1. Positive value makes motor rotating
    // clockwise. motorValues outside range are truncated to
    // within range
    if (MV >=0)
        {
            motor2direct = 1;
            pc.printf("motordirect=1\n\r"); //De motor draait in positieve richting, tegen de klok in
        }
    else
        {
            motor2direct = 0;
            pc.printf("motordirect=0\n\r"); //De motor draait in negatieve richting, met de klok mee
        }
    if (fabs(MV)>1) 
    {
        motor2pwm = 1;
        pc.printf("motorpwm = 1\n\r"); //De snelheid waarmee de motor draait is maximaal, dus 1
    }
    else
    {
        motor2pwm = fabs(MV);
        pc.printf("motorpwm = %f\n\n\r",fabs(MV)); //De snelheid waarmee de motor draait is de absolute waarde van de motorvalue
    }
}

void SetMotor3(float MV)
{
    if (MV > 0)
        {
            servo = 0.04;
            pc.printf("servo = 0.01\n\r"); //De servo draait maximaal rechtsom de klep dus open
        }
    else if (MV = 0)
        {
            servo = servo;
            pc.printf("servo = servo\n\r");
        }
    else (MV < 0)
        {
            servo = 0.08;
            pc.printf("servo = 0.15\n\r");
        }
}

void MeasureAndControl()
{
    float MV = GetMotorValue();
    switch(i)
    {
        case 0:
            pc.printf("Motor 1\n\r");
            SetMotor1(MV);
            SetMotor2(0);
            //SetMotor3(0);
            break;
        case 1:
            pc.printf("Motor 2\n\r");
            SetMotor1(0);
            SetMotor2(MV);
            //SetMotor3(0);
            break;
        case (2):
            pc.printf("Motor 3\n\r");
            SetMotor1(0);
            SetMotor2(0);
            //SetMotor3(MV);
            break;
    }
}

void count ()
{
    indrukken ++;
    pc.printf("Het knopje is %i x ingedrukt\n\r",indrukken);
    i = indrukken%3;
}

int main()
{
    //motorpwm.period(1.0/1000.0);
    pc.baud(115200);
    pc.printf("START\n\r");
    Ticker motordraaien;
    motordraaien.attach(MeasureAndControl,2);
    Spier3.fall(count);
    servo.period(0.020) //servo periode 20ms
    while(true){}  
}