File content as of revision 5:47262622a9bf:

#include <mbed.h>

#include "EncoderCounter.h"
#include "Controller.h"
#include "IRSensor.h"
#include "Motion.h"

//User Button
    InterruptIn button(USER_BUTTON);

//Sensors:
    
    AnalogIn lineSensor(PA_4);
    AnalogIn distance2(PC_3);
    AnalogIn distance4(PB_1);
    AnalogIn distance1(PC_2);
    AnalogIn distance3(PC_5);

    IRSensor irSensorL (distance2);
    IRSensor irSensorC (distance4);
    IRSensor irSensorR (distance1);
    IRSensor irSensorB (distance3);

//Motors:
    DigitalOut myled(LED1);
    
    DigitalOut enableMotorDriver(PB_2);
    
    DigitalIn motorDriverFault(PB_14);
    DigitalIn motorDriverWarning(PB_15);
    
    PwmOut pwmRight(PA_8);
    PwmOut pwmLeft(PA_10);
    
    EncoderCounter counterRight(PB_6, PB_7);
    EncoderCounter counterLeft(PA_0, PA_1);
    
    
    Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
    
    Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, enableMotorDriver);
    
//------------------------------------------------------------------------------
    
    volatile int start = 0;
    short countsLeft = 0;
    short countsRight = 0;
    short countsLeftOld = 0;
    short countsRightOld = 0;
    //den jeweiligen Test auf 1 setzen
    int testSensor = 0;
    int testMotion = 0;
    int testRotation = 0;
    int testCounts = 0;
    int testClass = 0;
    
//------------------------------------------------------------------------------

//User button toggle
void press() {
    start = !start;
}
    
    
//------------------------------------------------------------------------------

int main() {
    
/** 
    Micromouse Test 
*/
    
    
while(1) {
    
    button.fall(&press); //User button einlesen
    
    while(0) {
        myled = 1;
        printf("Hai\n");
        if (start == 0) break;  
    }
    


    /*
    Sensoren Test
    */
    while(1) {
        
        
        float distanceL = irSensorL.readL();
        float distanceC = irSensorC.readC();
        float distanceR = irSensorR.readR();
        float distanceB = irSensorB.readC();
        printf("Links: %f  Mitte: %f  Rechts: %f  Hinten: %f  Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
        
        //wait(0.5f);
    }
    
    /*
    Motoren Test: Gerade Bewegung
    */
    while(start == 1 && testMotion == 1) {
        
        controller.setDesiredSpeedLeft(20.0f); //Drehzahl in [rpm]
        controller.setDesiredSpeedRight(-20.0f);
        enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
                        
        countsLeft = counterLeft.read();
        countsRight = counterRight.read();
        
        printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
        
        if (start == 0) {
            
            motion.stop();
        }
    }
    
    /*
    Motoren Test: 180° Rotation
    */
        
    while(start == 1 && testRotation == 1) {
            
            
        while((countsLeft - countsLeftOld)  > -1614 || (countsRight - countsRightOld) > -1614) {
            
            countsLeft = counterLeft.read();
            countsRight = counterRight.read();
        
            controller.setDesiredSpeedLeft(-50.0f);
            controller.setDesiredSpeedRight(-50.0f);
            enableMotorDriver = 1;
        }
        
        motion.stop();
        enableMotorDriver = 0;
        printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
        
        if (start == 0) {
           countsLeftOld = countsLeft - 100;
           countsRightOld = countsRight - 100;
            
            break;
        }
    }


/* Benötigte Ticks für 180* Rotation am Ort:

    mm/count = 0.122
    Dr = 124mm
    Rr = 60mm
    180°=> 1597 counts
    90°=> 798 counts
    
        
    RotationsDistanz Rd= DistanzRäder*pi/2 = 194.8
    
    UmfangRad = Drad*pi = 188.5
    
    
    Counts = Rd/mm pro Count
    
    => Counts pro Drehung rausfinden */
    while(start == 1 && testCounts == 1) {
        
        while(countsLeft  < 5000 || countsRight > -5000) {
            
            countsLeft = counterLeft.read();
            countsRight = counterRight.read();
        
            controller.setDesiredSpeedLeft(20.0f);
            controller.setDesiredSpeedRight(-20.0f);
            enableMotorDriver = 1;
        }
        
        controller.setDesiredSpeedLeft(0.0f);
        controller.setDesiredSpeedRight(0.0f); 
        enableMotorDriver = 0;
        
    }
    
    //Class test
    if(start == 1 && testClass == 1) {
        motion.rotate180();
        counterLeft.reset();
        counterRight.reset();
        start = 0;
        }
        
    //Sensor calib
    if(start == 1) {
        int i;
        myled = !myled;
        for (i=0;i<5;i++) {
            float distanceL = irSensorL.readL();
            float distanceC = irSensorC.readC();
            float distanceR = irSensorR.readR();
            float distanceB = irSensorB.readC();
            printf("Links: %f  Mitte: %f  Rechts: %f  Hinten: %f  Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
       }
        printf("------------------------\n");
        start = 0;
        
       /* while((countsLeft - countsLeftOld) > -82 || (countsRight - countsRightOld) < 82) {
        
        controller.setDesiredSpeedLeft(-10.0f); //Drehzahl in [rpm]
        controller.setDesiredSpeedRight(10.0f);
        enableMotorDriver = 1; //Schaltet  den Leistungstreiber ein
                        
        countsLeft = counterLeft.read();
        countsRight = counterRight.read();
        }
        
        controller.setDesiredSpeedLeft(0.0f);
        controller.setDesiredSpeedRight(0.0f); 
        enableMotorDriver = 0;
        
        float distanceL = irSensorB.read();
        printf("Hinten: %f\n", distanceL);
        
        countsLeftOld = countsLeft;
        countsRightOld = countsRight;
        start = 0;*/
        
    }
}
}