File content as of revision 20:9f4ba1b3d06e:

#include "mbed.h"


PwmOut motorleftspeed(D6);
PwmOut motorrightspeed(D7);
DigitalOut MotorL1pin (D12);
DigitalOut MotorL2pin (D11);
DigitalOut MotorR1pin (D10);
DigitalOut MotorR2pin (D9);
AnalogIn Sensor1 (A1);
AnalogIn Sensor2 (A2);
AnalogIn Sensor3 (A3);
AnalogIn Sensor4 (A4);
AnalogIn zoeksensor (A5);
AnalogIn currentleftpin (A6);
AnalogIn currentrightpin (A7);
DigitalOut steppera (D2);
DigitalOut stepperb (D3);
DigitalOut stepperc (D4);
DigitalOut stepperd (D5);
DigitalIn knop(D8);
DigitalOut led(D1);


int afstand = zoeksensor;

int stapmode = 0;
int position = 0;
int pos;

bool robotrun;
int radarspeedsetleft;
int radarspeedsetright;


/*
******* main loop ********************************
hier maakt de robot de beslissingen wat hij moet doen in bepaalde situaties
dit staat in het programmaoverzicht beschreven


*/
int main ()
{
    while(robotrun == 1);
    {
        if(lijnsensor > 0) {
            switch(lijnsensor) {
                case 1: {
                    Hbrug(255, 128, 2);
                    break;
                }
                case 2: {
                    Hbrug(128, 255, 2);
                    break;
                }
                case 3: {
                    Hbrug(255, 255, 2);
                    break;
                }
                case 4: {
                    Hbrug(255, 128, 1);
                    break;
                }
                case 8: {
                    Hbrug(128, 255, 1);
                    break;
                }
                case 12: {
                    Hbrug(255, 255, 1);
                    break;
                }
                wait_ms(200);
            }
            else {
                if (lidardirection < 90) {
                    radarspeedsetleft = 255;
                    radarspeedsetright = 255-(lidardirection/90)*255;
                    Hbrug(radarspeedsetleft, radarspeedsetright, 1);
                }
                else if (lidardirection > 270) {
                    radarspeedsetleft = 255;
                    radarspeedsetright = ((lidardirection-270)/90)*255;
                    Hbrug(radarspeedsetleft, radarspeedsetright, 1);
                }
                else if (lidardirection > 270) {
                    radarspeedsetleft = 255;
                    radarspeedsetright = ((lidardirection-270)/90)*255;
                    Hbrug(radarspeedsetleft, radarspeedsetright, 1);
                }
                else if (lidardirection > 270) {
                    radarspeedsetleft = 255;
                    radarspeedsetright = ((lidardirection-270)/90)*255;
                    Hbrug(radarspeedsetleft, radarspeedsetright, 1);
                }
            }
        }
    }

}
int mapcurrent(float input = 0.00,float inputmin = 0.00, float inputmax = 0.00,int outputmin = 0, int outputmax = 0)
{
    return (input - inputmin) * (outputmax - outputmin) / (inputmax - inputmin) + outputmin;
}
// poar neemn
// twee poar neemn
// twee tettn in n envelop
//neuken tot de vellen er bij hangen
/*
******* radar direction ********************************
deze functie bepaalt de hoek waarin een andere robot is gedetecteerd
deze functie geeft een waarde terug van 0 tot 360

*/
int lidardirection()
{
    bool objectfound;
    int startangle;
    int stopangle;
    int outputangle;
    if (afstandzoeker == 1&&objectfound == 0) {
        objectfound = 1;
        int startangle = pos;

    }
    if (afstandzoeker == 0&&objectfound == 1) {
        objectfound = 0;
        stopangle = pos;
        outputangle = startangle + (stopangle-startangle/2)
    }
                  return outputangle;
}


int stepper()
{
    switch (stapmode) {
        case 0:
            steppera = 1;
            stepperb = 0;
            stepperc = 1;
            stepperd = 0;
            pos ++;
            break;
        case 1:
            steppera = 1;
            stepperb = 0;
            stepperc = 0;
            stepperd = 1;
            pos ++;
            break;
        case 2:
            steppera = 0;
            stepperb = 1;
            stepperc = 0;
            stepperd = 1;
            pos ++;
            break;
        case 3:
            steppera = 0;
            stepperb = 1;
            stepperc = 1;
            stepperd = 0;
            pos ++;
            break;
            ;
    }
    if (pos > 1000) { //na volledige rotatie ga naar nul
        pos = 0;
    }
    stapmode ++;
    if (stapmode > 3) {
        stapmode = 0;
    }
    return pos;

}

bool afstandzoeker ()
{
    bool a ;
    if (zoeksensor > 0.25) {
        a = 1;
    } else {
        a=0 ;
    }
    return a;
}

int lijnsensor ()

{
    bool a = 0;// sensor linksvoor
    bool b = 0;// sensor rechtsvoor
    bool c = 0;// sensor linksachter
    bool d = 0;// sensor rechtsachter

    int output;
    if (Sensor1 > 0.01) {
        a = 1;
    } else {
        a = 0;
    }
    if (Sensor2 > 0.01) {
        b = 1<<1;
    } else {
        b = 0<<1;
    }
    if (Sensor3 > 0.01) {
        c = 1<<2;
    } else {
        c = 0<<2;
    }
    if (Sensor4 > 0.01) {
        d = 1<<3;
    } else {
        d = 0<<3;
    }

    output = a || b || c || d;
    return output;
}



void Hbrug(int speedleft = 0, int speedright = 0, int direction = 0)
{
    int currenta = mapcurrent(currentleftpin,0,1,0,6600);
    int currentb = mapcurrent(currentrightpin,0,1,0,6600);


    bool MotorL1 = 0;
    bool MotorL2 = 0;
    bool MotorR1 = 0;
    bool MotorR2 = 0;

    switch (stapmode) {
        case 0:// stilstaan
            MotorL1 = 0;
            MotorL2 = 0;
            MotorR1 = 0;
            MotorR2 = 0;
            break;
        case 1:// vooruit
            MotorL1 = 0;
            MotorL2 = 1;
            MotorR1 = 0;
            MotorR2 = 1;
            break;
        case 2:// achteruit
            MotorL1 = 0;
            MotorL2 = 0;
            MotorR1 = 0;
            MotorR2 = 0;
            break;
        case 3:// naar linksvoor
            MotorL1 = 0;
            MotorL2 = 0;
            MotorR1 = 0;
            MotorR2 = 0;
            break;
        case 4:// naar rechtsvoor
            MotorL1 = 0;
            MotorL2 = 0;
            MotorR1 = 0;
            MotorR2 = 0;
            break;
        case 5:// naar linksachter
            MotorL1 = 0;
            MotorL2 = 0;
            MotorR1 = 0;
            MotorR2 = 0;
            break;
        case 6: // naar rechtsachter
            MotorL1 = 0;
            MotorL2 = 0;
            MotorR1 = 0;
            MotorR2 = 0;
            break;
        case 7: // links om as
            MotorL1 = 0;
            MotorL2 = 0;
            MotorR1 = 0;
            MotorR2 = 0;
            break;
        case 8: // rechts om as
            MotorL1 = 0;
            MotorL2 = 0;
            MotorR1 = 0;
            MotorR2 = 0;
            break;

    }
    MotorL1pin = MotorL1;
    MotorL2pin = MotorL2;
    MotorR1pin = MotorR1;
    MotorR2pin = MotorR2;

}