h

Dependencies:   mbed

Fork of StateMachine by Roboshark

main.cpp

Committer:
Jacqueline
Date:
2018-04-16
Revision:
2:a227732e1412
Parent:
0:e611a0b9f02a

File content as of revision 2:a227732e1412:

#include <mbed.h>

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

// DigitalOut myled(LED1);

//DigitalOut led0(PC_8);
//DigitalOut led1(PC_6);
//DigitalOut led2(PB_12);
//DigitalOut led3(PA_7);
//DigitalOut led4(PC_0);
//DigitalOut led5(PC_9);

//AnalogIn distance(PB_1);
//DigitalOut enable(PC_1);
//DigitalOut bit0(PH_1);
//DigitalOut bit1(PC_2);
//DigitalOut bit2(PC_3);

IRSensor irSensorF(PC_2);
IRSensor irSensorR(PC_3);
IRSensor irSensorL(PC_5);

BottomSensor lineDetector(RX/D0);

DigitalOut enableMotorDriver(PB_2);
DigitalIn motorDriverFault(PB_14);
DigitalIn motorDriverWarning(PB_15);

PwmOut pwmLeft(PA_9);
PwmOut pwmRight(PA_8);

EncoderCounter counterLeft(PB_6, PB_7);
EncoderCounter counterRight(PA_6, PC_7);

Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);

int main()
{

    controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm]
    controller.setDesiredSpeedRight(-150.0f);
    enable = 1;
    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein

    while(1) {

        myled =! myled;

        if (irSensorf.read() < 0.2f) led0 = 1;
        else led0 = 0;
        
        if (irSensorR.read() < 0.2f) led1 = 1;
        else led1 = 0;
        
        if (irSensorL.read() < 0.2f) led2 = 1;
        else led2 = 0;
        
        //if (irSensor3.read() < 0.2f) led3 = 1;
        //else led3 = 0;
        
        //if (irSensor4.read() < 0.2f) led4 = 1;
        //else led4 = 0;
        
        //if (irSensor5.read() < 0.2f) led5 = 1;
        //else led0 = 5;
        
        printf("iSumLeft: %f, iSumRight: %f, counterDiff; %d \n\r", controller.getIntegralLeft(), controller.getIntegralRight(), (counterLeft.read()+counterRight.read()));
                
        wait(0.2f); // 100 ms
        
    }
}