File content as of revision 2:0954eb04bb4c:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#include "HIDScope.h"
#include "Biquad.h"
 
DigitalOut green(LED_GREEN);
MODSERIAL pc(USBTX, USBRX);
///////////////////////////
// Hardware initialiseren//
///////////////////////////
//------------------------------------------------------------
//--------------------All sensors-----------------------------
//------------------------------------------------------------
//--------------------Encoders--------------------------------

QEI Encoder_L (D11, D10,NC, 32);  //D11 is poort A D10 is poort b
QEI Encoder_R (D13, D12,NC, 32); //D 13 is poort A 12 is poort b

double AnglePerPulse = 11.25;
double Position_L = 0.0;
double Position_R = 0.0;
double Previous_Position_L = 0.0;
double Previous_Position_R = 0.0;
//--------------------Analog---------------------------------
AnalogIn P_M_L(A0); //Motorspeed Left control
AnalogIn P_M_R(A1); //MotorSpeed Right control

//-------------------Hidscope----------------------------------
HIDScope scope(2); // Sending two sets of data

//-------------------Interrupts-------------------------------
InterruptIn Dir_L(D0); //Motor Dirrection left
InterruptIn Dir_R(D1); //Motor Dirrection Right
InterruptIn OnOff(SW3); //Motor On/off


//------------------Tickers------------------------------------

Ticker Measure; // ticker for data mesuaring
//------------------------------------------------------------
//--------------------All Motors----------------------------
//------------------------------------------------------------

//-------------------- Motor Links----------------------------
DigitalOut M_L_D(D4); //Richting motor links
PwmOut     M_L_S(D5); //Speed motor links
DigitalOut M_R_D(D7); //Richting motor Rechts
PwmOut     M_R_S(D6); //Speed motor Rechts

//--------------------------------------------------------------
//-----------------------Functions------------------------------
//--------------------------------------------------------------

//-----------------------Interrupts-----------------------------
volatile int Motor_Frequency = 1000;
volatile bool Active = false;
volatile float Speed_L= 0.0;
volatile float Speed_R= 0.0;

void Boot(){
  
    M_L_S.period(1.0/Motor_Frequency);
    M_L_D = 1;
    M_L_S = 0.0;
    M_R_S.period(1.0/Motor_Frequency);
    M_R_D = 0;
    M_R_S= 0.0;
    
    Encoder_L.reset();
    Encoder_R.reset();
    }
void Switch_Dirr_L(){ // Switching dirrection left motor (Every motor starts in clockwise dirrection)
     M_L_D = !M_L_D;
    }
    
void Switch_Dirr_R(){ // Switching dirrection Right motor
    M_R_D = !M_R_D;
    }
void De_Activate(){
    Active = !Active;
    }
    
void Information(){
    
//---------------------Motor Data-----------------------------------------------
//Position in Radials:
    Previous_Position_L = Position_L;
    Previous_Position_R = Position_R;
    Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
    Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;

//Speed in RadPers second
    Motor_Speed_L = Previous_Position_L - Position_R
//---------------------Sending data---------------------------------------------
    scope.set(0,M_L_S); //Sending motor left speed
    scope.set(1,M_R_S); //sending random value
    scope.send();
    }
//-----------------------Encoders-----------------------------------------------
int Get_Phase_L(int Phase){
    Phase = Encoder_L.getCurrentState();    
    return Phase;
    }
int main()
{
//---------------------Setting constants and system booting--------------------
    pc.baud(115200);
    pc.printf("\r\n**BOARD RESET**\r\n");
    Boot();

    
    Dir_L.fall(Switch_Dirr_L);
    Dir_R.fall(Switch_Dirr_R); 
    OnOff.fall(De_Activate);
    Measure.attach(Information, 0.01);     
    while (true) {  
       // control= pc.getc();  
       if (Active == true){
            green = 0;
            Speed_L =  P_M_L.read();//5.0+0.1;
            M_L_S = Speed_L/5+0.1f;
            Speed_R = P_M_R.read();//5.0+0.1;
            M_R_S = Speed_R/5+0.1f ;
        } else {
            if(Active == false){
            green = 1;
            M_L_S = 0;
            M_R_S = 0;
                }
        }       
    }
}