Roboshark / Mbed 2 deprecated Roboshark_V62

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

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

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
ahlervin 8:d0a27278c108 16 const float Regler :: PERIOD = 0.001f;
ahlervin 6:7bbcdd07bc2d 17 const int Regler :: FIXSPEED = 50;
ahlervin 8:d0a27278c108 18 float faktor = 0.12;
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 8:d0a27278c108 35 measR2 = iRSensor.readR(); // Converts and read the analog input value
ahlervin 7:862d80e0ea2d 36 measL2 = iRSensor.readL();
ahlervin 6:7bbcdd07bc2d 37
ahlervin 7:862d80e0ea2d 38 if((measR2 > 100) && (measL2 < 100)) { //keine Wnad rechts
ahlervin 7:862d80e0ea2d 39 if(measL2 > 47){
ahlervin 7:862d80e0ea2d 40 div3 = measL2 - 47;
ahlervin 6:7bbcdd07bc2d 41 kor3 = faktor*div3;
ahlervin 6:7bbcdd07bc2d 42 div2 = 0;
ahlervin 6:7bbcdd07bc2d 43 div1 = 0;
ahlervin 6:7bbcdd07bc2d 44 div4 = 0;
ahlervin 7:862d80e0ea2d 45 div5 = 0;
ahlervin 7:862d80e0ea2d 46 div6 = 0;
ahlervin 6:7bbcdd07bc2d 47 SpeedR = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 48 SpeedL = FIXSPEED + kor3;
ahlervin 7:862d80e0ea2d 49 } else if (measL2 < 52){
ahlervin 7:862d80e0ea2d 50 div4 = 52 - measR2;
ahlervin 6:7bbcdd07bc2d 51 kor4 = faktor*div4;
ahlervin 6:7bbcdd07bc2d 52 div2 = 0;
ahlervin 6:7bbcdd07bc2d 53 div1 = 0;
ahlervin 6:7bbcdd07bc2d 54 div3 = 0;
ahlervin 7:862d80e0ea2d 55 div5 = 0;
ahlervin 7:862d80e0ea2d 56 div6 = 0;
ahlervin 7:862d80e0ea2d 57 SpeedR = FIXSPEED;
ahlervin 7:862d80e0ea2d 58 SpeedL = FIXSPEED + kor4;
ahlervin 7:862d80e0ea2d 59 }else {
ahlervin 7:862d80e0ea2d 60 SpeedR = FIXSPEED;
ahlervin 7:862d80e0ea2d 61 SpeedL = FIXSPEED;
ahlervin 7:862d80e0ea2d 62 }
ahlervin 7:862d80e0ea2d 63 }
ahlervin 7:862d80e0ea2d 64 if((measL2 > 100) &&(measR2 < 100)) { //keine Wnad links
ahlervin 7:862d80e0ea2d 65 if(measR2 > 47){
ahlervin 7:862d80e0ea2d 66 div5 = measR2 - 47;
ahlervin 7:862d80e0ea2d 67 kor5 = faktor*div5;
ahlervin 7:862d80e0ea2d 68 div2 = 0;
ahlervin 7:862d80e0ea2d 69 div1 = 0;
ahlervin 7:862d80e0ea2d 70 div4 = 0;
ahlervin 7:862d80e0ea2d 71 div6 = 0;
ahlervin 7:862d80e0ea2d 72 div3 = 0;
ahlervin 7:862d80e0ea2d 73 SpeedR = FIXSPEED;
ahlervin 7:862d80e0ea2d 74 SpeedL = FIXSPEED + kor5;
ahlervin 7:862d80e0ea2d 75 } else if (measR2 < 52){
ahlervin 7:862d80e0ea2d 76 div6 = 52 - measR2;
ahlervin 7:862d80e0ea2d 77 kor6 = faktor*div4;
ahlervin 7:862d80e0ea2d 78 div2 = 0;
ahlervin 7:862d80e0ea2d 79 div1 = 0;
ahlervin 7:862d80e0ea2d 80 div3 = 0;
ahlervin 7:862d80e0ea2d 81 div4 = 0;
ahlervin 7:862d80e0ea2d 82 div5 = 0;
ahlervin 7:862d80e0ea2d 83 SpeedR = FIXSPEED + kor6;
ahlervin 6:7bbcdd07bc2d 84 SpeedL = FIXSPEED;
ahlervin 7:862d80e0ea2d 85 } else {
ahlervin 7:862d80e0ea2d 86 SpeedR = FIXSPEED;
ahlervin 7:862d80e0ea2d 87 SpeedL = FIXSPEED;
ahlervin 7:862d80e0ea2d 88 }
ahlervin 7:862d80e0ea2d 89 }
ahlervin 7:862d80e0ea2d 90 if ((measR2 < measL2)&& (measL2 - measR2 > 3)) { //IR Sensor werte werden verglichen und die Korrektur wird berechnet
ahlervin 7:862d80e0ea2d 91 div1 = measR2 - measL2; // An beiden seinen Wände
ahlervin 8:d0a27278c108 92 kor1 = 0.12f*div1;
ahlervin 7:862d80e0ea2d 93 div2 = 0;
ahlervin 7:862d80e0ea2d 94 div3 = 0;
ahlervin 7:862d80e0ea2d 95 div4 = 0;
ahlervin 7:862d80e0ea2d 96 div5 = 0;
ahlervin 7:862d80e0ea2d 97 div6 = 0;
ahlervin 6:7bbcdd07bc2d 98 SpeedR = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 99 SpeedL = FIXSPEED + kor1;
ahlervin 7:862d80e0ea2d 100 } else if ((measR2 > measL2) && (measR2 - measL2 >3)) {
ahlervin 6:7bbcdd07bc2d 101 div2 = measL2 - measR2;
ahlervin 8:d0a27278c108 102 kor2 = 0.12f*div2;
ahlervin 7:862d80e0ea2d 103 div1 = 0;
ahlervin 7:862d80e0ea2d 104 div3 = 0;
ahlervin 7:862d80e0ea2d 105 div4 = 0;
ahlervin 7:862d80e0ea2d 106 div5 = 0;
ahlervin 7:862d80e0ea2d 107 div6 = 0;
ahlervin 6:7bbcdd07bc2d 108 SpeedR = FIXSPEED + kor2;
ahlervin 6:7bbcdd07bc2d 109 SpeedL = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 110 } else {
ahlervin 6:7bbcdd07bc2d 111 SpeedR = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 112 SpeedL = FIXSPEED;
ahlervin 6:7bbcdd07bc2d 113 }
ahlervin 6:7bbcdd07bc2d 114 //printf("Div1 = %f\n",div1);
ahlervin 6:7bbcdd07bc2d 115 //printf("Div2 = %f\n",div2);
ahlervin 6:7bbcdd07bc2d 116 //printf("SpeedR1 = %f\n",SpeedR);
ahlervin 6:7bbcdd07bc2d 117 //printf("SpeedL1 = %f\n",SpeedL);
ahlervin 6:7bbcdd07bc2d 118 }
ahlervin 7:862d80e0ea2d 119 float Regler :: getSpeedR (){
ahlervin 6:7bbcdd07bc2d 120 SpeedR = SpeedR;
ahlervin 6:7bbcdd07bc2d 121 //printf("SpeedR2 = %f\n",SpeedR);
ahlervin 6:7bbcdd07bc2d 122 return SpeedR;
ahlervin 6:7bbcdd07bc2d 123 }
ahlervin 7:862d80e0ea2d 124 float Regler :: getSpeedL (){
ahlervin 6:7bbcdd07bc2d 125 SpeedL = SpeedL;
ahlervin 6:7bbcdd07bc2d 126 //printf("SpeedL2 = %f\n",SpeedL);
ahlervin 6:7bbcdd07bc2d 127 return SpeedL;
ahlervin 7:862d80e0ea2d 128 }
ahlervin 6:7bbcdd07bc2d 129
ahlervin 6:7bbcdd07bc2d 130
ahlervin 6:7bbcdd07bc2d 131
ahlervin 6:7bbcdd07bc2d 132
ahlervin 6:7bbcdd07bc2d 133