Roboshark / Mbed 2 deprecated Michu_Proeble_V6

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

Regler.cpp@9:bdbf4447b55e, 2018-05-06 (annotated)

Committer:
ahlervin
Date:
Sun May 06 15:18:14 2018 +0000
Revision:
9:bdbf4447b55e
Parent:
8:a7f1ee7840d0 
IR Front ?berwacht das nicht zu weit gefahren wird

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ahlervin 6:7bbcdd07bc2d 1 /*Roboshark V5
ahlervin 6:7bbcdd07bc2d 2 Regler.cpp
ahlervin 6:7bbcdd07bc2d 3 Erstellt: V.Ahlers
ahlervin 6:7bbcdd07bc2d 4 geändert: V.Ahlers
ahlervin 6:7bbcdd07bc2d 5 V.5.18
ahlervin 6:7bbcdd07bc2d 6 Regler zum geradeaus fahren
ahlervin 6:7bbcdd07bc2d 7 */
ahlervin 6:7bbcdd07bc2d 8
ahlervin 6:7bbcdd07bc2d 9 #include <cmath>
ahlervin 6:7bbcdd07bc2d 10 #include "Regler.h"
ahlervin 7:862d80e0ea2d 11 #include "IRSensor.h"
ahlervin 6:7bbcdd07bc2d 12
ahlervin 6:7bbcdd07bc2d 13
ahlervin 6:7bbcdd07bc2d 14 using namespace std;
ahlervin 6:7bbcdd07bc2d 15
fluckmi1 8:a7f1ee7840d0 16 const float Regler :: PERIOD = 0.5f;
ahlervin 6:7bbcdd07bc2d 17 const int Regler :: FIXSPEED = 50;
fluckmi1 8:a7f1ee7840d0 18 float faktor = 0.1f;
ahlervin 6:7bbcdd07bc2d 19
ahlervin 6:7bbcdd07bc2d 20
ahlervin 6:7bbcdd07bc2d 21
ahlervin 7:862d80e0ea2d 22 Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft, IRSensor& iRSensor):
ahlervin 7:862d80e0ea2d 23 IrRight (IrRight), IrLeft (IrLeft), iRSensor (iRSensor) {
ahlervin 6:7bbcdd07bc2d 24
ahlervin 6:7bbcdd07bc2d 25 SpeedR = 0;
ahlervin 6:7bbcdd07bc2d 26 SpeedL = 0;
ahlervin 6:7bbcdd07bc2d 27 ticker.attach(callback(this, &Regler::setSpeed), PERIOD);
ahlervin 6:7bbcdd07bc2d 28 }
ahlervin 6:7bbcdd07bc2d 29
ahlervin 6:7bbcdd07bc2d 30 Regler::~Regler(){
ahlervin 6:7bbcdd07bc2d 31 ticker.detach();
ahlervin 6:7bbcdd07bc2d 32 }
ahlervin 6:7bbcdd07bc2d 33
ahlervin 6:7bbcdd07bc2d 34 void Regler::setSpeed (){
ahlervin 9:bdbf4447b55e 35 measR2 = iRSensor.readR(); // Converts and read the analog input value
fluckmi1 8:a7f1ee7840d0 36 measL2 = iRSensor.readL();
ahlervin 6:7bbcdd07bc2d 37
fluckmi1 8:a7f1ee7840d0 38 if(measR2 == measL2){ //DAS ISCH MIN GAGAREGLER zum Pröble gsi
fluckmi1 8:a7f1ee7840d0 39 SpeedR = FIXSPEED;
fluckmi1 8:a7f1ee7840d0 40 SpeedL = FIXSPEED;
fluckmi1 8:a7f1ee7840d0 41 }else if(measR2 < measL2 && measR2 < 300 && measL2 < 300){ //float style implementiert
fluckmi1 8:a7f1ee7840d0 42 SpeedR = FIXSPEED;
fluckmi1 8:a7f1ee7840d0 43 SpeedL = FIXSPEED*1.04;
fluckmi1 8:a7f1ee7840d0 44 }else if(measR2 > measL2 && measR2 < 300 && measL2 < 300){
fluckmi1 8:a7f1ee7840d0 45 SpeedR = FIXSPEED*1.06;
fluckmi1 8:a7f1ee7840d0 46 SpeedL = FIXSPEED;
fluckmi1 8:a7f1ee7840d0 47 }
fluckmi1 8:a7f1ee7840d0 48
fluckmi1 8:a7f1ee7840d0 49 /*
ahlervin 7:862d80e0ea2d 50 if((measR2 > 100) && (measL2 < 100)) { //keine Wnad rechts
fluckmi1 8:a7f1ee7840d0 51 if(measL2 > 40){
fluckmi1 8:a7f1ee7840d0 52 div3 = measL2 - 40;
ahlervin 6:7bbcdd07bc2d 53 kor3 = faktor*div3;
ahlervin 6:7bbcdd07bc2d 54 div2 = 0;
ahlervin 6:7bbcdd07bc2d 55 div1 = 0;
ahlervin 6:7bbcdd07bc2d 56 div4 = 0;
ahlervin 7:862d80e0ea2d 57 div5 = 0;
ahlervin 7:862d80e0ea2d 58 div6 = 0;
ahlervin 6:7bbcdd07bc2d 59 SpeedR = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 60 SpeedL = FIXSPEED + kor3;
fluckmi1 8:a7f1ee7840d0 61 } else if (measL2 < 45){
fluckmi1 8:a7f1ee7840d0 62 div4 = 45 - measR2;
ahlervin 6:7bbcdd07bc2d 63 kor4 = faktor*div4;
ahlervin 6:7bbcdd07bc2d 64 div2 = 0;
ahlervin 6:7bbcdd07bc2d 65 div1 = 0;
ahlervin 6:7bbcdd07bc2d 66 div3 = 0;
ahlervin 7:862d80e0ea2d 67 div5 = 0;
ahlervin 7:862d80e0ea2d 68 div6 = 0;
ahlervin 7:862d80e0ea2d 69 SpeedR = FIXSPEED;
ahlervin 7:862d80e0ea2d 70 SpeedL = FIXSPEED + kor4;
ahlervin 7:862d80e0ea2d 71 }else {
ahlervin 7:862d80e0ea2d 72 SpeedR = FIXSPEED;
ahlervin 7:862d80e0ea2d 73 SpeedL = FIXSPEED;
ahlervin 7:862d80e0ea2d 74 }
ahlervin 7:862d80e0ea2d 75 }
ahlervin 7:862d80e0ea2d 76 if((measL2 > 100) &&(measR2 < 100)) { //keine Wnad links
ahlervin 7:862d80e0ea2d 77 if(measR2 > 47){
ahlervin 7:862d80e0ea2d 78 div5 = measR2 - 47;
ahlervin 7:862d80e0ea2d 79 kor5 = faktor*div5;
ahlervin 7:862d80e0ea2d 80 div2 = 0;
ahlervin 7:862d80e0ea2d 81 div1 = 0;
ahlervin 7:862d80e0ea2d 82 div4 = 0;
ahlervin 7:862d80e0ea2d 83 div6 = 0;
ahlervin 7:862d80e0ea2d 84 div3 = 0;
ahlervin 7:862d80e0ea2d 85 SpeedR = FIXSPEED;
ahlervin 7:862d80e0ea2d 86 SpeedL = FIXSPEED + kor5;
ahlervin 7:862d80e0ea2d 87 } else if (measR2 < 52){
ahlervin 7:862d80e0ea2d 88 div6 = 52 - measR2;
ahlervin 7:862d80e0ea2d 89 kor6 = faktor*div4;
ahlervin 7:862d80e0ea2d 90 div2 = 0;
ahlervin 7:862d80e0ea2d 91 div1 = 0;
ahlervin 7:862d80e0ea2d 92 div3 = 0;
ahlervin 7:862d80e0ea2d 93 div4 = 0;
ahlervin 7:862d80e0ea2d 94 div5 = 0;
ahlervin 7:862d80e0ea2d 95 SpeedR = FIXSPEED + kor6;
ahlervin 6:7bbcdd07bc2d 96 SpeedL = FIXSPEED;
ahlervin 7:862d80e0ea2d 97 } else {
ahlervin 7:862d80e0ea2d 98 SpeedR = FIXSPEED;
ahlervin 7:862d80e0ea2d 99 SpeedL = FIXSPEED;
ahlervin 7:862d80e0ea2d 100 }
ahlervin 7:862d80e0ea2d 101 }
ahlervin 7:862d80e0ea2d 102 if ((measR2 < measL2)&& (measL2 - measR2 > 3)) { //IR Sensor werte werden verglichen und die Korrektur wird berechnet
ahlervin 7:862d80e0ea2d 103 div1 = measR2 - measL2; // An beiden seinen Wände
ahlervin 7:862d80e0ea2d 104 kor1 = 0.1f*div1;
ahlervin 7:862d80e0ea2d 105 div2 = 0;
ahlervin 7:862d80e0ea2d 106 div3 = 0;
ahlervin 7:862d80e0ea2d 107 div4 = 0;
ahlervin 7:862d80e0ea2d 108 div5 = 0;
ahlervin 7:862d80e0ea2d 109 div6 = 0;
ahlervin 6:7bbcdd07bc2d 110 SpeedR = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 111 SpeedL = FIXSPEED + kor1;
ahlervin 7:862d80e0ea2d 112 } else if ((measR2 > measL2) && (measR2 - measL2 >3)) {
ahlervin 6:7bbcdd07bc2d 113 div2 = measL2 - measR2;
ahlervin 7:862d80e0ea2d 114 kor2 = 0.1f*div2;
ahlervin 7:862d80e0ea2d 115 div1 = 0;
ahlervin 7:862d80e0ea2d 116 div3 = 0;
ahlervin 7:862d80e0ea2d 117 div4 = 0;
ahlervin 7:862d80e0ea2d 118 div5 = 0;
ahlervin 7:862d80e0ea2d 119 div6 = 0;
ahlervin 6:7bbcdd07bc2d 120 SpeedR = FIXSPEED + kor2;
ahlervin 6:7bbcdd07bc2d 121 SpeedL = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 122 } else {
ahlervin 6:7bbcdd07bc2d 123 SpeedR = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 124 SpeedL = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 125 }
ahlervin 6:7bbcdd07bc2d 126 //printf("Div1 = %f\n",div1);
ahlervin 6:7bbcdd07bc2d 127 //printf("Div2 = %f\n",div2);
ahlervin 6:7bbcdd07bc2d 128 //printf("SpeedR1 = %f\n",SpeedR);
fluckmi1 8:a7f1ee7840d0 129 //printf("SpeedL1 = %f\n",SpeedL);
fluckmi1 8:a7f1ee7840d0 130
fluckmi1 8:a7f1ee7840d0 131 */
fluckmi1 8:a7f1ee7840d0 132
ahlervin 6:7bbcdd07bc2d 133 }
fluckmi1 8:a7f1ee7840d0 134 float Regler :: getSpeedR(){
ahlervin 6:7bbcdd07bc2d 135 SpeedR = SpeedR;
ahlervin 6:7bbcdd07bc2d 136 //printf("SpeedR2 = %f\n",SpeedR);
ahlervin 6:7bbcdd07bc2d 137 return SpeedR;
ahlervin 6:7bbcdd07bc2d 138 }
fluckmi1 8:a7f1ee7840d0 139 float Regler :: getSpeedL(){
ahlervin 6:7bbcdd07bc2d 140 SpeedL = SpeedL;
ahlervin 6:7bbcdd07bc2d 141 //printf("SpeedL2 = %f\n",SpeedL);
ahlervin 6:7bbcdd07bc2d 142 return SpeedL;
ahlervin 7:862d80e0ea2d 143 }
ahlervin 6:7bbcdd07bc2d 144
ahlervin 6:7bbcdd07bc2d 145
ahlervin 6:7bbcdd07bc2d 146
ahlervin 6:7bbcdd07bc2d 147
ahlervin 6:7bbcdd07bc2d 148