File content as of revision 2:684d5cf2f683:

#include "mbed.h"
#include "QEI.h"
#include "PID.h"
#include "ReceiverIR.h"
#include "Servo.h"

ReceiverIR ir_rx(D2);

AnalogIn PotMeter1(A0);
AnalogIn PotMeter2(A1);
InterruptIn Button(PTA4);
 
PwmOut MotorThrottle1(D5);
PwmOut MotorThrottle2(D6);
DigitalOut MotorDirection1(D4);
DigitalOut MotorDirection2(D7);
DigitalOut servo(D3);

DigitalIn EndSwitch1(D9);
DigitalIn EndSwitch2(D8);

//DigitalOut Magneet(D7);


QEI EncoderMotor1(D12,D13,NC,4200);
QEI EncoderMotor2(D10,D11,NC,4200);

Ticker ControlTicker;
Ticker GetRefTicker;

float Interval=0.02f;
float pi=3.14159268;
PID controller1(20.0f,0.0f,0.001f, Interval);
PID controller2(20.0f,0.0f,0.002f, Interval);

RemoteIR::Format format;
uint8_t buf[4];

Serial pc(PTB17,PTB16);

float PosRef1=1.0f;
float PosRef2=1.0f;




void HomingLoop(){

    EndSwitch1.mode(PullUp);
    EndSwitch2.mode(PullUp);
    
    MotorThrottle1=0.1f;
    MotorThrottle2=0.2f;
    
    MotorDirection1=1;
    MotorDirection2=1;
    
    bool dummy1=0;
    bool dummy2=0;
    
    while(EndSwitch1.read()|EndSwitch2.read()){
        if((EndSwitch1.read()==0)&&(dummy1==0)){
            MotorThrottle1=0.0f;
            dummy1=1;
        }
        if((EndSwitch2.read()==0)&&(dummy2==0)){
            MotorThrottle2=0.0f;
            dummy2=1;
        }
    }
    MotorThrottle1=0.0f;
    MotorThrottle2=0.0f;
    EncoderMotor1.reset();
    EncoderMotor2.reset();
}

void ControlLoop(){
    
    float Pos1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
    float Pos2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
    float error1 = PosRef1 - Pos1;
    float error2 = PosRef2 - Pos2;
    
    // PID input
    controller1.setSetPoint(PosRef1);
    controller2.setSetPoint(PosRef2);
    
    controller1.setProcessValue(Pos1);
    controller2.setProcessValue(Pos2);
    
    // PID output
    float OutPut1=controller1.compute();
    float OutPut2=controller2.compute();

    // Direction handling
    float Direction1=0;
    float Direction2=0;
    
    if(OutPut1<0){
        Direction1=1;
    }
    if(OutPut2<0){
        Direction2=1;
    }
    
    OutPut1=fabs(OutPut1);
    OutPut2=fabs(OutPut2);
    
    // Output schrijven
    MotorThrottle1.write(OutPut1);
    MotorThrottle2.write(OutPut2);
    MotorDirection1=Direction1;
    MotorDirection2=Direction2;
}


void PickUp(){

}
    
void LayDown(){
    
    
    }


void RemoteController(){
    
   int bitcount;


   bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
    int state = buf[2];
    switch(state)
    {
        case 22: //1
            printf("1\n\r");
        break;
        case 25: //1
            printf("2\n\r");
            break;
        case 13: //1                
            printf("3\n\r");
            break;
        case 12: //1
            printf("4\n\r");
            break;
        case 24: //1
            printf("5\n\r");
            ControlTicker.detach();
            MotorThrottle1=0.0f;
            MotorThrottle2=0.0f;
            PickUp();
            ControlTicker.attach(&ControlLoop, Interval);
            break;
        case 94: //1
            printf("6\n\r");
            break;
        case 8: //1
            printf("7\n\r");
            break;
        case 28: //1
            printf("8\n\r");
            ControlTicker.detach();
            MotorThrottle1=0.0f;
            MotorThrottle2=0.0f;
            LayDown();
            ControlTicker.attach(&ControlLoop, Interval);
            break;
        case 90: //1
            printf("9\n\r");
            break;
        case 70: //1
            pc.printf("Boven\n\r");
            PosRef2=PosRef2-0.1f;
            pc.printf("%f\n\r", PosRef2);
            break;
        case 21: //1
            pc.printf("Onder\n\r");
            PosRef2=PosRef2+0.1f;  
            pc.printf("%f\n\r", PosRef2);
            break;
        case 68: //1
            pc.printf("Links\n\r");
            PosRef1=PosRef1+0.1f;
            pc.printf("%f\n\r", PosRef1);
            break;
        case 67: //1
            pc.printf("Rechts\n\r");
            PosRef1=PosRef1-0.1f;
            pc.printf("%f\n\r", PosRef1);
            break;
        case 64: //1
            pc.printf("OK\n\r");
            ControlTicker.detach();
            MotorThrottle1=0.0f;
            MotorThrottle2=0.0f;
            HomingLoop();
            ControlTicker.attach(&ControlLoop, Interval);
            
            break;
        default:
            break;
    }
}




void DeterminePosRef(){
    PosRef1=2*pi*PotMeter1.read(); // Reference in Rad
    PosRef2=2*pi*PotMeter2.read(); // Reference in Rad
}

int main() {
    pc.baud(115200);
    pc.printf("startup");
    
    controller1.setInputLimits(0.5f,2.0f*pi);
    controller2.setInputLimits(0.5f,2.0f*pi);
    controller1.setOutputLimits(-0.15f,0.15f);
    controller2.setOutputLimits(-0.5f,0.5f);  
    
    
    HomingLoop();
    
    
    ControlTicker.attach(&ControlLoop, Interval);
    //GetRefTicker.attach(&DeterminePosRef, 0.1f);
    

    
    while(1)
    { 
        if (ir_rx.getState() == ReceiverIR::Received)
        {

            pc.printf("received");
            RemoteController();
            wait(0.01);
        }
    }       
    
    
    while(1){}


}