Antonia Baumgartner / Mbed 2 deprecated Versuch21

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 Versuch20 by Alexander Wyss

Classes/Motion.cpp@5:64d7b4b69fdf, 2018-05-12 (annotated)

Committer:
baumgant
Date:
Sat May 12 12:49:18 2018 +0000
Revision:
5:64d7b4b69fdf
Parent:
4:3c6d2c035243 
PES2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
baumgant 0:b886f13e4ac6 1 #include "Motion.h"
baumgant 0:b886f13e4ac6 2 #include "Controller.h"
baumgant 0:b886f13e4ac6 3 #include "EncoderCounter.h"
baumgant 0:b886f13e4ac6 4 #include "Spurhaltung.h"
mrbb2 1:6ef5bc60e69c 5 #include "IRSensorGF.h"
mrbb2 1:6ef5bc60e69c 6 #include "IRSensorG.h"
mrbb2 1:6ef5bc60e69c 7 #include "IRSensorK.h"
mrbb2 1:6ef5bc60e69c 8 #include "IRSensorZ.h"
baumgant 0:b886f13e4ac6 9
baumgant 5:64d7b4b69fdf 10 // Left + = Vorwärts Left - = Vorwärts
baumgant 5:64d7b4b69fdf 11 // Right + = Rückwärts Right - = Rückwärts
baumgant 5:64d7b4b69fdf 12
baumgant 5:64d7b4b69fdf 13
baumgant 0:b886f13e4ac6 14
baumgant 0:b886f13e4ac6 15 using namespace std;
baumgant 0:b886f13e4ac6 16
baumgant 5:64d7b4b69fdf 17 const float Motion::SPEED = 150;
baumgant 5:64d7b4b69fdf 18 const float Motion::DREHEN90 = 1607;
baumgant 5:64d7b4b69fdf 19 const float Motion::GERADE = 3280;
baumgant 5:64d7b4b69fdf 20
mrbb2 1:6ef5bc60e69c 21 Motion::Motion(EncoderCounter& counterLeft, EncoderCounter& counterRight, Controller& controller, Spurhaltung& spurhaltung, IRSensorGF& Sensor1, IRSensorG& Sensor2, IRSensorG& Sensor3, IRSensorK& Sensor4, IRSensorK& Sensor5, IRSensorZ& Sensor6):
mrbb2 1:6ef5bc60e69c 22 counterLeft(counterLeft), counterRight(counterRight), controller(controller), spurhaltung(spurhaltung), Sensor1(Sensor1), Sensor2(Sensor2), Sensor3(Sensor3), Sensor4(Sensor4), Sensor5(Sensor5), Sensor6(Sensor6)
baumgant 0:b886f13e4ac6 23 {
mrbb2 3:f44ef28cfb2d 24
baumgant 0:b886f13e4ac6 25 }
baumgant 0:b886f13e4ac6 26
baumgant 0:b886f13e4ac6 27 Motion::~Motion() {}
baumgant 0:b886f13e4ac6 28
baumgant 2:efa9a78591da 29 //------------------------------------------------------------------------------
mrbb2 1:6ef5bc60e69c 30
mrbb2 1:6ef5bc60e69c 31 void Motion::drehenl90()
mrbb2 1:6ef5bc60e69c 32 {
mrbb2 1:6ef5bc60e69c 33 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 34 int lastCountRight = counterRight.read();
mrbb2 1:6ef5bc60e69c 35
baumgant 5:64d7b4b69fdf 36 while(counterRight.read()-lastCountRight > -DREHEN90) {
baumgant 5:64d7b4b69fdf 37 controller.setDesiredSpeedLeft(-SPEED); //Drehzahl in [rpm]
baumgant 5:64d7b4b69fdf 38 controller.setDesiredSpeedRight(-SPEED);
mrbb2 1:6ef5bc60e69c 39 }
mrbb2 1:6ef5bc60e69c 40 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 41 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 42
mrbb2 1:6ef5bc60e69c 43 }
mrbb2 1:6ef5bc60e69c 44
baumgant 2:efa9a78591da 45 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 46
baumgant 2:efa9a78591da 47 void Motion::gerade()
baumgant 2:efa9a78591da 48 {
baumgant 2:efa9a78591da 49
baumgant 2:efa9a78591da 50 controller.resetCounter();
baumgant 2:efa9a78591da 51 int lastCountLeft = counterLeft.read();
baumgant 5:64d7b4b69fdf 52
baumgant 5:64d7b4b69fdf 53 while((counterLeft.read()-lastCountLeft < GERADE) && (Sensor1.read() > 120 )) { // 1.806
baumgant 5:64d7b4b69fdf 54 controller.setDesiredSpeedLeft(spurhaltung.speedl()); //Drehzahl in [rpm]
baumgant 5:64d7b4b69fdf 55 controller.setDesiredSpeedRight(-spurhaltung.speedr());
baumgant 5:64d7b4b69fdf 56 printf("");
baumgant 5:64d7b4b69fdf 57 //printf("SpurhaltungLinks: %d\r\n", spurhaltung.speedl());
baumgant 5:64d7b4b69fdf 58 //printf("SpurhaltungRechts: %d\r\n", spurhaltung.speedr());
baumgant 5:64d7b4b69fdf 59 //printf("Sensor2:%d\r\n", Sensor2.read());
baumgant 5:64d7b4b69fdf 60 //printf("Sensor1:%d\r\n\r\n", Sensor1.read());
baumgant 5:64d7b4b69fdf 61 }
baumgant 5:64d7b4b69fdf 62 if((Sensor1.read() > 120) && (Sensor1.read() < 250)) {
baumgant 5:64d7b4b69fdf 63 while(Sensor1.read() > 120) {
baumgant 5:64d7b4b69fdf 64 controller.setDesiredSpeedLeft(spurhaltung.speedl()); //Drehzahl in [rpm]
baumgant 5:64d7b4b69fdf 65 controller.setDesiredSpeedRight(-spurhaltung.speedr());
baumgant 5:64d7b4b69fdf 66 printf("");
mrbb2 3:f44ef28cfb2d 67 }
baumgant 5:64d7b4b69fdf 68
baumgant 2:efa9a78591da 69 }
baumgant 2:efa9a78591da 70 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
baumgant 2:efa9a78591da 71 controller.setDesiredSpeedRight(0.0f);
baumgant 2:efa9a78591da 72 }
baumgant 2:efa9a78591da 73
baumgant 2:efa9a78591da 74 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 75
mrbb2 1:6ef5bc60e69c 76 void Motion::drehenr90()
mrbb2 1:6ef5bc60e69c 77 {
mrbb2 1:6ef5bc60e69c 78 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 79 int lastCountLeft = counterLeft.read();
mrbb2 1:6ef5bc60e69c 80
baumgant 5:64d7b4b69fdf 81 while(counterLeft.read()-lastCountLeft < DREHEN90) {
baumgant 5:64d7b4b69fdf 82 controller.setDesiredSpeedLeft(SPEED); //Drehzahl in [rpm]
baumgant 5:64d7b4b69fdf 83 controller.setDesiredSpeedRight(SPEED);
mrbb2 1:6ef5bc60e69c 84 }
mrbb2 1:6ef5bc60e69c 85 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 86 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 87
mrbb2 1:6ef5bc60e69c 88 }
mrbb2 1:6ef5bc60e69c 89
baumgant 2:efa9a78591da 90 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 91
mrbb2 1:6ef5bc60e69c 92 void Motion::drehen180()
mrbb2 1:6ef5bc60e69c 93 {
mrbb2 1:6ef5bc60e69c 94 controller.resetCounter();
mrbb2 1:6ef5bc60e69c 95 int lastCountRight = counterRight.read();
mrbb2 1:6ef5bc60e69c 96
baumgant 5:64d7b4b69fdf 97 while(counterRight.read()-lastCountRight < 2*1607 + 150) {
mrbb2 1:6ef5bc60e69c 98 controller.setDesiredSpeedLeft(150.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 99 controller.setDesiredSpeedRight(150.0f);
mrbb2 1:6ef5bc60e69c 100 }
mrbb2 1:6ef5bc60e69c 101 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
mrbb2 1:6ef5bc60e69c 102 controller.setDesiredSpeedRight(0.0f);
mrbb2 1:6ef5bc60e69c 103 }
baumgant 0:b886f13e4ac6 104
baumgant 2:efa9a78591da 105 //------------------------------------------------------------------------------
baumgant 2:efa9a78591da 106
baumgant 0:b886f13e4ac6 107 void Motion::switching(int D)
baumgant 0:b886f13e4ac6 108 {
baumgant 0:b886f13e4ac6 109 switch(D) {
baumgant 0:b886f13e4ac6 110 case 1:
mrbb2 1:6ef5bc60e69c 111 // Left
mrbb2 1:6ef5bc60e69c 112 drehenl90();
baumgant 5:64d7b4b69fdf 113 wait(0.05f);
baumgant 0:b886f13e4ac6 114 // Forward
mrbb2 3:f44ef28cfb2d 115 gerade();
baumgant 5:64d7b4b69fdf 116 wait(0.05f);
baumgant 0:b886f13e4ac6 117 break;
baumgant 0:b886f13e4ac6 118
baumgant 0:b886f13e4ac6 119 case 2:
mrbb2 1:6ef5bc60e69c 120 // Forward
mrbb2 1:6ef5bc60e69c 121 gerade();
baumgant 5:64d7b4b69fdf 122 wait(0.05f);
baumgant 0:b886f13e4ac6 123 break;
baumgant 0:b886f13e4ac6 124
baumgant 0:b886f13e4ac6 125 case 3:
mrbb2 1:6ef5bc60e69c 126 // Right
mrbb2 1:6ef5bc60e69c 127 drehenr90();
baumgant 5:64d7b4b69fdf 128 wait(0.05f);
baumgant 0:b886f13e4ac6 129 // Forward
mrbb2 1:6ef5bc60e69c 130 gerade();
baumgant 5:64d7b4b69fdf 131 wait(0.05f);
baumgant 0:b886f13e4ac6 132 break;
baumgant 0:b886f13e4ac6 133
mrbb2 1:6ef5bc60e69c 134 case 4:
mrbb2 1:6ef5bc60e69c 135 // 180 Drehen
mrbb2 1:6ef5bc60e69c 136 drehen180();
baumgant 5:64d7b4b69fdf 137 wait(0.05f);
mrbb2 1:6ef5bc60e69c 138 break;
baumgant 0:b886f13e4ac6 139
baumgant 0:b886f13e4ac6 140 default:
baumgant 0:b886f13e4ac6 141 controller.setDesiredSpeedLeft(0.0f); //Drehzahl in [rpm]
baumgant 0:b886f13e4ac6 142 controller.setDesiredSpeedRight(0.0f);
baumgant 0:b886f13e4ac6 143 }
mrbb2 3:f44ef28cfb2d 144
baumgant 2:efa9a78591da 145 //------------------------------------------------------------------------------
mrbb2 1:6ef5bc60e69c 146
mrbb2 3:f44ef28cfb2d 147 /*void Motion::switching()
mrbb2 3:f44ef28cfb2d 148 {
mrbb2 3:f44ef28cfb2d 149 if(Sensor4.read() == 0) {
mrbb2 3:f44ef28cfb2d 150 // Left
mrbb2 3:f44ef28cfb2d 151 drehenl90();
mrbb2 3:f44ef28cfb2d 152 // Forward
mrbb2 3:f44ef28cfb2d 153 gerade();
mrbb2 3:f44ef28cfb2d 154 } else if (Sensor1.read() == 0) {
mrbb2 3:f44ef28cfb2d 155 // Forward
mrbb2 3:f44ef28cfb2d 156 gerade();
mrbb2 3:f44ef28cfb2d 157 } else if (Sensor5.read() == 0) {
mrbb2 3:f44ef28cfb2d 158 // Right
mrbb2 3:f44ef28cfb2d 159 drehenr90();
mrbb2 3:f44ef28cfb2d 160 // Forward
mrbb2 3:f44ef28cfb2d 161 gerade();
mrbb2 3:f44ef28cfb2d 162 } else {
mrbb2 3:f44ef28cfb2d 163 // 180 Drehen
mrbb2 3:f44ef28cfb2d 164 drehen180();
mrbb2 3:f44ef28cfb2d 165 // Forward
mrbb2 3:f44ef28cfb2d 166 gerade();
mrbb2 3:f44ef28cfb2d 167
mrbb2 3:f44ef28cfb2d 168 }*/
baumgant 0:b886f13e4ac6 169
baumgant 0:b886f13e4ac6 170 }