Roboshark / Mbed 2 deprecated Roboshark_V2

kann fahren und drehen

Dependencies:   mbed

main.cpp@1:a95ba1510438, 2018-04-23 (annotated)

Committer:
Jacqueline
Date:
Mon Apr 23 11:57:53 2018 +0000
Revision:
1:a95ba1510438
Parent:
0:6d0671ae4648
Child:
2:402b1a74fff6
neu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jacqueline 0:6d0671ae4648 1
Jacqueline 0:6d0671ae4648 2 #include <mbed.h>
Jacqueline 0:6d0671ae4648 3
Jacqueline 0:6d0671ae4648 4 #include "EncoderCounter.h"
Jacqueline 0:6d0671ae4648 5 #include "Controller.h"
Jacqueline 0:6d0671ae4648 6 #include "IRSensor.h"
Jacqueline 0:6d0671ae4648 7 #include "Fahren.h"
Jacqueline 0:6d0671ae4648 8 #include "StateMachine.h"
Jacqueline 0:6d0671ae4648 9
Jacqueline 0:6d0671ae4648 10 DigitalOut myled(LED1);
Jacqueline 0:6d0671ae4648 11
Jacqueline 0:6d0671ae4648 12 DigitalOut led0(PC_8);
Jacqueline 0:6d0671ae4648 13 DigitalOut led1(PC_6);
Jacqueline 0:6d0671ae4648 14 DigitalOut led2(PB_12);
Jacqueline 0:6d0671ae4648 15 DigitalOut led3(PA_7);
Jacqueline 0:6d0671ae4648 16 DigitalOut led4(PC_0);
Jacqueline 0:6d0671ae4648 17 DigitalOut led5(PC_9);
Jacqueline 0:6d0671ae4648 18
Jacqueline 0:6d0671ae4648 19 AnalogIn distance(PB_1);
Jacqueline 0:6d0671ae4648 20 DigitalOut enable(PC_1);
Jacqueline 0:6d0671ae4648 21 DigitalOut bit0(PH_1);
Jacqueline 0:6d0671ae4648 22 DigitalOut bit1(PC_2);
Jacqueline 0:6d0671ae4648 23 DigitalOut bit2(PC_3);
Jacqueline 0:6d0671ae4648 24
Jacqueline 0:6d0671ae4648 25 /*IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
Jacqueline 0:6d0671ae4648 26 IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
Jacqueline 0:6d0671ae4648 27 IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
Jacqueline 0:6d0671ae4648 28 IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
Jacqueline 0:6d0671ae4648 29 IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
Jacqueline 0:6d0671ae4648 30 IRSensor irSensor5(distance, bit0, bit1, bit2, 5);*/
Jacqueline 0:6d0671ae4648 31
Jacqueline 0:6d0671ae4648 32 AnalogIn IrRight(PC_3); //von main Vincent kopiert
Jacqueline 0:6d0671ae4648 33 AnalogIn IrLeft (PC_5);
Jacqueline 0:6d0671ae4648 34 AnalogIn IrFront(PC_2);
Jacqueline 0:6d0671ae4648 35 float disR = 0;
Jacqueline 0:6d0671ae4648 36 float disL = 0;
Jacqueline 0:6d0671ae4648 37 float disF = 0;
Jacqueline 0:6d0671ae4648 38
Jacqueline 0:6d0671ae4648 39 float dis2R = 0;
Jacqueline 0:6d0671ae4648 40 float dis2L = 0;
Jacqueline 0:6d0671ae4648 41 float dis2F = 0;
Jacqueline 0:6d0671ae4648 42 int IrR = 0;
Jacqueline 0:6d0671ae4648 43 int IrL = 0;
Jacqueline 0:6d0671ae4648 44 int IrF = 0;
Jacqueline 0:6d0671ae4648 45 int caseDrive = 0; //von main Vincent kopiert
Jacqueline 0:6d0671ae4648 46
Jacqueline 0:6d0671ae4648 47 IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F); //von main Vincent kopiert
Jacqueline 0:6d0671ae4648 48 StateMachine stateMachine(IrR, IrL, IrF); //von main Vincent kopiert
Jacqueline 0:6d0671ae4648 49
Jacqueline 0:6d0671ae4648 50
Jacqueline 0:6d0671ae4648 51 DigitalOut enableMotorDriver(PB_2);
Jacqueline 0:6d0671ae4648 52 DigitalIn motorDriverFault(PB_14);
Jacqueline 0:6d0671ae4648 53 DigitalIn motorDriverWarning(PB_15);
Jacqueline 0:6d0671ae4648 54
Jacqueline 0:6d0671ae4648 55 PwmOut pwmLeft(PA_8);
Jacqueline 0:6d0671ae4648 56 PwmOut pwmRight(PA_9);
Jacqueline 0:6d0671ae4648 57
Jacqueline 0:6d0671ae4648 58 EncoderCounter counterLeft(PB_6, PB_7);
Jacqueline 0:6d0671ae4648 59 EncoderCounter counterRight(PA_6, PC_7);
Jacqueline 0:6d0671ae4648 60
Jacqueline 0:6d0671ae4648 61 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Jacqueline 0:6d0671ae4648 62
Jacqueline 0:6d0671ae4648 63 Fahren fahren(controller, counterLeft, counterRight); //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
Jacqueline 0:6d0671ae4648 64
Jacqueline 0:6d0671ae4648 65 int main()
Jacqueline 0:6d0671ae4648 66 {
Jacqueline 0:6d0671ae4648 67
Jacqueline 0:6d0671ae4648 68 //controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm] //DEFAULT VON TOBI
Jacqueline 0:6d0671ae4648 69 //controller.setDesiredSpeedRight(-150.0f); //DEFAULT VON TOBI
Jacqueline 0:6d0671ae4648 70 enable = 1;
Jacqueline 0:6d0671ae4648 71 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
Jacqueline 0:6d0671ae4648 72
Jacqueline 0:6d0671ae4648 73 while(1) {
Jacqueline 0:6d0671ae4648 74
Jacqueline 0:6d0671ae4648 75 // Test um Drehungen und geradeaus zu testen
Jacqueline 0:6d0671ae4648 76
Jacqueline 0:6d0671ae4648 77 /*fahren.geradeaus(); //Geradeausfahren aufrufen
Jacqueline 0:6d0671ae4648 78 wait(1.0f);
Jacqueline 0:6d0671ae4648 79
Jacqueline 0:6d0671ae4648 80 fahren.rechts90();
Jacqueline 0:6d0671ae4648 81 wait(1.0f);
Jacqueline 0:6d0671ae4648 82
Jacqueline 0:6d0671ae4648 83 fahren.rechts180();
Jacqueline 0:6d0671ae4648 84 wait(1.0f);
Jacqueline 0:6d0671ae4648 85
Jacqueline 0:6d0671ae4648 86 fahren.rechts270();
Jacqueline 0:6d0671ae4648 87 wait(1.0f);*/
Jacqueline 0:6d0671ae4648 88
Jacqueline 0:6d0671ae4648 89 // IR Sensoren einlesen Programm Vincent
Jacqueline 0:6d0671ae4648 90
Jacqueline 0:6d0671ae4648 91
Jacqueline 0:6d0671ae4648 92 float disR = iRSensor.readR(); // Distanz in mm
Jacqueline 0:6d0671ae4648 93 float disL = iRSensor.readL();
Jacqueline 0:6d0671ae4648 94 float disF = iRSensor.readF();
Jacqueline 0:6d0671ae4648 95 dis2R = disR;
Jacqueline 0:6d0671ae4648 96 dis2L = disL;
Jacqueline 0:6d0671ae4648 97 dis2F = disF;
Jacqueline 0:6d0671ae4648 98 int IrR = iRSensor.codeR(); // min Distanz unterschritten = 1
Jacqueline 0:6d0671ae4648 99 int IrL = iRSensor.codeL();
Jacqueline 0:6d0671ae4648 100 int IrF = iRSensor.codeF();
Jacqueline 1:a95ba1510438 101 /*int caseDrive = stateMachine.drive();*/ //entscheidung welcher Drive Case
Jacqueline 0:6d0671ae4648 102
Jacqueline 0:6d0671ae4648 103
Jacqueline 0:6d0671ae4648 104 printf ("IR Right = %d \n", IrR);
Jacqueline 0:6d0671ae4648 105 printf("IR Left = %d\n",IrL);
Jacqueline 0:6d0671ae4648 106 printf("IR Front = %d\n",IrF);
Jacqueline 0:6d0671ae4648 107
Jacqueline 0:6d0671ae4648 108 if((IrR==0) && (IrL==0) && (IrF==1)){
Jacqueline 0:6d0671ae4648 109 caseDrive = 2; // Folge: 90 Grad nach rechts drehen
Jacqueline 0:6d0671ae4648 110 } else if ((IrR==0) && (IrL==1) && (IrF==0)){
Jacqueline 0:6d0671ae4648 111 caseDrive = 2; // Folge: 90 Grad nach rechts drehen
Jacqueline 0:6d0671ae4648 112 } else if ((IrR==0) && (IrL==1) && (IrF==1)){
Jacqueline 0:6d0671ae4648 113 caseDrive = 2; // Folge: 90 Grad nach rechts drehen
Jacqueline 0:6d0671ae4648 114 } else if ((IrR==1) && (IrL==0) && (IrF==0)){
Jacqueline 0:6d0671ae4648 115 caseDrive = 1; // Folge: geradeaus fahren
Jacqueline 0:6d0671ae4648 116 } else if ((IrR==1) && (IrL==0) && (IrF==1)){
Jacqueline 0:6d0671ae4648 117 caseDrive = 3; // Folge: 270 Grad nach rechts drehen
Jacqueline 0:6d0671ae4648 118 } else if ((IrR==1) && (IrL==1) && (IrF==0)){
Jacqueline 0:6d0671ae4648 119 caseDrive = 1; // Folge: geradeaus fahren
Jacqueline 0:6d0671ae4648 120 } else if ((IrR==1) && (IrL==1) && (IrF==1)){
Jacqueline 0:6d0671ae4648 121 caseDrive = 4; // Folge: 180 Grad nach rechts drehen
Jacqueline 0:6d0671ae4648 122 } else{ caseDrive=0;
Jacqueline 0:6d0671ae4648 123 }
Jacqueline 0:6d0671ae4648 124
Jacqueline 0:6d0671ae4648 125 printf("caseDrive = %d\n",caseDrive);
Jacqueline 0:6d0671ae4648 126
Jacqueline 0:6d0671ae4648 127 wait (1.0);
Jacqueline 0:6d0671ae4648 128
Jacqueline 1:a95ba1510438 129
Jacqueline 0:6d0671ae4648 130
Jacqueline 0:6d0671ae4648 131
Jacqueline 0:6d0671ae4648 132 if(caseDrive == 1){
Jacqueline 0:6d0671ae4648 133 fahren.geradeaus();
Jacqueline 0:6d0671ae4648 134 } else if (caseDrive == 2){
Jacqueline 0:6d0671ae4648 135 fahren.rechts90();
Jacqueline 1:a95ba1510438 136 fahren.geradeaus();
Jacqueline 0:6d0671ae4648 137 } else if (caseDrive == 3){
Jacqueline 0:6d0671ae4648 138 fahren.rechts270();
Jacqueline 1:a95ba1510438 139 fahren.geradeaus();
Jacqueline 0:6d0671ae4648 140 } else if (caseDrive == 4){
Jacqueline 0:6d0671ae4648 141 fahren.rechts180();
Jacqueline 1:a95ba1510438 142 fahren.geradeaus();
Jacqueline 0:6d0671ae4648 143 }
Jacqueline 0:6d0671ae4648 144
Jacqueline 0:6d0671ae4648 145
Jacqueline 0:6d0671ae4648 146 }
Jacqueline 0:6d0671ae4648 147 }
Jacqueline 0:6d0671ae4648 148
Jacqueline 0:6d0671ae4648 149
Jacqueline 0:6d0671ae4648 150