Antonia Baumgartner / Mbed 2 deprecated YB_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of Versuch21 by Antonia Baumgartner

Classes/Motion.cpp

Committer:
baumgant
Date:
2018-05-09
Revision:
5:93d3efe46493
Parent:
4:3c6d2c035243

File content as of revision 5:93d3efe46493:

#include "Motion.h"
#include "Controller.h"
#include "EncoderCounter.h"
#include "Spurhaltung.h"
#include "IRSensorGF.h"
#include "IRSensorG.h"
#include "IRSensorK.h"
#include "IRSensorZ.h"

// Left + = Vorwärts Left - = Vorwärts
// Right + = Rückwärts Right - = Rückwärts



using namespace std;

const float Motion::SPEED = 150;
const float Motion::DREHEN90 = 1607;
const float Motion::GERADE = 3280;

Motion::Motion(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, Spurhaltung& spurhaltung, IRSensorGF& Sensor1, IRSensorG& Sensor2, IRSensorG& Sensor3, IRSensorK& Sensor4, IRSensorK& Sensor5, IRSensorZ& Sensor6):
    counterLeft(counterLeft), counterRight(counterRight), controller(controller), spurhaltung(spurhaltung), Sensor1(Sensor1), Sensor2(Sensor2), Sensor3(Sensor3), Sensor4(Sensor4), Sensor5(Sensor5), Sensor6(Sensor6)
{

}

Motion::~Motion() {}

//------------------------------------------------------------------------------

void Motion::drehenl90()
{
    controller.resetCounter();
    int lastCountRight = counterRight.read();

    while(counterRight.read()-lastCountRight > -DREHEN90) {
        controller.setDesiredSpeedLeft(-SPEED);             //Drehzahl in [rpm]
        controller.setDesiredSpeedRight(-SPEED);
    }
    controller.setDesiredSpeedLeft(0.0f);                   //Drehzahl in [rpm]
    controller.setDesiredSpeedRight(0.0f);

}

//------------------------------------------------------------------------------

void Motion::gerade()
{

    controller.resetCounter();
    int lastCountLeft = counterLeft.read();

    while((counterLeft.read()-lastCountLeft < GERADE) && (Sensor1.read() > 120 )) { // 1.806
        controller.setDesiredSpeedLeft(spurhaltung.speedl());   //Drehzahl in [rpm]
        controller.setDesiredSpeedRight(-spurhaltung.speedr());
        printf("");
        //printf("SpurhaltungLinks: %d\r\n", spurhaltung.speedl());
        //printf("SpurhaltungRechts: %d\r\n", spurhaltung.speedr());
        //printf("Sensor2:%d\r\n", Sensor2.read());
        //printf("Sensor1:%d\r\n\r\n", Sensor1.read());
    }
    if((Sensor1.read() > 120) && (Sensor1.read() < 250)) {
        while(Sensor1.read() > 120) {
            controller.setDesiredSpeedLeft(spurhaltung.speedl());   //Drehzahl in [rpm]
            controller.setDesiredSpeedRight(-spurhaltung.speedr());
            printf("");
        }

    }
    controller.setDesiredSpeedLeft(0.0f);                       //Drehzahl in [rpm]
    controller.setDesiredSpeedRight(0.0f);
}

//------------------------------------------------------------------------------

void Motion::drehenr90()
{
    controller.resetCounter();
    int lastCountLeft = counterLeft.read();

    while(counterLeft.read()-lastCountLeft < DREHEN90) {
        controller.setDesiredSpeedLeft(SPEED);            //Drehzahl in [rpm]
        controller.setDesiredSpeedRight(SPEED);
    }
    controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
    controller.setDesiredSpeedRight(0.0f);

}

//------------------------------------------------------------------------------

void Motion::drehen180()
{
    controller.resetCounter();
    int lastCountRight = counterRight.read();

    while(counterRight.read()-lastCountRight < 2*1607 + 150) {
        controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
        controller.setDesiredSpeedRight(150.0f);
    }
    controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
    controller.setDesiredSpeedRight(0.0f);
}

//------------------------------------------------------------------------------

void Motion::switching(int D)
{
    switch(D) {
        case 1:
            // Left
            drehenl90();
            wait(0.05f);
            // Forward
            gerade();
            wait(0.05f);
            break;

        case 2:
            // Forward
            gerade();
            wait(0.05f);
            break;

        case 3:
            // Right
            drehenr90();
            wait(0.05f);
            // Forward
            gerade();
            wait(0.05f);
            break;

        case 4:
            // 180 Drehen
            drehen180();
            wait(0.05f);
            break;

        default:
            controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
            controller.setDesiredSpeedRight(0.0f);
    }

//------------------------------------------------------------------------------

    /*void Motion::switching()
    {
         if(Sensor4.read() == 0) {
                // Left
                drehenl90();
                // Forward
                gerade();
        } else if (Sensor1.read() == 0) {
                // Forward
                gerade();
        } else if (Sensor5.read() == 0) {
                // Right
                drehenr90();
                // Forward
                gerade();
        } else {
                // 180 Drehen
                drehen180();
                // Forward
                gerade();

        }*/

}