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@15:8e8b23d4abb4, 2018-05-15 (annotated)

Committer:
ahlervin
Date:
Tue May 15 21:50:26 2018 +0000
Revision:
15:8e8b23d4abb4
Parent:
14:feafcee53fed 
mit Links Hand Methode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ahlervin 14:feafcee53fed 1 /*Roboshark V10
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 14:feafcee53fed 6 main zu Robishark
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"
Jacqueline 0:6d0671ae4648 16
Jacqueline 0:6d0671ae4648 17 DigitalOut myled(LED1);
Jacqueline 0:6d0671ae4648 18
ahlervin 14:feafcee53fed 19 DigitalOut led0(PC_8); //Ready
ahlervin 14:feafcee53fed 20 DigitalOut ledLinksAktiv(PC_6); // Links Aktiv
ahlervin 14:feafcee53fed 21 DigitalOut ledRechtsAktiv(PB_12); // Rechts Aktiv
Jacqueline 0:6d0671ae4648 22 DigitalOut led3(PA_7);
Jacqueline 0:6d0671ae4648 23 DigitalOut led4(PC_0);
Jacqueline 0:6d0671ae4648 24 DigitalOut led5(PC_9);
Jacqueline 0:6d0671ae4648 25
Jacqueline 0:6d0671ae4648 26 DigitalOut enable(PC_1);
Jacqueline 0:6d0671ae4648 27 DigitalOut bit0(PH_1);
Jacqueline 0:6d0671ae4648 28 DigitalOut bit1(PC_2);
Jacqueline 0:6d0671ae4648 29 DigitalOut bit2(PC_3);
Jacqueline 0:6d0671ae4648 30
ahlervin 14:feafcee53fed 31 AnalogIn IrRight(PC_3); // IR Sensoren
Jacqueline 0:6d0671ae4648 32 AnalogIn IrLeft (PC_5);
Jacqueline 0:6d0671ae4648 33 AnalogIn IrFront(PC_2);
ahlervin 14:feafcee53fed 34 AnalogIn LineSensor(PB_1); // Line Sensor
ahlervin 14:feafcee53fed 35 DigitalIn button (USER_BUTTON); //Auswählen ob Rechts oder Links methode
ahlervin 14:feafcee53fed 36
Jacqueline 0:6d0671ae4648 37 float disR = 0;
Jacqueline 0:6d0671ae4648 38 float disL = 0;
Jacqueline 0:6d0671ae4648 39 float disF = 0;
Jacqueline 0:6d0671ae4648 40
Jacqueline 0:6d0671ae4648 41 float dis2R = 0;
Jacqueline 0:6d0671ae4648 42 float dis2L = 0;
Jacqueline 0:6d0671ae4648 43 float dis2F = 0;
Jacqueline 0:6d0671ae4648 44 int IrR = 0;
Jacqueline 0:6d0671ae4648 45 int IrL = 0;
Jacqueline 0:6d0671ae4648 46 int IrF = 0;
ahlervin 4:767fd282dd9c 47 int caseDrive = 0;
ahlervin 4:767fd282dd9c 48 double line = 0;
ahlervin 4:767fd282dd9c 49 bool finish = 0;
ahlervin 4:767fd282dd9c 50 bool finishLast = 0;
ahlervin 4:767fd282dd9c 51 int ende = 0;
ahlervin 6:7bbcdd07bc2d 52 int temp = 0;
ahlervin 6:7bbcdd07bc2d 53 float SpeedR = 0;
ahlervin 6:7bbcdd07bc2d 54 float SpeedL = 0;
ahlervin 14:feafcee53fed 55 int links = 0;
ahlervin 14:feafcee53fed 56 int linksLast = 0;
ahlervin 14:feafcee53fed 57 int skip = 0;
ahlervin 15:8e8b23d4abb4 58 int BL = 1;
ahlervin 6:7bbcdd07bc2d 59 //von main Vincent kopiert
Jacqueline 0:6d0671ae4648 60
Jacqueline 0:6d0671ae4648 61 DigitalOut enableMotorDriver(PB_2);
Jacqueline 0:6d0671ae4648 62 DigitalIn motorDriverFault(PB_14);
Jacqueline 0:6d0671ae4648 63 DigitalIn motorDriverWarning(PB_15);
Jacqueline 0:6d0671ae4648 64
Jacqueline 0:6d0671ae4648 65 PwmOut pwmLeft(PA_8);
Jacqueline 0:6d0671ae4648 66 PwmOut pwmRight(PA_9);
Jacqueline 0:6d0671ae4648 67
Jacqueline 0:6d0671ae4648 68 EncoderCounter counterLeft(PB_6, PB_7);
Jacqueline 0:6d0671ae4648 69 EncoderCounter counterRight(PA_6, PC_7);
Jacqueline 0:6d0671ae4648 70
ahlervin 15:8e8b23d4abb4 71 IRSensor iRSensor(IrRight, IrLeft, IrFront, dis2R, dis2L, dis2F, LineSensor, button); //von main Vincent kopiert
ahlervin 6:7bbcdd07bc2d 72
Jacqueline 0:6d0671ae4648 73 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Jacqueline 0:6d0671ae4648 74
ahlervin 9:feabe0b7cea4 75 Fahren fahren(controller, counterLeft, counterRight, iRSensor); //Reihenfolge!!!!!!!!!!!!!!!!!!! Ultra wichtig
Jacqueline 0:6d0671ae4648 76
ahlervin 14:feafcee53fed 77 int main(){
Jacqueline 0:6d0671ae4648 78
Jacqueline 0:6d0671ae4648 79 enable = 1;
ahlervin 14:feafcee53fed 80 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
ahlervin 14:feafcee53fed 81
Jacqueline 0:6d0671ae4648 82 while(1) {
ahlervin 15:8e8b23d4abb4 83
ahlervin 14:feafcee53fed 84
ahlervin 15:8e8b23d4abb4 85 if(button == linksLast){
ahlervin 14:feafcee53fed 86 links = 1;
ahlervin 15:8e8b23d4abb4 87 linksLast = links;
ahlervin 14:feafcee53fed 88 } else { links = 0;
ahlervin 14:feafcee53fed 89 }
ahlervin 15:8e8b23d4abb4 90
ahlervin 6:7bbcdd07bc2d 91 // IR Sensoren einlesen
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();
ahlervin 6:7bbcdd07bc2d 101 ende = iRSensor.get_ende(); // Line erkennt = 1
ahlervin 4:767fd282dd9c 102
ahlervin 15:8e8b23d4abb4 103 BL = button;
ahlervin 15:8e8b23d4abb4 104 if (button == 0){
ahlervin 15:8e8b23d4abb4 105 temp = 0;
ahlervin 15:8e8b23d4abb4 106 ende = 0;
ahlervin 15:8e8b23d4abb4 107 skip = 0;
ahlervin 15:8e8b23d4abb4 108 }
ahlervin 15:8e8b23d4abb4 109
ahlervin 15:8e8b23d4abb4 110 if ((links == 0) && (skip == 0)){
ahlervin 15:8e8b23d4abb4 111 wait (1);
ahlervin 15:8e8b23d4abb4 112 skip = 1;
ahlervin 15:8e8b23d4abb4 113 } else { if ((links == 1) && ( skip == 0)){
ahlervin 15:8e8b23d4abb4 114 wait (1);
ahlervin 15:8e8b23d4abb4 115 skip = 1;
ahlervin 15:8e8b23d4abb4 116 }
ahlervin 15:8e8b23d4abb4 117 }
ahlervin 4:767fd282dd9c 118
ahlervin 4:767fd282dd9c 119
ahlervin 7:862d80e0ea2d 120 /* printf (" ende = %d\n",ende);
ahlervin 4:767fd282dd9c 121 printf (" finish = %d\n",finish);
ahlervin 4:767fd282dd9c 122 printf (" finishLast = %d\n",finishLast);
ahlervin 4:767fd282dd9c 123 printf("line = %f\n", line);
Jacqueline 0:6d0671ae4648 124
ahlervin 6:7bbcdd07bc2d 125 printf ("IR Right = %d \n", IrR);
Jacqueline 0:6d0671ae4648 126 printf("IR Left = %d\n",IrL);
ahlervin 4:767fd282dd9c 127 printf("IR Front = %d\n",IrF);*/
Jacqueline 0:6d0671ae4648 128
ahlervin 15:8e8b23d4abb4 129 switch(links) {
ahlervin 15:8e8b23d4abb4 130 case 0: // State machine für methode Rechts
ahlervin 4:767fd282dd9c 131 if((IrR==0) && (IrL==0) && (IrF==1) && (ende == 0)){
ahlervin 14:feafcee53fed 132 caseDrive = 2;
ahlervin 4:767fd282dd9c 133 } else if ((IrR==0) && (IrL==1) && (IrF==0) && (ende == 0)){
ahlervin 14:feafcee53fed 134 caseDrive = 2;
ahlervin 4:767fd282dd9c 135 } else if ((IrR==0) && (IrL==1) && (IrF==1) && (ende == 0)){
ahlervin 14:feafcee53fed 136 caseDrive = 2;
ahlervin 4:767fd282dd9c 137 } else if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
ahlervin 14:feafcee53fed 138 caseDrive = 1;
ahlervin 4:767fd282dd9c 139 } else if ((IrR==1) && (IrL==0) && (IrF==1) && (ende == 0)){
ahlervin 14:feafcee53fed 140 caseDrive = 3;
ahlervin 4:767fd282dd9c 141 } else if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
ahlervin 14:feafcee53fed 142 caseDrive = 1;
ahlervin 4:767fd282dd9c 143 } else if ((IrR==1) && (IrL==1) && (IrF==1) && (ende == 0)){
ahlervin 14:feafcee53fed 144 caseDrive = 4;
ahlervin 4:767fd282dd9c 145 } else if ((ende == 1) && (temp == 0)){
ahlervin 14:feafcee53fed 146 caseDrive = 5;
ahlervin 4:767fd282dd9c 147 temp = 1;
ahlervin 14:feafcee53fed 148 } else {caseDrive = 0;
Jacqueline 0:6d0671ae4648 149 }
ahlervin 15:8e8b23d4abb4 150 break;
ahlervin 15:8e8b23d4abb4 151
ahlervin 15:8e8b23d4abb4 152 case 1: //State Machine für Methode Links
ahlervin 14:feafcee53fed 153 if((IrR==0) && (IrL==0) && (IrF==0) && (ende == 0)){
ahlervin 14:feafcee53fed 154 caseDrive = 3;
ahlervin 14:feafcee53fed 155 } else if ((IrR==1) && (IrL==0) && (IrF==0) && (ende == 0)){
ahlervin 14:feafcee53fed 156 caseDrive = 3;
ahlervin 14:feafcee53fed 157 } else if ((IrR==0) && (IrL==1) && (IrF==0) && (ende == 0)){
ahlervin 14:feafcee53fed 158 caseDrive = 1;
ahlervin 14:feafcee53fed 159 } else if ((IrR==1) && (IrL==1) && (IrF==0) && (ende == 0)){
ahlervin 14:feafcee53fed 160 caseDrive = 1;
ahlervin 14:feafcee53fed 161 } else if ((IrR==0) && (IrL==0) && (IrF==1) && (ende == 0)){
ahlervin 14:feafcee53fed 162 caseDrive = 3;
ahlervin 15:8e8b23d4abb4 163 } else if ((IrR==1) && (IrL==0)&& (IrF==1) && (ende == 0)){
ahlervin 14:feafcee53fed 164 caseDrive = 3;
ahlervin 14:feafcee53fed 165 } else if ((IrR==0)&& (IrL==1) && (IrF==1) && (ende == 0)){
ahlervin 14:feafcee53fed 166 caseDrive = 2;
ahlervin 14:feafcee53fed 167 } else if ((IrR==1)&& (IrL==1) && (IrF==1) && (ende == 0)){
ahlervin 14:feafcee53fed 168 caseDrive = 4;
ahlervin 14:feafcee53fed 169 } else if ((ende == 1) && (temp == 0)){
ahlervin 14:feafcee53fed 170 caseDrive = 5;
ahlervin 14:feafcee53fed 171 temp = 1;
ahlervin 14:feafcee53fed 172 } else {caseDrive = 0;
ahlervin 14:feafcee53fed 173 }
ahlervin 14:feafcee53fed 174 }
ahlervin 14:feafcee53fed 175
ahlervin 10:fb2195d0de0f 176
ahlervin 15:8e8b23d4abb4 177 printf("ende = %d\n",ende);
Jacqueline 0:6d0671ae4648 178
ahlervin 10:fb2195d0de0f 179 wait (0.01);
Jacqueline 0:6d0671ae4648 180
Jacqueline 1:a95ba1510438 181
Jacqueline 0:6d0671ae4648 182
Jacqueline 0:6d0671ae4648 183
ahlervin 9:feabe0b7cea4 184 if(caseDrive == 1){ // Aufrufen der verschiedenen fahr funktionen
ahlervin 6:7bbcdd07bc2d 185 fahren.geradeausG();
Jacqueline 0:6d0671ae4648 186 } else if (caseDrive == 2){
Jacqueline 0:6d0671ae4648 187 fahren.rechts90();
ahlervin 6:7bbcdd07bc2d 188 fahren.geradeausG();
Jacqueline 0:6d0671ae4648 189 } else if (caseDrive == 3){
Jacqueline 2:402b1a74fff6 190 fahren.links90();
ahlervin 6:7bbcdd07bc2d 191 fahren.geradeausG();
Jacqueline 0:6d0671ae4648 192 } else if (caseDrive == 4){
Jacqueline 0:6d0671ae4648 193 fahren.rechts180();
ahlervin 6:7bbcdd07bc2d 194 fahren.geradeausG();
ahlervin 4:767fd282dd9c 195 } else if (caseDrive == 0){
ahlervin 4:767fd282dd9c 196 fahren.stopp();
ahlervin 4:767fd282dd9c 197 } else if(caseDrive == 5){
ahlervin 4:767fd282dd9c 198 fahren.ziel();
ahlervin 4:767fd282dd9c 199 fahren.stopp();
ahlervin 4:767fd282dd9c 200 }
Jacqueline 0:6d0671ae4648 201 }
Jacqueline 0:6d0671ae4648 202 }