kann fahren und drehen
Dependencies: mbed
main.cpp
- Committer:
- Jacqueline
- Date:
- 2018-04-23
- Revision:
- 2:402b1a74fff6
- Parent:
- 1:a95ba1510438
File content as of revision 2:402b1a74fff6:
#include <mbed.h> #include "EncoderCounter.h" #include "Controller.h" #include "IRSensor.h" #include "Fahren.h" #include "StateMachine.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 irSensor0(distance, bit0, bit1, bit2, 0); IRSensor irSensor1(distance, bit0, bit1, bit2, 1); IRSensor irSensor2(distance, bit0, bit1, bit2, 2); IRSensor irSensor3(distance, bit0, bit1, bit2, 3); IRSensor irSensor4(distance, bit0, bit1, bit2, 4); IRSensor irSensor5(distance, bit0, bit1, bit2, 5);*/ AnalogIn IrRight(PC_3); //von main Vincent kopiert AnalogIn IrLeft (PC_5); AnalogIn IrFront(PC_2); float disR = 0; float disL = 0; float disF = 0; float dis2R = 0; float dis2L = 0; float dis2F = 0; int IrR = 0; int IrL = 0; int IrF = 0; int caseDrive = 0; //von main Vincent kopiert IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F); //von main Vincent kopiert StateMachine stateMachine(IrR, IrL, IrF); //von main Vincent kopiert DigitalOut enableMotorDriver(PB_2); DigitalIn motorDriverFault(PB_14); DigitalIn motorDriverWarning(PB_15); PwmOut pwmLeft(PA_8); PwmOut pwmRight(PA_9); EncoderCounter counterLeft(PB_6, PB_7); EncoderCounter counterRight(PA_6, PC_7); Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); Fahren fahren(controller, counterLeft, counterRight); //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig int main() { //controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm] //DEFAULT VON TOBI //controller.setDesiredSpeedRight(-150.0f); //DEFAULT VON TOBI enable = 1; enableMotorDriver = 1; // Schaltet den Leistungstreiber ein while(1) { // Test um Drehungen und geradeaus zu testen /*fahren.geradeaus(); //Geradeausfahren aufrufen wait(1.0f); fahren.rechts90(); wait(1.0f); fahren.rechts180(); wait(1.0f); fahren.links90(); wait(1.0f);*/ // IR Sensoren einlesen Programm Vincent float disR = iRSensor.readR(); // Distanz in mm float disL = iRSensor.readL(); float disF = iRSensor.readF(); dis2R = disR; dis2L = disL; dis2F = disF; int IrR = iRSensor.codeR(); // min Distanz unterschritten = 1 int IrL = iRSensor.codeL(); int IrF = iRSensor.codeF(); /*int caseDrive = stateMachine.drive();*/ //entscheidung welcher Drive Case printf ("IR Right = %d \n", IrR); printf("IR Left = %d\n",IrL); printf("IR Front = %d\n",IrF); if((IrR==0) && (IrL==0) && (IrF==1)){ caseDrive = 2; // Folge: 90 Grad nach rechts drehen } else if ((IrR==0) && (IrL==1) && (IrF==0)){ caseDrive = 2; // Folge: 90 Grad nach rechts drehen } else if ((IrR==0) && (IrL==1) && (IrF==1)){ caseDrive = 2; // Folge: 90 Grad nach rechts drehen } else if ((IrR==1) && (IrL==0) && (IrF==0)){ caseDrive = 1; // Folge: geradeaus fahren } else if ((IrR==1) && (IrL==0) && (IrF==1)){ caseDrive = 3; // Folge: 270 Grad nach rechts drehen } else if ((IrR==1) && (IrL==1) && (IrF==0)){ caseDrive = 1; // Folge: geradeaus fahren } else if ((IrR==1) && (IrL==1) && (IrF==1)){ caseDrive = 4; // Folge: 180 Grad nach rechts drehen } else{ caseDrive=0; } printf("caseDrive = %d\n",caseDrive); wait (0.5); if(caseDrive == 1){ fahren.geradeaus(); } else if (caseDrive == 2){ fahren.rechts90(); fahren.geradeaus(); } else if (caseDrive == 3){ fahren.links90(); fahren.geradeaus(); } else if (caseDrive == 4){ fahren.rechts180(); fahren.geradeaus(); } } }