File content as of revision 10:b2742f42de44:

#include "mbed.h"
#include "MODSERIAL.h"
#include "encoder.h"
#include "HIDScope.h"

Serial pc(USBTX,USBRX);
HIDScope scope(2);

Ticker          mod;

//MOTOR
// motor 1 en 2 zijn omgedraait
// pinnen motor 1 -> pinnen motor 2

//motor 1 gegevens
PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
DigitalOut motor1_rich(D7); // digitaal signaal voor richting 
// einde motor 1

//motor 2 gegevens
PwmOut motor2_aan(D5);      // PWM signaal motor 1 (uit sheets)
DigitalOut motor2_rich(D4); // digitaal signaal voor richting 
// einde motor 1
//EINDE MOTOR

// ENCODER
Encoder motor1_enc(D12,D11);
Encoder motor2_enc(D10,D9);


//POTMETERS
AnalogIn potleft(A1);
AnalogIn potright(A0);

DigitalIn button(PTA4);
int button_on = 0;


void move_mot1(double left)
{
    if(left < 0.4)
    {
        double calc1 = left - 1;
        double calc2 = abs(calc1);
        double leftin1 = (calc2-0.6)*2.5 ;
        motor1_aan.write(leftin1);
        motor1_rich.write(0);
    }
    else if(left > 0.4 && left < 0.6)
    {
        motor1_aan.write(0);
    }
    else if(left > 0.6)
    {
        double leftin2 = (left-0.6)*2.5;
        motor1_aan.write(leftin2);
        motor1_rich.write(1);
    }
}

void move_mot2(double right)
{
    if(right < 0.4)
    { 
        double calc3 = right - 1;
        double calc4 = abs(calc3);
        double rightin1 = (calc4-0.6)*2.5 ;
        motor2_aan.write(rightin1);
        motor2_rich.write(0);
    }
    else if(right > 0.4 && right < 0.6)
    {
        motor2_aan.write(0);
    }
    else if(right > 0.6)
    {
        double rightin2 = (right-0.6)*2.5;
        motor2_aan.write(rightin2);
        motor2_rich.write(1);
    }
}  

void send()
{
    int counts1 = motor1_enc.getPosition();
    int counts2 = motor2_enc.getPosition();
    pc.printf("motor 1 counts, %d motor 2 counts %d \n", counts1, counts2);

    }
    
double get_radians(double counts)
{
   double pi = 3.14159265359;
   double radians = (counts/4200)*2*pi;
   return radians;   
}



int main()
{
    pc.baud(115200);
    mod.attach(&send,1);
    while(true)
    {
        double left = potleft.read();
        double right = potright.read();        
        move_mot1(left);
        move_mot2(right);
        int counts1 = motor1_enc.getPosition();
        int counts2 = motor2_enc.getPosition();
        double radians_1 = get_radians(counts1);
        double radians_2 = get_radians(counts2); 
        
        if( button.read() == button_on)
        {
            motor1_enc.setPosition(0);
            motor2_enc.setPosition(0);
        }
        scope.set(0,radians_1);     
        scope.set(1,radians_2);     
        scope.send();
        wait(0.01f);            // is noodzakelijk voor functioneren gradual increase, waarom????
    
    }
}