Roboshark / Mbed 2 deprecated Roboshark_V10

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 Roboshark_V9 by Roboshark

main.cpp@8:d0a27278c108, 2018-05-07 (annotated)

Committer:
ahlervin
Date:
Mon May 07 14:11:06 2018 +0000
Revision:
8:d0a27278c108
Parent:
7:862d80e0ea2d
Child:
9:feabe0b7cea4
funkt regler

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ahlervin 7:862d80e0ea2d 1 /*Roboshark V5
ahlervin 6:7bbcdd07bc2d 2 main.cpp
ahlervin 6:7bbcdd07bc2d 3 Erstellt: J. Blunschi
ahlervin 6:7bbcdd07bc2d 4 geändert: V.Ahlers
ahlervin 6:7bbcdd07bc2d 5 V.5.18
ahlervin 6:7bbcdd07bc2d 6 main zu Robishark V4
ahlervin 6:7bbcdd07bc2d 7 */
ahlervin 6:7bbcdd07bc2d 8
Jacqueline 0:6d0671ae4648 9
Jacqueline 0:6d0671ae4648 10 #include <mbed.h>
Jacqueline 0:6d0671ae4648 11
Jacqueline 0:6d0671ae4648 12 #include "EncoderCounter.h"
Jacqueline 0:6d0671ae4648 13 #include "Controller.h"
Jacqueline 0:6d0671ae4648 14 #include "IRSensor.h"
Jacqueline 0:6d0671ae4648 15 #include "Fahren.h"
ahlervin 6:7bbcdd07bc2d 16 #include "Regler.h"
Jacqueline 0:6d0671ae4648 17
Jacqueline 0:6d0671ae4648 18 DigitalOut myled(LED1);
Jacqueline 0:6d0671ae4648 19
Jacqueline 0:6d0671ae4648 20 DigitalOut led0(PC_8);
Jacqueline 0:6d0671ae4648 21 DigitalOut led1(PC_6);
Jacqueline 0:6d0671ae4648 22 DigitalOut led2(PB_12);
Jacqueline 0:6d0671ae4648 23 DigitalOut led3(PA_7);
Jacqueline 0:6d0671ae4648 24 DigitalOut led4(PC_0);
Jacqueline 0:6d0671ae4648 25 DigitalOut led5(PC_9);
Jacqueline 0:6d0671ae4648 26
ahlervin 4:767fd282dd9c 27 AnalogIn LineSensor(PB_1); // Line Sensor
Jacqueline 0:6d0671ae4648 28 DigitalOut enable(PC_1);
Jacqueline 0:6d0671ae4648 29 DigitalOut bit0(PH_1);
Jacqueline 0:6d0671ae4648 30 DigitalOut bit1(PC_2);
Jacqueline 0:6d0671ae4648 31 DigitalOut bit2(PC_3);
Jacqueline 0:6d0671ae4648 32
Jacqueline 0:6d0671ae4648 33 AnalogIn IrRight(PC_3); //von main Vincent kopiert
Jacqueline 0:6d0671ae4648 34 AnalogIn IrLeft (PC_5);
Jacqueline 0:6d0671ae4648 35 AnalogIn IrFront(PC_2);
Jacqueline 0:6d0671ae4648 36 float disR = 0;
Jacqueline 0:6d0671ae4648 37 float disL = 0;
Jacqueline 0:6d0671ae4648 38 float disF = 0;
Jacqueline 0:6d0671ae4648 39
Jacqueline 0:6d0671ae4648 40 float dis2R = 0;
Jacqueline 0:6d0671ae4648 41 float dis2L = 0;
Jacqueline 0:6d0671ae4648 42 float dis2F = 0;
Jacqueline 0:6d0671ae4648 43 int IrR = 0;
Jacqueline 0:6d0671ae4648 44 int IrL = 0;
Jacqueline 0:6d0671ae4648 45 int IrF = 0;
ahlervin 4:767fd282dd9c 46 int caseDrive = 0;
ahlervin 4:767fd282dd9c 47 double line = 0;
ahlervin 4:767fd282dd9c 48 bool finish = 0;
ahlervin 4:767fd282dd9c 49 bool finishLast = 0;
ahlervin 4:767fd282dd9c 50 int ende = 0;
ahlervin 6:7bbcdd07bc2d 51 int temp = 0;
ahlervin 6:7bbcdd07bc2d 52 float SpeedR = 0;
ahlervin 6:7bbcdd07bc2d 53 float SpeedL = 0;
ahlervin 7:862d80e0ea2d 54 int reglerEin = 0;
ahlervin 6:7bbcdd07bc2d 55 //von main Vincent kopiert
Jacqueline 0:6d0671ae4648 56
Jacqueline 0:6d0671ae4648 57 DigitalOut enableMotorDriver(PB_2);
Jacqueline 0:6d0671ae4648 58 DigitalIn motorDriverFault(PB_14);
Jacqueline 0:6d0671ae4648 59 DigitalIn motorDriverWarning(PB_15);
Jacqueline 0:6d0671ae4648 60
Jacqueline 0:6d0671ae4648 61 PwmOut pwmLeft(PA_8);
Jacqueline 0:6d0671ae4648 62 PwmOut pwmRight(PA_9);
Jacqueline 0:6d0671ae4648 63
Jacqueline 0:6d0671ae4648 64 EncoderCounter counterLeft(PB_6, PB_7);
Jacqueline 0:6d0671ae4648 65 EncoderCounter counterRight(PA_6, PC_7);
Jacqueline 0:6d0671ae4648 66
ahlervin 6:7bbcdd07bc2d 67 IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor); //von main Vincent kopiert
ahlervin 6:7bbcdd07bc2d 68
Jacqueline 0:6d0671ae4648 69 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Jacqueline 0:6d0671ae4648 70
ahlervin 7:862d80e0ea2d 71 Regler regler(IrRight, IrLeft, iRSensor);
ahlervin 6:7bbcdd07bc2d 72
ahlervin 8:d0a27278c108 73 Fahren fahren(controller, counterLeft, counterRight, regler, reglerEin, iRSensor); //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
Jacqueline 0:6d0671ae4648 74
Jacqueline 0:6d0671ae4648 75 int main()
Jacqueline 0:6d0671ae4648 76 {
Jacqueline 0:6d0671ae4648 77
Jacqueline 0:6d0671ae4648 78 //controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm] //DEFAULT VON TOBI
Jacqueline 0:6d0671ae4648 79 //controller.setDesiredSpeedRight(-150.0f); //DEFAULT VON TOBI
Jacqueline 0:6d0671ae4648 80 enable = 1;
Jacqueline 0:6d0671ae4648 81 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
Jacqueline 0:6d0671ae4648 82 while(1) {
Jacqueline 0:6d0671ae4648 83
Jacqueline 0:6d0671ae4648 84 // Test um Drehungen und geradeaus zu testen
Jacqueline 0:6d0671ae4648 85
Jacqueline 0:6d0671ae4648 86 /*fahren.geradeaus(); //Geradeausfahren aufrufen
Jacqueline 0:6d0671ae4648 87 wait(1.0f);
Jacqueline 0:6d0671ae4648 88
Jacqueline 0:6d0671ae4648 89 fahren.rechts90();
Jacqueline 0:6d0671ae4648 90 wait(1.0f);
Jacqueline 0:6d0671ae4648 91
Jacqueline 0:6d0671ae4648 92 fahren.rechts180();
Jacqueline 0:6d0671ae4648 93 wait(1.0f);
Jacqueline 0:6d0671ae4648 94
Jacqueline 2:402b1a74fff6 95 fahren.links90();
Jacqueline 0:6d0671ae4648 96 wait(1.0f);*/
ahlervin 6:7bbcdd07bc2d 97 // IR Sensoren einlesen
Jacqueline 0:6d0671ae4648 98 float disR = iRSensor.readR(); // Distanz in mm
Jacqueline 0:6d0671ae4648 99 float disL = iRSensor.readL();
Jacqueline 0:6d0671ae4648 100 float disF = iRSensor.readF();
Jacqueline 0:6d0671ae4648 101 dis2R = disR;
Jacqueline 0:6d0671ae4648 102 dis2L = disL;
Jacqueline 0:6d0671ae4648 103 dis2F = disF;
Jacqueline 0:6d0671ae4648 104 int IrR = iRSensor.codeR(); // min Distanz unterschritten = 1
Jacqueline 0:6d0671ae4648 105 int IrL = iRSensor.codeL();
Jacqueline 0:6d0671ae4648 106 int IrF = iRSensor.codeF();
ahlervin 6:7bbcdd07bc2d 107 ende = iRSensor.get_ende(); // Line erkennt = 1
ahlervin 4:767fd282dd9c 108
ahlervin 4:767fd282dd9c 109
ahlervin 4:767fd282dd9c 110
ahlervin 7:862d80e0ea2d 111 /* printf (" ende = %d\n",ende);
ahlervin 4:767fd282dd9c 112 printf (" finish = %d\n",finish);
ahlervin 4:767fd282dd9c 113 printf (" finishLast = %d\n",finishLast);
ahlervin 4:767fd282dd9c 114 printf("line = %f\n", line);
Jacqueline 0:6d0671ae4648 115
ahlervin 6:7bbcdd07bc2d 116 printf ("IR Right = %d \n", IrR);
Jacqueline 0:6d0671ae4648 117 printf("IR Left = %d\n",IrL);
ahlervin 4:767fd282dd9c 118 printf("IR Front = %d\n",IrF);*/
Jacqueline 0:6d0671ae4648 119
ahlervin 4:767fd282dd9c 120 if((IrR==0) && (IrL==0) && (IrF==1) && (ende == 0)){
Jacqueline 0:6d0671ae4648 121 caseDrive = 2; // Folge: 90 Grad nach rechts drehen
ahlervin 4:767fd282dd9c 122 } else if ((IrR==0) && (IrL==1) && (IrF==0) && (ende == 0)){
ahlervin 4:767fd282dd9c 123 caseDrive = 2; // Folge: 90 Grad nach rechts drehen
ahlervin 4:767fd282dd9c 124 } else if ((IrR==0) && (IrL==1) && (IrF==1) && (ende == 0)){
Jacqueline 0:6d0671ae4648 125 caseDrive = 2; // Folge: 90 Grad nach rechts drehen
ahlervin 4:767fd282dd9c 126 } else if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
Jacqueline 0:6d0671ae4648 127 caseDrive = 1; // Folge: geradeaus fahren
ahlervin 4:767fd282dd9c 128 } else if ((IrR==1) && (IrL==0) && (IrF==1) && (ende == 0)){
ahlervin 6:7bbcdd07bc2d 129 caseDrive = 3; // Folge: 90 Grad nach Links drehen
ahlervin 4:767fd282dd9c 130 } else if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
Jacqueline 0:6d0671ae4648 131 caseDrive = 1; // Folge: geradeaus fahren
ahlervin 4:767fd282dd9c 132 } else if ((IrR==1) && (IrL==1) && (IrF==1) && (ende == 0)){
Jacqueline 0:6d0671ae4648 133 caseDrive = 4; // Folge: 180 Grad nach rechts drehen
ahlervin 4:767fd282dd9c 134 } else if ((ende == 1) && (temp == 0)){
ahlervin 6:7bbcdd07bc2d 135 caseDrive = 5; // Folge: Ziel erreicht
ahlervin 4:767fd282dd9c 136 temp = 1;
ahlervin 6:7bbcdd07bc2d 137 } else {caseDrive = 0; // Folge; Stop
Jacqueline 0:6d0671ae4648 138 }
Jacqueline 0:6d0671ae4648 139
ahlervin 8:d0a27278c108 140 // printf("caseDrive = %d\n",caseDrive);
ahlervin 6:7bbcdd07bc2d 141 //printf("ende = %d\n",ende);
Jacqueline 0:6d0671ae4648 142
ahlervin 7:862d80e0ea2d 143 wait (0.1);
Jacqueline 0:6d0671ae4648 144
Jacqueline 1:a95ba1510438 145
Jacqueline 0:6d0671ae4648 146
Jacqueline 0:6d0671ae4648 147
ahlervin 6:7bbcdd07bc2d 148 if(caseDrive == 1){ // Aufrufen der verschiedenen fahr funktionen
ahlervin 7:862d80e0ea2d 149 reglerEin = 1;
ahlervin 6:7bbcdd07bc2d 150 fahren.geradeausG();
ahlervin 7:862d80e0ea2d 151 reglerEin = 0;
Jacqueline 0:6d0671ae4648 152 } else if (caseDrive == 2){
Jacqueline 0:6d0671ae4648 153 fahren.rechts90();
ahlervin 7:862d80e0ea2d 154 reglerEin = 1;
ahlervin 6:7bbcdd07bc2d 155 fahren.geradeausG();
ahlervin 7:862d80e0ea2d 156 reglerEin = 0;
Jacqueline 0:6d0671ae4648 157 } else if (caseDrive == 3){
Jacqueline 2:402b1a74fff6 158 fahren.links90();
ahlervin 7:862d80e0ea2d 159 reglerEin = 1;
ahlervin 6:7bbcdd07bc2d 160 fahren.geradeausG();
ahlervin 7:862d80e0ea2d 161 reglerEin = 0;
Jacqueline 0:6d0671ae4648 162 } else if (caseDrive == 4){
Jacqueline 0:6d0671ae4648 163 fahren.rechts180();
ahlervin 7:862d80e0ea2d 164 reglerEin = 1;
ahlervin 6:7bbcdd07bc2d 165 fahren.geradeausG();
ahlervin 7:862d80e0ea2d 166 reglerEin = 0;
ahlervin 4:767fd282dd9c 167 } else if (caseDrive == 0){
ahlervin 4:767fd282dd9c 168 fahren.stopp();
ahlervin 4:767fd282dd9c 169 } else if(caseDrive == 5){
ahlervin 4:767fd282dd9c 170 fahren.ziel();
ahlervin 4:767fd282dd9c 171 fahren.stopp();
ahlervin 4:767fd282dd9c 172 }
Jacqueline 0:6d0671ae4648 173 }
Jacqueline 0:6d0671ae4648 174 }